Řízení stejnosměrného motoru pomocí Pico a H-můstku L298

/*  motor_hm_l298.c
    Řízení stejnosměrného motoru pomocí H můstku s obvodem L298N
    (c) Jirka Chráska 2025, <jirka@lixis.cz>
*/

#define PARAM_ASSERTIONS_ENABLED_PWM 1

#include "pico/stdlib.h"
#include "hardware/pwm.h"

// datová struktura pro řízení motoru
typedef struct
{
    uint gpioForward;
    uint gpioBackward;
    uint slice;
    uint Fchan;
    uint Bchan;
    bool forward;
    uint speed;
    uint freq;
    uint resolution;
    bool on;
} BiMotor;

uint32_t pwm_get_wrap( uint slice_num )
{
    valid_params_if( PWM, slice_num >= 0 && slice_num < NUM_PWM_SLICES );
    return pwm_hw->slice[slice_num].top;
}

void pwm_set_duty( uint slice_num, uint chan, int d )
{
    pwm_set_chan_level( slice_num, chan, pwm_get_wrap( slice_num ) * d / 100 );
}

// nastavení kmitočtu a střídy
uint32_t pwm_set_freq_duty( uint slice_num, uint chan, uint32_t f, int d)
{
uint32_t clock      = 125000000;
uint32_t divider16  = clock / f / 4096 + ( clock % ( f * 4096 ) != 0 );

    if( divider16 / 16 == 0 )    { divider16 = 16; }

uint32_t wrap = clock * 16 / divider16 / f - 1;

    pwm_set_clkdiv_int_frac( slice_num, divider16 / 16, divider16 & 0xf);
    pwm_set_wrap( slice_num, wrap );
    pwm_set_chan_level( slice_num, chan, wrap * d / 100);
    return wrap;
}

// inicializace struktury BiMotor
// @param m:            motor
// @param gpioForward:  pin pro řízení dopředu
// @param gpioBackward: pin pro řízení dozadu
// @param freq:         řídící kmitočet
void BiMotorInit( BiMotor *m, uint gpioForward, uint gpioBackward, uint freq )
{
    // nastaveni pinu pro chod dopředu
    gpio_set_function( gpioForward, GPIO_FUNC_PWM );
    m->gpioForward  = gpioForward;
    m->slice        = pwm_gpio_to_slice_num( gpioForward );
    m->Fchan        = pwm_gpio_to_channel( gpioForward );
    // nastavení pinu pro chod dozadu
    gpio_set_function( gpioBackward, GPIO_FUNC_PWM );
    m->gpioBackward = gpioBackward;
    m->Bchan        = pwm_gpio_to_channel( gpioBackward );

    m->freq         = freq;
    m->speed        = 0;
    m->forward      = true;
    m->resolution   = pwm_set_freq_duty( m->slice, m->Fchan, m->freq, 0 );
    pwm_set_duty( m->slice, m->Bchan, 0);
    m->on           = false;
}

// nastavení rychlosti s motoru a směru otáčení forward
// @param m:        motor
// @param s:        rychlost
// @param forward:  true - dopředu, false dozadu
void BiMotorSpeed( BiMotor *m, int s, bool forward )
{
    if( forward ) {
        pwm_set_duty( m->slice, m->Bchan, 0 );
        pwm_set_duty( m->slice, m->Fchan, s );
    } else {
        pwm_set_duty( m->slice, m->Fchan, 0 );
        pwm_set_duty( m->slice, m->Bchan, s );
    }
    m->speed = s;
}


// zapnutí motoru
void BiMotorOn( BiMotor *m )
{
    pwm_set_enabled(m->slice, true );
    m->on = true;
}

// vypnutí motoru
void BiMotorOff( BiMotor *m )
{
    pwm_set_enabled(m->slice, false );
    m->on = false;
}

int main()
{
BiMotor mot1;

    // GPIO20 slouží k řízení PWM dopředu
    // GPIO21 slouží k řízení PWM dozadu
    // ENA na H-můstku je spojeno jumperem s 5V
    // jumper 5V je zapojen
    // kmitočet PWM 3kHz
    BiMotorInit( &mot1, 20, 21, 3000);

    BiMotorOn( &mot1 );
    while (true) {
        BiMotorSpeed( &mot1, 30, true );    // jedeme dopředu pomalu
        sleep_ms( 2000 );
        BiMotorSpeed( &mot1, 60, true );    // jedeme dopředu rychleji
        sleep_ms( 2000 );
        BiMotorSpeed( &mot1, 20, true );    // jedeme dopředu hodně pomalu
        sleep_ms( 2000 );
        BiMotorSpeed( &mot1, 30, false );   // jedeme dozadu pomalu (tady motor cukne)
        sleep_ms( 2000 );
        BiMotorSpeed( &mot1, 20, false );   // jedeme dozadu hodně pomalu
        sleep_ms( 2000 );
        BiMotorSpeed( &mot1, 0, true );     // stojíme
        sleep_ms( 2000 );
    }
return 0;
}

cmake_minimum_required(VERSION 3.12)

include($ENV{PICO_SDK_PATH}/external/pico_sdk_import.cmake)

project(motor1)

pico_sdk_init()

add_executable(motor1
    motor_hm_l298.c
)

target_link_libraries(motor1
    pico_stdlib
    hardware_pwm
)

pico_enable_stdio_usb(motor1 1)
pico_enable_stdio_uart(motor1 0)

pico_add_extra_outputs(motor1)

$ mkdir build
$ cd build
$ cmake ..
$ make -j8

# řízení stejnosměrného motoru  H-můstkem L298N

from machine import Pin, PWM
from time import sleep

# nastavení pinů
in1 = Pin(16, Pin.OUT) # pin GP16 pro řízení dopředu - spojen s IN1 na H můstku
in2 = Pin(17, Pin.OUT) # pin GP17 pro řízení dozadu  - spojen s IN2 na H můstku
# dopředu se motor točí pokud je in1 HIGH a in2 LOW
# dozadu se motor točí pokud je in1 LOW a in2 HIGH
ena = PWM(Pin(20))     # pin 20 pro nastavení rychlosti otáčení - spojen s ENA na H můstku
ena.freq(300)          # nastavení kmitočtu řízení 300 Hz

# nastavení rychlosti dopředu
def motor_dopredu( rychlost ):
    in2.low()
    in1.high()
    ena.duty_u16( rychlost )     # rychost musí být v rozsahu 0 - 65535

# nastavení rychosti dozadu
def motor_dozadu( rychlost ):
    in1.low()
    in2.high()
    ena.duty_u16( rychlost )     # rychlost musí být v rozsahu 0 - 65535

# zastavení motoru
def motor_stop():
    in1.low()
    in2.low()
    ena.duty_u16(0)

# testování
try:
    while True:
        print("Jedeme dopředu 25")
        motor_dopredu(16384)
        sleep(2)
        print("Jedeme dopředu 50")
        motor_dopredu(32762)
        sleep(2)
        print("Jedeme dopředu 91")
        motor_dopredu(60000)
        sleep(2)
        print("Zastavíme")
        motor_stop()
        sleep(2)
        print("Couváme 25")
        motor_dozadu(16384)
        sleep(4)
        motor_stop()
        sleep(2)
except KeyboardInterrupt:
    motor_stop()
    print("Zastavili jsme, konec programu.")