From a1f952125e0b39799c76e30bc570066e4d126621 Mon Sep 17 00:00:00 2001 From: "E.Hunckler" Date: Sat, 27 Jul 2024 01:09:52 -0500 Subject: [PATCH] - added motor driver for basic control no pwm --- main_app.c | 331 ++++++++++------------------------------------ motor_driver.c | 86 +++++++++--- motor_driver.h | 7 +- pin_definitions.h | 15 +++ 4 files changed, 157 insertions(+), 282 deletions(-) create mode 100644 pin_definitions.h diff --git a/main_app.c b/main_app.c index 9e70e96..1214492 100644 --- a/main_app.c +++ b/main_app.c @@ -3,267 +3,83 @@ #include #include - #include "hardware/gpio.h" #include "hardware/pwm.h" #include "pico/stdlib.h" #include "pico/multicore.h" - #include "motor_driver.h" - #include "bt_hid.h" -// These magic values are just taken from M0o+, not calibrated for -// the Tiny chassis. -#define PWM_MIN 80 -#define PWM_MAX (PWM_MIN + 127) - -static inline int8_t clamp8(int16_t value) { - if (value > 127) { - return 127; - } else if (value < -128) { - return -128; - } - - return value; -} - -struct slice { - unsigned int slice_num; - unsigned int pwm_min; -}; - -struct half_slice { - struct slice *slice; - enum pwm_chan chan; -}; - -struct chassis { - struct slice slice_l; - struct slice slice_r; - - int8_t l; - int8_t r; -}; - -struct halfslice_chassis { -#define MAX_N_SLICES 4 - int n_slices; - struct slice slices[MAX_N_SLICES]; - struct half_slice la, lb, ra, rb; - - int8_t l; - int8_t r; -}; - -static inline uint8_t abs8(int8_t v) { - return v < 0 ? -v : v; -} - -void init_slice(struct slice *slice, unsigned int slice_num, unsigned int pwm_min); - -void __init_halfslice_pin(struct halfslice_chassis *chassis, struct half_slice *hs, uint8_t pin) -{ - struct slice *slice = NULL; - - enum pwm_chan chan = pwm_gpio_to_channel(pin); - uint slice_num = pwm_gpio_to_slice_num(pin); - printf("pin %d on slice %d\n", pin, slice_num); - - for (int i = 0; i < chassis->n_slices; i++) { - if (chassis->slices[i].slice_num == slice_num) { - printf("slice %d already at idx %d\n", slice_num, i); - slice = &chassis->slices[i]; - break; - } - } - if (slice == NULL) { - slice = &chassis->slices[chassis->n_slices]; - init_slice(slice, slice_num, PWM_MIN); - printf("assigned slice %d at idx %d\n", slice_num, chassis->n_slices); - chassis->n_slices++; - } - gpio_set_function(pin, GPIO_FUNC_PWM); - - hs->slice = slice; - hs->chan = chan; - printf("pin: %d, slice: %p, slice_num: %d, chan: %d\n", - pin, slice, slice_num, chan); -} - -void halfslice_chassis_init(struct halfslice_chassis *chassis, - uint8_t pin_la, uint8_t pin_lb, uint8_t pin_ra, uint8_t pin_rb) -{ - __init_halfslice_pin(chassis, &chassis->la, pin_la); - __init_halfslice_pin(chassis, &chassis->lb, pin_lb); - __init_halfslice_pin(chassis, &chassis->ra, pin_ra); - __init_halfslice_pin(chassis, &chassis->rb, pin_rb); -} - -void halfslice_pair_set_with_brake(struct half_slice *a, struct half_slice *b, int8_t value, bool brake) -{ - uint8_t mag = abs8(value); - - if (value == 0) { - pwm_set_chan_level(a->slice->slice_num, a->chan, brake ? a->slice->pwm_min + 127 : 0); - pwm_set_chan_level(b->slice->slice_num, b->chan, brake ? a->slice->pwm_min + 127 : 0); - } else if (value < 0) { - pwm_set_chan_level(a->slice->slice_num, a->chan, a->slice->pwm_min + mag); - pwm_set_chan_level(b->slice->slice_num, b->chan, 0); - } else { - pwm_set_chan_level(a->slice->slice_num, a->chan, 0); - pwm_set_chan_level(b->slice->slice_num, b->chan, b->slice->pwm_min + mag); - } -} - -void halfslice_pair_set(struct half_slice *a, struct half_slice *b, int8_t value) -{ - halfslice_pair_set_with_brake(a, b, value, false); -} - -void halfslice_chassis_set_raw(struct halfslice_chassis *chassis, int8_t left, int8_t right) -{ - halfslice_pair_set(&chassis->la, &chassis->lb, left); - halfslice_pair_set(&chassis->ra, &chassis->rb, right); - - chassis->l = left; - chassis->r = right; -} - -void halfslice_chassis_set(struct halfslice_chassis *chassis, int8_t linear, int8_t rot) +static int clamp(int val, int min, int max) { - // Positive rotation == CCW == right goes faster - - if (linear < -127) { - linear = -127; - } - - if (rot < -127) { - rot = -127; - } - - int l = linear - rot; - int r = linear + rot; - int adj = 0; - - if (l > 127) { - adj = l - 127; - } else if (l < -127) { - adj = l + 127; - }else if (r > 127) { - adj = r - 127; - } else if (r < -127) { - adj = r + 127; + if( val > max ) + { + return max; } - - l -= adj; - r -= adj; - - // FIXME: Motor directions should be a parameter - r = -r; - - halfslice_chassis_set_raw(chassis, l, r); -} - -void init_slice(struct slice *slice, unsigned int slice_num, unsigned int pwm_min) -{ - slice->slice_num = slice_num; - slice->pwm_min = pwm_min; - pwm_set_wrap(slice->slice_num, slice->pwm_min + 127 + 1); - pwm_set_chan_level(slice->slice_num, PWM_CHAN_A, 0); - pwm_set_chan_level(slice->slice_num, PWM_CHAN_B, 0); - pwm_set_enabled(slice->slice_num, true); -} - -void chassis_init(struct chassis *chassis, uint8_t pin_la, uint8_t pin_ra) -{ - - init_slice(&chassis->slice_l, pwm_gpio_to_slice_num(pin_la), PWM_MIN); - gpio_set_function(pin_la, GPIO_FUNC_PWM); - gpio_set_function(pin_la + 1, GPIO_FUNC_PWM); - init_slice(&chassis->slice_r, pwm_gpio_to_slice_num(pin_ra), PWM_MIN); - gpio_set_function(pin_ra, GPIO_FUNC_PWM); - gpio_set_function(pin_ra + 1, GPIO_FUNC_PWM); -} - -void slice_set_with_brake(struct slice *slice, int8_t value, bool brake) -{ - uint8_t mag = abs8(value); - - if (value == 0) { - pwm_set_both_levels(slice->slice_num, brake ? slice->pwm_min + 127 : 0, brake ? slice->pwm_min + 127 : 0); - } else if (value < 0) { - pwm_set_both_levels(slice->slice_num, slice->pwm_min + mag, 0); - } else { - pwm_set_both_levels(slice->slice_num, 0, slice->pwm_min + mag); + if( val < min ) + { + return min; } + return val; } -void slice_set(struct slice *slice, int8_t value) -{ - slice_set_with_brake(slice, value, false); -} - -void chassis_set_raw(struct chassis *chassis, int8_t left, int8_t right) -{ - slice_set(&chassis->slice_l, left); - slice_set(&chassis->slice_r, right); - - chassis->l = left; - chassis->r = right; -} - -void chassis_set(struct chassis *chassis, int8_t linear, int8_t rot) +static void update_motor_driver_from_control_input( struct bt_hid_state* state ) { - // Positive rotation == CCW == right goes faster - if (linear < -127) { - linear = -127; + // printf("buttons: %04x, l: %d,%d, r: %d,%d, l2,r2: %d,%d hat: %d\n", + // state->buttons, state->lx, state->ly, state->rx, state->ry, + // state->l2, state->r2, state->hat); + + // Motor Mixer + int forward_speed = (256 - state->ly) - 128; // 128 is full forward, 0 is stop, -128 is backwards + int swerve_right_rate = (state->rx - 128); // 128 is max right swerve rate, 0 is no swerve, -128 is max left swerve + + int left_output = forward_speed; + int right_output = forward_speed; + if( forward_speed > 0 ) + { + if( swerve_right_rate > 0 ) + { + right_output -= swerve_right_rate / 2; + right_output = clamp(right_output, 0, 128); + } + else if( swerve_right_rate < 0 ) + { + left_output -= swerve_right_rate / 2; + left_output = clamp(left_output, 0, 128); + } } - - if (rot < -127) { - rot = -127; + else if( forward_speed < 0 ) + { + if( swerve_right_rate > 0 ) + { + right_output += swerve_right_rate / 2; + right_output = clamp(right_output, -127, 0); + } + else if( swerve_right_rate < 0 ) + { + left_output += swerve_right_rate / 2; + left_output = clamp(left_output, -127, 0); + } } - - int l = linear - rot; - int r = linear + rot; - int adj = 0; - - if (l > 127) { - adj = l - 127; - } else if (l < -127) { - adj = l + 127; - }else if (r > 127) { - adj = r - 127; - } else if (r < -127) { - adj = r + 127; + else + { + if( swerve_right_rate != 0 ) + { + // rotate in place + // This handles both rotate left and right as left will be negative and right positive + left_output = swerve_right_rate / 2; + right_output = -1*swerve_right_rate / 2; + } + // otherwise no rotation } - l -= adj; - r -= adj; - - // FIXME: Motor directions should be a parameter - r = -r; - - chassis_set_raw(chassis, l, r); -} - -static void update_motor_driver_from_control_input( struct halfslice_chassis* chassis, struct bt_hid_state* state ) -{ - - printf("buttons: %04x, l: %d,%d, r: %d,%d, l2,r2: %d,%d hat: %d\n", - state->buttons, state->lx, state->ly, state->rx, state->ry, - state->l2, state->r2, state->hat); + left_output = clamp(left_output, -127, 128); + right_output = clamp(right_output, -127, 128); - // int8_t linear_scaling = - - // motor_driver_set_speed( float val ); - // motor_driver_set_direction( bool forward ); - - float speed_scale = 1.0; - int8_t linear = clamp8(-(state->ly - 128) * speed_scale); - int8_t rot = clamp8(-(state->rx - 128)); - halfslice_chassis_set( chassis, linear, rot); + // Set motor driver + motor_driver_set_left( left_output ); + motor_driver_set_right( right_output ); } enum SystemStates @@ -273,23 +89,21 @@ enum SystemStates // Add additional state here... }; -void main(void) { - stdio_init_all(); - - multicore_launch_core1(bt_main); - // Wait for init (should do a handshake with the fifo here?) - sleep_ms(1000); - - //struct chassis chassis = { 0 }; - //chassis_init(&chassis, 6, 8); - struct halfslice_chassis chassis = { 0 }; - halfslice_chassis_init(&chassis, 19, 17, 16, 15); - +void main(void) +{ + // Variables used to control the main state machine enum SystemStates next_system_state = SystemState_DISCONNECTED; enum SystemStates system_state = SystemState_DISCONNECTED; enum SystemStates prev_system_state = SystemState_DISCONNECTED; - + // Structure used to hold the Bluetooth controller data struct bt_hid_state controller_state = {0}; + + stdio_init_all(); + multicore_launch_core1(bt_main); + // Wait for init + sleep_ms(1000); + motor_driver_setup(); + while ( 1 ) { const bool system_state_entered = ( system_state != prev_system_state ); @@ -302,7 +116,7 @@ void main(void) { { // When entering the state run these functions 1 time. // Set the motors to a safe state. - motor_driver_disable(); + motor_driver_stop(); } // Run the following continuously while in the disconnected mode. // Check to see if we are connected. @@ -317,15 +131,14 @@ void main(void) { if( system_state_entered ) { // When entering the state run these functions 1 time. - // Set the motors to a safe state. - motor_driver_enable(); + printf("SystemState_CONNECTED\n"); } // Check to see if we are connected. // Handle the latest controller input. if( bt_hid_is_connected() ) { // handle commands and update the motor. - update_motor_driver_from_control_input(&chassis, &controller_state); + update_motor_driver_from_control_input(&controller_state); } else { diff --git a/motor_driver.c b/motor_driver.c index 2ddbcaa..2dfdf66 100644 --- a/motor_driver.c +++ b/motor_driver.c @@ -2,39 +2,87 @@ // Eddie Hunckler 2024 // +#include #include "motor_driver.h" #include "hardware/gpio.h" #include "hardware/pwm.h" #include "pico/stdlib.h" - -// #define motor_driver_printf( x ) -#define motor_driver_printf( x ) printf( x ) +#include "pin_definitions.h" void motor_driver_setup(void) { - motor_driver_printf("motor driver setup\n"); - // TODO -} + printf("motor driver setup\n"); -void motor_driver_enable(void) -{ - motor_driver_printf("motor driver enable\n"); - // TODO + // Left motor driver initialization + gpio_init(PIN_LEFT_MOTOR_INA); + gpio_init(PIN_LEFT_MOTOR_INB); + gpio_set_dir(PIN_LEFT_MOTOR_INA, GPIO_OUT); + gpio_set_dir(PIN_LEFT_MOTOR_INB, GPIO_OUT); + gpio_put(PIN_LEFT_MOTOR_INA, 0); + gpio_put(PIN_LEFT_MOTOR_INB, 0); + + // Right motor driver initialization + gpio_init(PIN_RIGHT_MOTOR_INA); + gpio_init(PIN_RIGHT_MOTOR_INB); + gpio_set_dir(PIN_RIGHT_MOTOR_INA, GPIO_OUT); + gpio_set_dir(PIN_RIGHT_MOTOR_INB, GPIO_OUT); + gpio_put(PIN_RIGHT_MOTOR_INA, 0); + gpio_put(PIN_RIGHT_MOTOR_INB, 0); } -void motor_driver_disable(void) +void motor_driver_stop(void) { - motor_driver_printf("motor driver disable\n"); - // TODO + printf("motor driver stop\n"); + gpio_put(PIN_LEFT_MOTOR_INA, 0); + gpio_put(PIN_LEFT_MOTOR_INB, 0); + gpio_put(PIN_RIGHT_MOTOR_INA, 0); + gpio_put(PIN_RIGHT_MOTOR_INB, 0); } -void motor_driver_set_speed( float val ) +void motor_driver_set_left( int val ) { - // TODO - + if( val > 0 ) + { + // clockwise + gpio_put(PIN_LEFT_MOTOR_INA, 0); // 1); + gpio_put(PIN_LEFT_MOTOR_INB, 1); // 0); + // TODO Set PWM + } + else if( val < 0 ) + { + // counter closkwise + gpio_put(PIN_LEFT_MOTOR_INA, 1); // 0); + gpio_put(PIN_LEFT_MOTOR_INB, 0); // 1); + // TODO Set PWM + } + else + { + // stop + gpio_put(PIN_LEFT_MOTOR_INA, 0); + gpio_put(PIN_LEFT_MOTOR_INB, 0); + } } -void motor_driver_set_direction( bool forward ) +void motor_driver_set_right( int val ) { - // TODO -} + if( val > 0 ) + { + // counter clockwise + gpio_put(PIN_RIGHT_MOTOR_INA, 0); + gpio_put(PIN_RIGHT_MOTOR_INB, 1); + // TODO Set PWM + } + else if( val < 0 ) + { + // clockwise + gpio_put(PIN_RIGHT_MOTOR_INA, 1); + gpio_put(PIN_RIGHT_MOTOR_INB, 0); + // TODO Set PWM + } + else + { + // stop + gpio_put(PIN_RIGHT_MOTOR_INA, 0); + gpio_put(PIN_RIGHT_MOTOR_INB, 0); + } +} \ No newline at end of file diff --git a/motor_driver.h b/motor_driver.h index 883e9a2..0b59668 100644 --- a/motor_driver.h +++ b/motor_driver.h @@ -4,9 +4,8 @@ #include void motor_driver_setup(void); -void motor_driver_enable(void); -void motor_driver_disable(void); -void motor_driver_set_speed( float val ); -void motor_driver_set_direction( bool forward ); +void motor_driver_stop(void); +void motor_driver_set_left( int val ); +void motor_driver_set_right( int val ); #endif // _MOTOR_DRIVER_H_ \ No newline at end of file diff --git a/pin_definitions.h b/pin_definitions.h new file mode 100644 index 0000000..f0aed77 --- /dev/null +++ b/pin_definitions.h @@ -0,0 +1,15 @@ +#ifndef _PIN_DEFINITIONS_H_ +#define _PIN_DEFINITIONS_H_ + +// LEFT MOTOR DRIVER +#define PIN_LEFT_MOTOR_INA 6 // GPO +#define PIN_LEFT_MOTOR_INB 7 // GPO +#define PIN_LEFT_MOTOR_PWM 8 // PWM4 A + +// RIGHT MOTOR DRIVER +#define PIN_RIGHT_MOTOR_INA 10 // GPO +#define PIN_RIGHT_MOTOR_INB 11 // GPO +#define PIN_RIGHT_MOTOR_PWM 12 // PWM6 A + + +#endif // _PIN_DEFINITIONS_H_ \ No newline at end of file