59 lines
1.3 KiB
C
59 lines
1.3 KiB
C
#include "headers/motors.h"
|
|
#include "i2c/headers/i2c_slave.h"
|
|
|
|
#include <pico/stdlib.h>
|
|
#include <hardware/pwm.h>
|
|
#include <headers/robot.h>
|
|
|
|
|
|
#include <stdio.h>
|
|
|
|
static inline uint get_motor_pwm_pin(motors_enum_t motor)
|
|
{
|
|
switch(motor)
|
|
{
|
|
case MOTOR1:
|
|
return MOTOR1_PWM_PIN;
|
|
|
|
case MOTOR2:
|
|
return MOTOR2_PWM_PIN;
|
|
|
|
default:
|
|
return -1;
|
|
}
|
|
}
|
|
|
|
// Init all motors defined in the MOTORS_DEF array
|
|
void motors_init(void)
|
|
{
|
|
for(motors_enum_t motor = MOTOR1; motor < NB_MOTORS; motor++)
|
|
{
|
|
// Init PWM
|
|
const uint MOTOR_PWM_PIN = get_motor_pwm_pin(motor);
|
|
|
|
const uint SLICE_NUM = pwm_gpio_to_slice_num(MOTOR_PWM_PIN);
|
|
|
|
gpio_set_function(MOTOR_PWM_PIN, GPIO_FUNC_PWM);
|
|
|
|
pwm_set_clkdiv(SLICE_NUM, 255);
|
|
pwm_set_wrap(SLICE_NUM, 256);
|
|
|
|
pwm_set_enabled(SLICE_NUM, true);
|
|
|
|
motor_set_speed(motor, 0);
|
|
}
|
|
}
|
|
|
|
// Set [motor] in motor_enum_t at [value] between 0 and 255
|
|
void motor_set_speed(motors_enum_t motor, uint8_t value)
|
|
{
|
|
uint MOTOR_PWM_PIN = get_motor_pwm_pin(motor);
|
|
pwm_set_gpio_level(MOTOR_PWM_PIN, (uint16_t)value);
|
|
}
|
|
|
|
void motors_update(void)
|
|
{
|
|
motor_set_speed(MOTOR1, get_vitesse_moteur_1());
|
|
motor_set_speed(MOTOR2, get_vitesse_moteur_2());
|
|
}
|