Skip to content

Commit

Permalink
- added motor driver for basic control no pwm
Browse files Browse the repository at this point in the history
  • Loading branch information
@ehunck
ehunck committed Jul 27, 2024
1 parent 9bb9052 commit a1f9521
Show file tree
Hide file tree
Showing 4 changed files with 157 additions and 282 deletions.
331 changes: 72 additions & 259 deletions main_app.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,267 +3,83 @@

#include <stdio.h>
#include <string.h>

#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
Expand All @@ -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 );
Expand All @@ -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.
Expand All @@ -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
{
Expand Down
Loading

0 comments on commit a1f9521

Please sign in to comment.