From c1abc8937f0498581dc30cf4376365dbde6df2b1 Mon Sep 17 00:00:00 2001 From: Sean Farhat Date: Sun, 17 Nov 2019 15:46:07 -0800 Subject: [PATCH 1/2] implemented composite fsm logic for power and turning --- buckler/buckler_main.c | 36 +++++++++++++----- buckler/fsm.c | 83 ++++++++++++++++++++++++++++++++++++++++++ buckler/fsm.h | 31 ++++++++++++++++ buckler/states.h | 23 +++++------- rpi_ble/send_pkt.py | 8 ++-- 5 files changed, 154 insertions(+), 27 deletions(-) create mode 100644 buckler/fsm.c create mode 100644 buckler/fsm.h diff --git a/buckler/buckler_main.c b/buckler/buckler_main.c index e690910..453a3b9 100644 --- a/buckler/buckler_main.c +++ b/buckler/buckler_main.c @@ -63,26 +63,42 @@ static uint16_t controller_bytes; simple_ble_app_t* simple_ble_app; // controls ordering: accelerate, decelerate, left, right +// This is incomplete, just a basic mapping to go forward and turn + +// Buttons are X (accelerate), B (decelerate), Left, Right static bool controls[NUM_BUTTONS] = {false, false, false, false}; -static uint16_t masks[NUM_BUTTONS] = {0b1, 0b1 << 3, 0b11 << 10, 0b1 << 11}; +static uint16_t masks[NUM_BUTTONS] = {0b1 << 3, 0b1, 0b11 << 10, 0b1 << 11}; void ble_evt_write(ble_evt_t const* p_ble_evt) { // TODO: logic for each characteristic and related state changes + // TODO: for the stick presses, we need to do equality checks instead for (unsigned int i = 0; i < NUM_BUTTONS; i++) { controls[i] = controller_bytes & masks[i]; } + // TODO: There's no rest state at the moment, we are just constantly decelerating but hitting th floor of 0 if (controls[0] == true) { - if (controls[2] == true) { - state = LEFT; - } else if (controls[3] == true) { - state = RIGHT; - } else { - state = ACCELERATE; - } - } else if (controls[1] == true) { - state = DECELERATE; + // Acclerating + on_X_press(); + } else if (p_fsm.state == REST) { + rest(); + } else { + // Decelerate + on_X_release(); } + + if (controls[2] == true) { + // Turning Left + on_l_stick_press(); + } else if (controls[3] == true) { + // Turning right + on_r_stick_press(); + } else { + // Go straight + on_stick_release(); + } + + drive(); } void print_state(states current_state){ diff --git a/buckler/fsm.c b/buckler/fsm.c new file mode 100644 index 0000000..3835c54 --- /dev/null +++ b/buckler/fsm.c @@ -0,0 +1,83 @@ +#include "kobukiActuator.h" +#include "kobukiSensorPoll.h" +#include "kobukiSensorTypes.h" +#include "kobukiUtilities.h" + +#include "fsm.h" +#include "time.h" +#include "limits.h" + +#define TURNING_RATE .6 + +power_fsm p_fsm; +turning_fsm t_fsm; + +void rest() { + p_fsm.state = REST; + p_fsm.t_prev = clock() + p_fsm.t_curr = p_fsm.t_prev; + + 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_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(); + + p_update(); +} + +void p_update() { + + double diff = ((double) (p_fsm.t_curr - p_fsm.t_prev)) / CLOCKS_PER_SEC; + + // Handles overflow + // if ((p_fsm.t_curr - p_fsm.t_prev) < 0) { + // diff = UINT_MAX - diff; + // } + + p_fsm.p = p + diff * p_dot; + + 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); +} + +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); +} + +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; +} + +void drive() { + kobukiDriveDirect(t_fsm.p_left, t_fsm.p_right); +} + + diff --git a/buckler/fsm.h b/buckler/fsm.h new file mode 100644 index 0000000..ab260fa --- /dev/null +++ b/buckler/fsm.h @@ -0,0 +1,31 @@ +#ifndef FSM_H +#define FSM_H + +#include "time.h" +#include "states.h" + +typedef struct { + power_states state; + float p; + float p_dot; + float p_max; + clock_t t_curr; + clock_t t_prev; +} power_fsm; + +typedef struct { + turning_states state; + float p_left; + float p_right; +} turning_fsm; + +void rest(); +void on_X_press(); +void on_X_release(); +void p_update(); +void on_l_stick_press(); +void on_r_stick_press(); +void on_stick_release(); +void drive(); + +#endif diff --git a/buckler/states.h b/buckler/states.h index a34414c..70a2851 100644 --- a/buckler/states.h +++ b/buckler/states.h @@ -1,21 +1,18 @@ -/* - * states.h - * - * Created on: Sep 22, 2018 - * Author: shromonaghosh - */ - #ifndef STATES_H_ #define STATES_H_ #include typedef enum { - OFF, - ACCELERATE, - DECELERATE, - LEFT, - RIGHT, -} states; + REST, + ACCELERATE, + DECELERATE +} power_states; + +typedef enum { + CENTER, + LEFT, + RIGHT +} turning_states; #endif /* STATES_H_ */ diff --git a/rpi_ble/send_pkt.py b/rpi_ble/send_pkt.py index cfe2efe..7705fde 100644 --- a/rpi_ble/send_pkt.py +++ b/rpi_ble/send_pkt.py @@ -18,10 +18,10 @@ SERVICE_UUID = "4607eda0-f65e-4d59-a9ff-84420d87a4ca" CHAR_UUIDS = [ - "4607eda1-f65e-4d59-a9ff-84420d87a4ca", - "4607eda2-f65e-4d59-a9ff-84420d87a4ca", - "4607eda3-f65e-4d59-a9ff-84420d87a4ca", - "4607eda4-f65e-4d59-a9ff-84420d87a4ca" + "4607eda1-f65e-4d59-a9ff-84420d87a4ca" + # "4607eda2-f65e-4d59-a9ff-84420d87a4ca", + # "4607eda3-f65e-4d59-a9ff-84420d87a4ca", + # "4607eda4-f65e-4d59-a9ff-84420d87a4ca" ] class RobotController(): From 2dc34eba7ed1fbd8ee18ad5d49585c73dc6e9b67 Mon Sep 17 00:00:00 2001 From: GrantMeAWish Date: Sun, 17 Nov 2019 23:15:47 -0800 Subject: [PATCH 2/2] 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 {