120 lines
3.2 KiB
C
120 lines
3.2 KiB
C
#include <pico/stdlib.h>
|
|
#include <hardware/pwm.h>
|
|
|
|
#include "include/motors.h"
|
|
|
|
const motor_def_t MOTORS_DEFS[] = {
|
|
{0, 4, 5, 0x00},
|
|
{1, 6, 7, 0x01},
|
|
{2, 8, 9, 0x02},
|
|
{3, 10, 11, 0x03},
|
|
};
|
|
|
|
const servo_motor_def_t SERVO_MOTORS_DEFS[] = {
|
|
{12, 0, 25000, 0x04},
|
|
{13, 0, 25000, 0x05},
|
|
{14, 0, 25000, 0x06},
|
|
{15, 0, 25000, 0x07},
|
|
};
|
|
|
|
// Init all motors defined in the MOTORS_DEF array
|
|
void init_motors(void)
|
|
{
|
|
for(motors_enum_t actual_motor = MOTOR1; actual_motor < NB_MOTORS; actual_motor++)
|
|
{
|
|
const motor_def_t *motor_def = &MOTORS_DEFS[actual_motor];
|
|
|
|
// Init PWM
|
|
uint slice_num = pwm_gpio_to_slice_num(motor_def->pwm_pin);
|
|
|
|
gpio_set_function(motor_def->pwm_pin, GPIO_FUNC_PWM);
|
|
pwm_set_wrap(slice_num, 128);
|
|
pwm_set_enabled(slice_num, true);
|
|
|
|
// Init dir pins
|
|
gpio_init(motor_def->dir1_pin);
|
|
gpio_set_dir(motor_def->dir1_pin, GPIO_OUT);
|
|
|
|
gpio_init(motor_def->dir2_pin);
|
|
gpio_set_dir(motor_def->dir2_pin, GPIO_OUT);
|
|
|
|
motor_zero(actual_motor);
|
|
}
|
|
}
|
|
|
|
// Init all servo motors defined in the SERVO_MOTORS_DEF array
|
|
void init_servo_motors(void)
|
|
{
|
|
for(servo_motors_enum_t actual_servo_motor = SERVO_MOTOR1; actual_servo_motor < NB_SERVO_MOTORS; actual_servo_motor++)
|
|
{
|
|
const servo_motor_def_t *servo_motor_def = &SERVO_MOTORS_DEFS[actual_servo_motor];
|
|
|
|
// Init PWM //
|
|
uint slice_num = pwm_gpio_to_slice_num(servo_motor_def->pwm_pin);
|
|
|
|
gpio_set_function(servo_motor_def->pwm_pin, GPIO_FUNC_PWM);
|
|
pwm_set_wrap(slice_num, 25000);
|
|
pwm_set_clkdiv(slice_num, 100);
|
|
pwm_set_enabled(slice_num, true);
|
|
|
|
servo_motor_zero(actual_servo_motor);
|
|
}
|
|
}
|
|
|
|
// Set [motor] to 0
|
|
void motor_zero(motors_enum_t motor)
|
|
{
|
|
const motor_def_t *motor_def = &MOTORS_DEFS[motor];
|
|
|
|
// Set PWM to zero
|
|
pwm_set_gpio_level(motor_def->pwm_pin, 0);
|
|
|
|
// Set dir pins to false
|
|
gpio_put(motor_def->dir1_pin, false);
|
|
gpio_put(motor_def->dir2_pin, false);
|
|
}
|
|
|
|
// Set [motor] in motor_enum_t at [value] between -128 and 127 (for this config)
|
|
void motor_set(motors_enum_t motor, int value)
|
|
{
|
|
const motor_def_t *motor_def = &MOTORS_DEFS[motor];
|
|
|
|
if(value < 0)
|
|
{
|
|
gpio_put(motor_def->dir1_pin, true);
|
|
gpio_put(motor_def->dir2_pin, false);
|
|
|
|
value = -value;
|
|
}
|
|
else if(value > 0)
|
|
{
|
|
gpio_put(motor_def->dir1_pin, false);
|
|
gpio_put(motor_def->dir2_pin, true);
|
|
}
|
|
else
|
|
{
|
|
gpio_put(motor_def->dir1_pin, false);
|
|
gpio_put(motor_def->dir2_pin, false);
|
|
}
|
|
|
|
pwm_set_gpio_level(motor_def->pwm_pin, (uint16_t)value);
|
|
}
|
|
|
|
// Set servo motor to its open pos
|
|
void servo_motor_zero(servo_motors_enum_t servo_motor)
|
|
{
|
|
const servo_motor_def_t *servo_motor_def = &SERVO_MOTORS_DEFS[servo_motor];
|
|
|
|
// Set PWM to zero //
|
|
pwm_set_gpio_level(servo_motor_def->pwm_pin, servo_motor_def->open_pos);
|
|
}
|
|
|
|
// Set servo to its close pos if [close] else open pos
|
|
void servo_motor_set(servo_motors_enum_t servo_motor, bool close)
|
|
{
|
|
const servo_motor_def_t *servo_motor_def = &SERVO_MOTORS_DEFS[servo_motor];
|
|
|
|
// Set PWM to zero //
|
|
pwm_set_gpio_level(servo_motor_def->pwm_pin, close ? servo_motor_def->close_pos : servo_motor_def->open_pos);
|
|
}
|