demo with basic smooth driving working

njriasan · Nov 18, 2019 · 2dc34eb · 2dc34eb
1 parent f6fc1b6
commit 2dc34eb
Show file tree

Hide file tree

Showing 4 changed files with 117 additions and 86 deletions.
diff --git 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
@@ -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
@@ -1,30 +1,33 @@
 #ifndef FSM_H
 #define FSM_H
 
-#include "time.h"
+#include <stdint.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;
+	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
@@ -6,7 +6,8 @@
 typedef enum {
 	REST,
 	ACCELERATE,
-	DECELERATE
+	DECELERATE,
+	BRAKE
 } power_states;
 
 typedef enum {