make stay-on-table non-blocking

Change-Id: I627c013087236d642c5607b13ffe9841ea363ab3

diff --git a/build-stages/i_chooser/i_chooser.ino b/build-stages/i_chooser/i_chooser.ino
index b90574a3e776c82dc7c6d1e800b5531cc6c8dc14..6eabcaf90f157a2ce79a7bceef243165b27ee5cb 100644 (file)
--- a/build-stages/i_chooser/i_chooser.ino
+++ b/build-stages/i_chooser/i_chooser.ino
@@ -69,14 +69,6 @@ int ping(int trigger, int echo){
   delay(50);
   return ping_time;}
 
   delay(50);
   return ping_time;}
 

-void backup(int backup_time){
-  go(-250, -250);
-  delay(backup_time);}
-
-void turn_around(int turn_around_time){
-  go(-250, 250);
-  delay(turn_around_time);}
-

 void setup(){
   Serial.begin(9600);
 
 void setup(){
   Serial.begin(9600);
 


@@ -108,71 +100,120 @@ void setup(){
   off(right_motor_backward_pin);
   off(right_trigger_pin);}
 
   off(right_motor_backward_pin);
   off(right_trigger_pin);}
 

-void stay_on_table(){
-  Serial.print("stay on table");
-  int left_speed;
-  int right_speed;
+enum class Stay_on_table_state {
+  going, start_backing, backing, start_turning, turning};
+static Stay_on_table_state stay_on_table_state =
+  Stay_on_table_state::going;

 
 

+void going() {
+  Serial.print("going ");

   int forward_speed = 250;
   int stop_speed = 0;
 
   int forward_speed = 250;
   int stop_speed = 0;
 

+  int left_speed;
+  int right_speed;
+

   // adjust this number as necessary for your robot.
   // it represents how far the table is from your sonar sensor.
   // larger values mean larger distance.  default is 800
   // adjust this number as necessary for your robot.
   // it represents how far the table is from your sonar sensor.
   // larger values mean larger distance.  default is 800

-  int right_max_ping_time_over_table = 800;
-  int left_max_ping_time_over_table = 800;
-  int backup_time = 500;
+  const int right_max_ping_time_over_table = 800;
+  const int left_max_ping_time_over_table = 800;
+
+  const int left_ping_time =
+    ping(left_trigger_pin, left_echo_pin);
+  const int right_ping_time =
+    ping(right_trigger_pin, right_echo_pin);
+
+  if (left_ping_time <= left_max_ping_time_over_table
+      || right_ping_time <= right_max_ping_time_over_table) {
+    if(left_ping_time <= left_max_ping_time_over_table) {
+      left_speed = forward_speed; }
+    else {
+      left_speed = stop_speed; }
+    if(right_ping_time <= right_max_ping_time_over_table) {
+      right_speed = forward_speed; }
+    else {
+      right_speed = stop_speed; } }
+  else { // both ping times were above max acceptable ping time
+    left_speed = right_speed = 0;
+    stay_on_table_state = Stay_on_table_state::start_backing; }
+
+  go(left_speed, right_speed); }
+
+void backing(unsigned long start_backing) {
+  Serial.print("backing ");
+  static const unsigned int allowed_backup_duration = 500;
+  unsigned long now = millis();
+  unsigned long backup_duration = now - start_backing;
+
+  go(-250, -250);
+
+  if(backup_duration > allowed_backup_duration) {
+    stay_on_table_state = Stay_on_table_state::start_turning; } }
+
+void turning(unsigned long start_turning_time) {
+  Serial.print("turning ");

   // the exact amount of time for turning around might need
   // twerking for your robot.  the default value is 3200
   // the exact amount of time for turning around might need
   // twerking for your robot.  the default value is 3200

-  int turn_around_time = 2500;
+  static const unsigned int allowed_turning_duration = 2500;
+  unsigned long now = millis();
+  unsigned long turning_duration = now - start_turning_time;
+
+  go(-250, 250);
+
+  if(turning_duration > allowed_turning_duration) {
+    stay_on_table_state = Stay_on_table_state::going; } }
+
+void stay_on_table(){
+  Serial.print("stay on table ");
+  static unsigned long start_backing_time = 0;
+  static unsigned long start_turning_time = 0;
+

 
   int actual_left_ping_time = ping(left_trigger_pin, left_echo_pin);
   int actual_right_ping_time = ping(right_trigger_pin, right_echo_pin);
 
 
   int actual_left_ping_time = ping(left_trigger_pin, left_echo_pin);
   int actual_right_ping_time = ping(right_trigger_pin, right_echo_pin);
 

-  /* int left_led_value = (int)(actual_left_ping_time / 16.0); */
-  /* int right_led_value = (int)(actual_right_ping_time / 16.0); */
-
-  /* Serial.print("left led value = "); */
-  /* Serial.print(left_led_value); */
-  /* Serial.print(", right led value = "); */
-  /* Serial.print(right_led_value); */
-
-  /* analogWrite(left_led_pin, left_led_value); */
-  /* analogWrite(right_led_pin, right_led_value); */
-
-  /* Serial.print("left ping = "); */
-  /* Serial.print(actual_left_ping_time); */
-  /* Serial.print("\tright_ping = "); */
-  /* Serial.print(actual_right_ping_time); */
-
-  // if the left sonar senses a table, keep driving left side forward,
-  // otherwise, stop left side
-  if(actual_left_ping_time < left_max_ping_time_over_table){
-    left_speed = forward_speed;}
-  else{
-    left_speed = stop_speed;}
-  // if the right sonar senses a table, keep driving right side forward,
-  // otherwise, stop right side
-  if(actual_right_ping_time < right_max_ping_time_over_table){
-    right_speed = forward_speed;}
-  else{
-    right_speed = stop_speed;}
-
-  // if both sonars detect being off the table, backup and turn around
-  // otherwise, go the correct speed for each wheel
-  if(actual_left_ping_time >= left_max_ping_time_over_table
-     && actual_right_ping_time >= right_max_ping_time_over_table){
-    /* Serial.print("backing up"); */
-    backup(backup_time);
-    /* Serial.print("turning around"); */
-    turn_around(turn_around_time);}
-  else{
-    /* Serial.print("going"); */
-    go(left_speed, right_speed);}}
+  switch(stay_on_table_state) {
+  case Stay_on_table_state::going: going(); break;
+  case Stay_on_table_state::start_backing:
+    Serial.print("start backing ");
+    start_backing_time = millis();
+    stay_on_table_state = Stay_on_table_state::backing;
+  case Stay_on_table_state::backing:
+    backing(start_backing_time);
+    break;
+  case Stay_on_table_state::start_turning:
+    Serial.print("start turning ");
+    start_turning_time = millis();
+    stay_on_table_state = Stay_on_table_state::turning;
+  case Stay_on_table_state::turning:
+    turning(start_turning_time);
+    break; } }

 
 void follow() {
 
 void follow() {

-  Serial.print("following!");
-  delay(100); }
+  int left_speed;
+  int right_speed;
+
+  // you'll need to adjust these based on your sonar sensor's behavior
+  int desired_right_ping_time = 800;
+  int desired_left_ping_time = 800;
+
+  unsigned int actual_left_ping_time = ping(left_trigger_pin, left_echo_pin);
+  unsigned int actual_right_ping_time = ping(right_trigger_pin, right_echo_pin);
+
+  left_speed = actual_left_ping_time - desired_left_ping_time;
+  right_speed = actual_right_ping_time - desired_right_ping_time;
+
+  Serial.print(", left: ping = ");
+  Serial.print(actual_left_ping_time);
+  Serial.print(" speed = ");
+  Serial.print(left_speed);
+  Serial.print(" right: ping = ");
+  Serial.print(actual_right_ping_time);
+  Serial.print(" speed = ");
+  Serial.print(right_speed);
+
+  go(left_speed, right_speed); }

 
 enum class Behavior {stay_on_table, follow};
 Behavior behavior = Behavior::stay_on_table;
 
 enum class Behavior {stay_on_table, follow};
 Behavior behavior = Behavior::stay_on_table;


@@ -215,3 +256,7 @@ void loop() {
   prior_button_state = button_state;
 
   Serial.println(); }
   prior_button_state = button_state;
 
   Serial.println(); }

+
+void loop_asdf() {
+  go(250, 250);
+}