X-Git-Url: http://challenge-bot.com/repos/?p=challenge-bot;a=blobdiff_plain;f=build-stages%2Fi_chooser%2Fi_chooser.ino;h=492add5046dc287a8c3bc3f3c8e9921fed8d90ba;hp=7948767d93139aeb66e27dcc36d2693092cb803b;hb=ca385942f7edbd56c3a74bde9436b006c366504c;hpb=73ad88453aa7e7f04596f2a3a3f8642cb9cf010d

diff --git a/build-stages/i_chooser/i_chooser.ino b/build-stages/i_chooser/i_chooser.ino
index 7948767..492add5 100644
--- a/build-stages/i_chooser/i_chooser.ino
+++ b/build-stages/i_chooser/i_chooser.ino
@@ -14,7 +14,7 @@
     along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-#include "bot_map.h"
+#include "bot_with_leds_map.h"
 
 void on(int pin){
   digitalWrite(pin, HIGH);}
@@ -41,7 +41,7 @@ void set_motor(int speed_pin,
     speed = 255;}
   analogWrite(speed_pin, speed);}
 
-void go(int left_motor_speed, int right_motor_speed){
+void go(const int left_motor_speed, const int right_motor_speed){
   set_motor(left_motor_speed_pin,
             left_motor_forward_pin,
             left_motor_backward_pin,
@@ -69,14 +69,6 @@ int ping(int trigger, int echo){
   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);
 
@@ -108,104 +100,159 @@ void setup(){
   off(right_motor_backward_pin);
   off(right_trigger_pin);}
 
-void 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 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
-  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
-  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 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.println(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.println("backing up");
-    backup(backup_time);
-    Serial.println("turning around");
-    turn_around(turn_around_time);}
-  else{
-    Serial.println("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() {
-  Serial.println("following!");
-  delay(100); }
+  int left_speed;
+  int right_speed;
 
-void loop() {
-  digitalWrite(LED_BUILTIN, digitalRead(button_pin)); }
+  // 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;
 
-/*
-enum Behavior {STAY_ON_TABLE, FOLLOW};
-Behavior behavior = STAY_ON_TABLE;
+  unsigned int left_ping_time = ping(left_trigger_pin, left_echo_pin);
+  unsigned int right_ping_time = ping(right_trigger_pin, right_echo_pin);
 
-enum Button_state {UP, DOWN};
-Button_state prior_button_state = UP;
+  left_speed = left_ping_time - desired_left_ping_time;
+  right_speed = right_ping_time - desired_right_ping_time;
 
-enum Button_state get_button_state() {
-  return
-    (digitalRead(button_pin) == LOW)
-    ? DOWN
-    : UP; }
-void loop_asdf() {
-  static Button_state button_state = getButtonState();
+  Serial.print(", left: ping = ");
+  Serial.print(left_ping_time);
+  Serial.print(" speed = ");
+  Serial.print(left_speed);
+  Serial.print(" right: ping = ");
+  Serial.print(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 Button_state {up, down};
+
+Button_state prior_button_state = Button_state::up;
+
+void loop() {
+  static int count = 0;
+  Serial.print(count);
+  Serial.print(" ");
+  count++;
+
+  Button_state button_state =
+    (digitalRead(button_pin) == HIGH)
+      ? Button_state::up
+      : Button_state::down;
+  switch(button_state) {
+  case Button_state::up: Serial.print("up "); break;
+  case Button_state::down: Serial.print("down "); break; }
 
   // if button was just pressed
-  if (prior_button_state == UP && button_state == DOWN) {
+  if (prior_button_state == Button_state::up
+      &&    button_state == Button_state::down) {
     // indicate button press recognized
     on(LED_BUILTIN);
     // turn off motors, to allow robot to be set down
     go(0, 0);
     delay(1000);
     switch(behavior) {
-    case STAY_ON_TABLE: behavior =        FOLLOW; break;
-    case        FOLLOW: behavior = STAY_ON_TABLE; break; }
+    case Behavior::stay_on_table: behavior = Behavior::follow; break;
+    case Behavior::follow: behavior = Behavior::stay_on_table; break; }
     off(LED_BUILTIN); }
 
   switch(behavior) {
-  case STAY_ON_TABLE: stay_on_table(); break;
-  case        FOLLOW:        follow(); break; }
+  case Behavior::stay_on_table: stay_on_table(); break;
+  case Behavior::       follow:        follow(); break; }
+
+  prior_button_state = button_state;
 
-  prior_button_state = button_state; }
-*/
+  Serial.println(); }