Skip to content

Commit

Permalink
Merge pull request #7 from njriasan/fsm
Browse files Browse the repository at this point in the history
  • Loading branch information
@GrantMeAWish
GrantMeAWish committed Nov 18, 2019
2 parents bbfc6f8 + 2dc34eb commit 275a264
Show file tree
Hide file tree
Showing 5 changed files with 270 additions and 125 deletions.
184 changes: 78 additions & 106 deletions buckler/buckler_main.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,6 +28,7 @@
#include "simple_ble.h"

#include "states.h"
#include "fsm.h"

#define NUM_BUTTONS 4

Expand All @@ -37,8 +38,6 @@ NRF_TWI_MNGR_DEF(twi_mngr_instance, 5, 0);
// global variables
KobukiSensors_t sensors = {0};

states state = OFF;

// Intervals for advertising and connections
static simple_ble_config_t ble_config = {
// c0:98:e5:yy:xx:xx
Expand All @@ -65,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;
Expand All @@ -83,50 +82,48 @@ void ble_evt_write(ble_evt_t const* p_ble_evt) {
for (unsigned int i = 0; i < NUM_BUTTONS; i++) {
buttons[i]->value = (buttons[i]->mask & controller_bytes) >> buttons[i]->shift_amount;
}
uint8_t *bytes_look = (uint8_t *) &controller_bytes;
//uint8_t *bytes_look = (uint8_t *) &controller_bytes;
//printf("%x\n", stick_push_button.value);
//printf("%x %x\n", bytes_look[0], bytes_look[1]);
//printf("\n\n");
}


if (x_button.value == 1) {
if (stick_push_button.value == 6) {
state = LEFT;
} else if (stick_push_button.value == 2) {
state = RIGHT;
} else {
state = ACCELERATE;
}
} else if (b_button.value == 1) {
state = DECELERATE;
} else {
state = OFF;
}
void print_power_state(power_states current_state){
switch(current_state){
case REST: {
display_write("OFF", DISPLAY_LINE_0);
break;
}
case ACCELERATE: {
display_write("ACCELERATE", DISPLAY_LINE_0);
break;
}
case DECELERATE: {
display_write("DECELERATE", DISPLAY_LINE_0);
break;
}
case BRAKE: {
display_write("BRAKE", DISPLAY_LINE_0);
break;
}
}
}

void print_state(states current_state){
void print_turning_state(turning_states current_state){
switch(current_state){
case OFF: {
display_write("OFF", DISPLAY_LINE_0);
break;
}
case ACCELERATE: {
display_write("ACCELERATE", DISPLAY_LINE_0);
break;
}
case DECELERATE: {
display_write("DECELERATE", DISPLAY_LINE_0);
break;
}
case LEFT: {
display_write("LEFT", DISPLAY_LINE_0);
break;
}
case RIGHT: {
display_write("RIGHT", DISPLAY_LINE_0);
break;
}
}
case LEFT: {
display_write("LEFT", DISPLAY_LINE_1);
break;
}
case CENTER: {
display_write("CENTER", DISPLAY_LINE_1);
break;
}
case RIGHT: {
display_write("RIGHT", DISPLAY_LINE_1);
break;
}
}
}

int main(void) {
Expand Down Expand Up @@ -188,75 +185,50 @@ int main(void) {
kobukiInit();
printf("Kobuki initialized!\n");

// Initialize the fsms
timer_init();
init_power_fsm(&p_fsm);
init_turning_fsm(&t_fsm);

// loop forever, running state machine
while (1) {
// 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_button_release();
}

if (b_button.value == 1) {
//braking
on_B_press();
} else if (p_fsm.state == REST) {
rest();
}

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();
}
// Drive the kobuki
drive();

print_power_state(p_fsm.state);
print_turning_state(t_fsm.state);
/* May read sensors later. */
// read sensors from robot
int status = kobukiSensorPoll(&sensors);

// TODO: complete state machine
switch(state) {
case OFF: {
print_state(state);

// transition logic
if (is_button_pressed(&sensors)) {
// state = FORWARD;
} else {
state = OFF;
// perform state-specific actions here
kobukiDriveDirect(0, 0);
}
break; // each case needs to end with break!
}

case ACCELERATE: {
print_state(state);

if (is_button_pressed(&sensors)) {
state = OFF;
} else {
// perform state-specific actions here
kobukiDriveDirect(700, 700);
}
break; // each case needs to end with break!
}

case DECELERATE: {
print_state(state);

if (is_button_pressed(&sensors)) {
state = OFF;
} else {
// perform state-specific actions here
kobukiDriveDirect(-100, -100);
}
break; // each case needs to end with break!
}

case LEFT: {
print_state(state);

if (is_button_pressed(&sensors)) {
state = OFF;
} else {
// perform state-specific actions here
kobukiDriveDirect(650, 700);
}
break; // each case needs to end with break!
}

case RIGHT: {
print_state(state);

if (is_button_pressed(&sensors)) {
state = OFF;
} else {
// perform state-specific actions here
kobukiDriveDirect(700, 650);
}
break; // each case needs to end with break!
}
}
//int status = kobukiSensorPoll(&sensors);
//nrf_delay_ms(100);
}
}

135 changes: 135 additions & 0 deletions buckler/fsm.c
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,135 @@
#include "kobukiActuator.h"
#include "kobukiSensorPoll.h"
#include "kobukiSensorTypes.h"
#include "kobukiUtilities.h"

#include "fsm.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.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.0;
fsm->p_right = 0.0;
}

void rest() {
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 = 400;
p_fsm.t_prev = p_fsm.t_curr;
p_fsm.t_curr = read_timer();

p_update();
}

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();
}

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();

p_update();
}

void p_update() {

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);
}

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);
}


Loading

0 comments on commit 275a264

Please sign in to comment.