From 2dc34eba7ed1fbd8ee18ad5d49585c73dc6e9b67 Mon Sep 17 00:00:00 2001 From: GrantMeAWish Date: Sun, 17 Nov 2019 23:15:47 -0800 Subject: [PATCH] demo with basic smooth driving working --- buckler/buckler_main.c | 40 +++++------- buckler/fsm.c | 139 ++++++++++++++++++++++++++--------------- buckler/fsm.h | 21 ++++--- buckler/states.h | 3 +- 4 files changed, 117 insertions(+), 86 deletions(-) diff --git a/buckler/buckler_main.c b/buckler/buckler_main.c index 6f15b08..4ade360 100644 --- a/buckler/buckler_main.c +++ b/buckler/buckler_main.c @@ -64,7 +64,7 @@ simple_ble_app_t* simple_ble_app; // controls ordering: accelerate, decelerate, left, right typedef struct { - char* name; + const char* name; uint16_t mask; uint8_t shift_amount; uint8_t value; @@ -86,29 +86,6 @@ void ble_evt_write(ble_evt_t const* p_ble_evt) { //printf("%x\n", stick_push_button.value); //printf("%x %x\n", bytes_look[0], bytes_look[1]); //printf("\n\n"); - - // TODO: There's no rest state at the moment, we are just constantly decelerating but hitting th floor of 0 - if (x_button.value == 1) { - // Acclerating - on_X_press(); - } else if (p_fsm.state == REST) { - rest(); - } else { - // Decelerate - on_X_release(); - } - - if (stick_push_button.value == 6) { - // Turning Left - on_l_stick_press(); - } else if (stick_push_button.value == 2) { - // Turning right - on_r_stick_press(); - } else { - // Go straight - on_stick_release(); - } - } void print_power_state(power_states current_state){ @@ -125,6 +102,10 @@ void print_power_state(power_states current_state){ display_write("DECELERATE", DISPLAY_LINE_0); break; } + case BRAKE: { + display_write("BRAKE", DISPLAY_LINE_0); + break; + } } } @@ -205,6 +186,7 @@ int main(void) { printf("Kobuki initialized!\n"); // Initialize the fsms + timer_init(); init_power_fsm(&p_fsm); init_turning_fsm(&t_fsm); @@ -218,7 +200,14 @@ int main(void) { rest(); } else { // Decelerate - on_X_release(); + on_button_release(); + } + + if (b_button.value == 1) { + //braking + on_B_press(); + } else if (p_fsm.state == REST) { + rest(); } if (stick_push_button.value == 6) { @@ -239,6 +228,7 @@ int main(void) { /* May read sensors later. */ // read sensors from robot //int status = kobukiSensorPoll(&sensors); + //nrf_delay_ms(100); } } diff --git a/buckler/fsm.c b/buckler/fsm.c index 934fb4a..a18a4c7 100644 --- a/buckler/fsm.c +++ b/buckler/fsm.c @@ -4,95 +4,132 @@ #include "kobukiUtilities.h" #include "fsm.h" -#include "time.h" #include "limits.h" +#include "nrf.h" #define TURNING_RATE .6 +#define PRESCALE_VALUE 4 +#define BASE_CLOCK 16000000.0 + power_fsm p_fsm; turning_fsm t_fsm; +static uint32_t read_timer(); + +static uint32_t read_timer() { + NRF_TIMER4->TASKS_CAPTURE[1] = 0x1; + return NRF_TIMER4->CC[1]; +} + +void timer_init(void) { + // Place your timer initialization code here + NRF_TIMER4->BITMODE = 0x3; + NRF_TIMER4->PRESCALER = PRESCALE_VALUE; + NRF_TIMER4->INTENSET = 0; + NRF_TIMER4->TASKS_CLEAR = 0x1; + NRF_TIMER4->TASKS_START = 0x1; +} + void init_power_fsm(power_fsm *fsm) { - fsm->state = REST; - fsm->p = 0.0f; - fsm->p_dot = 0.0f; - fsm->p_max = 700.0f; - fsm->t_prev = clock(); - fsm->t_curr = fsm->t_prev; + fsm->state = REST; + fsm->p = 0.0; + fsm->p_dot = 0.0; + fsm->p_max = 800.0; + fsm->t_prev = read_timer(); + fsm->t_curr = fsm->t_prev; } void init_turning_fsm(turning_fsm *fsm) { - fsm->state = CENTER; - fsm->p_left = 0.0f; - fsm->p_right = 0.0f; + fsm->state = CENTER; + fsm->p_left = 0.0; + fsm->p_right = 0.0; } void rest() { - p_fsm.state = REST; - p_fsm.t_prev = clock(); - p_fsm.t_curr = p_fsm.t_prev; - - p_update(); + p_fsm.state = REST; + p_fsm.t_prev = p_fsm.t_curr; + p_fsm.t_curr = read_timer(); + p_update(); } void on_X_press() { - p_fsm.state = ACCELERATE; - p_fsm.p_dot = 25; - p_fsm.t_prev = p_fsm.t_curr; - p_fsm.t_curr = clock(); + p_fsm.state = ACCELERATE; + p_fsm.p_dot = 400; + p_fsm.t_prev = p_fsm.t_curr; + p_fsm.t_curr = read_timer(); - p_update(); + p_update(); } -void on_X_release() { - p_fsm.state = DECELERATE; - p_fsm.p_dot = -10; - p_fsm.t_prev = p_fsm.t_curr; - p_fsm.t_curr = clock(); +void on_button_release() { + p_fsm.state = DECELERATE; + p_fsm.p_dot = -200; + p_fsm.t_prev = p_fsm.t_curr; + p_fsm.t_curr = read_timer(); - p_update(); + p_update(); } -void p_update() { - - double diff = ((double) (p_fsm.t_curr - p_fsm.t_prev)) / CLOCKS_PER_SEC; +void on_B_press() { + p_fsm.state = BRAKE; + p_fsm.p_dot = -400; + p_fsm.t_prev = p_fsm.t_curr; + p_fsm.t_curr = read_timer(); - // Handles overflow - // if ((p_fsm.t_curr - p_fsm.t_prev) < 0) { - // diff = UINT_MAX - diff; - // } + p_update(); +} - p_fsm.p = p_fsm.p + diff * p_fsm.p_dot; +void p_update() { - if (p_fsm.p > p_fsm.p_max) { - p_fsm.p = p_fsm.p_max; - } else if (p_fsm.p < 0) { - // TODO: Implement moving backwards - p_fsm.p = 0; - p_fsm.state = REST; - } + double change = 0.0; + if (p_fsm.t_curr < p_fsm.t_prev) { + uint32_t rescaled_curr = UINT32_MAX - p_fsm.t_curr; + change = (double) (rescaled_curr + 1 + p_fsm.t_prev); + } else { + change = (double) (p_fsm.t_curr - p_fsm.t_prev); + } + double diff = (change) / (BASE_CLOCK / (1 << PRESCALE_VALUE)); + printf("%lf\n", diff); + + // Handles overflow + // if ((p_fsm.t_curr - p_fsm.t_prev) < 0) { + // diff = UINT_MAX - diff; + // } + + p_fsm.p = p_fsm.p + diff * p_fsm.p_dot; + printf("%lf\n\n\n\n", p_fsm.p); + + + if (p_fsm.p > p_fsm.p_max) { + p_fsm.p = p_fsm.p_max; + } else if (p_fsm.p < 0) { + // TODO: Implement moving backwards + p_fsm.p = 0; + p_fsm.state = REST; + } } void on_l_stick_press() { - t_fsm.state = LEFT; - t_fsm.p_right = p_fsm.p * TURNING_RATE; - t_fsm.p_left = p_fsm.p * (1 - TURNING_RATE); + t_fsm.state = LEFT; + t_fsm.p_right = p_fsm.p * TURNING_RATE; + t_fsm.p_left = p_fsm.p * (1 - TURNING_RATE); } void on_r_stick_press() { - t_fsm.state = RIGHT; - t_fsm.p_left = p_fsm.p * TURNING_RATE; - t_fsm.p_right = p_fsm.p * (1 - TURNING_RATE); + t_fsm.state = RIGHT; + t_fsm.p_left = p_fsm.p * TURNING_RATE; + t_fsm.p_right = p_fsm.p * (1 - TURNING_RATE); } void on_stick_release() { - t_fsm.state = CENTER; - t_fsm.p_left = p_fsm.p / 2.0; - t_fsm.p_right = p_fsm.p / 2.0; + t_fsm.state = CENTER; + t_fsm.p_left = p_fsm.p / 2.0; + t_fsm.p_right = p_fsm.p / 2.0; } void drive() { - kobukiDriveDirect(t_fsm.p_left, t_fsm.p_right); + kobukiDriveDirect(t_fsm.p_left, t_fsm.p_right); } diff --git a/buckler/fsm.h b/buckler/fsm.h index 276bf73..c95ad2f 100644 --- a/buckler/fsm.h +++ b/buckler/fsm.h @@ -1,30 +1,33 @@ #ifndef FSM_H #define FSM_H -#include "time.h" +#include #include "states.h" typedef struct { power_states state; - float p; - float p_dot; - float p_max; - clock_t t_curr; - clock_t t_prev; + double p; + double p_dot; + double p_max; + uint32_t t_curr; + uint32_t t_prev; } power_fsm; typedef struct { turning_states state; - float p_left; - float p_right; + double p_left; + double p_right; } turning_fsm; void init_power_fsm(power_fsm *fsm); void init_turning_fsm(turning_fsm *fsm); +void timer_init(); + void rest(); void on_X_press(); -void on_X_release(); +void on_button_release(); +void on_B_press(); void p_update(); void on_l_stick_press(); void on_r_stick_press(); diff --git a/buckler/states.h b/buckler/states.h index 07793c2..0c810cc 100644 --- a/buckler/states.h +++ b/buckler/states.h @@ -6,7 +6,8 @@ typedef enum { REST, ACCELERATE, - DECELERATE + DECELERATE, + BRAKE } power_states; typedef enum {