From 3cb3ea0ba5ea81fb679adac1c9acffe53cedeeb5 Mon Sep 17 00:00:00 2001
From: ozzloy <ozzloy+launchpad_net@gmail.com>
Date: Sun, 12 May 2019 20:29:27 -0700
Subject: [PATCH] switch between follow and stay on table with button press

Change-Id: Ic9fe97fd7a4e7d5a25cafe6e69b6d1d9b6c8f36f
---
 build-stages/i_chooser/i_chooser.ino | 64 +++++++++++++++-------------
 1 file changed, 35 insertions(+), 29 deletions(-)

diff --git a/build-stages/i_chooser/i_chooser.ino b/build-stages/i_chooser/i_chooser.ino
index 7948767..b90574a 100644
--- a/build-stages/i_chooser/i_chooser.ino
+++ b/build-stages/i_chooser/i_chooser.ino
@@ -109,6 +109,7 @@ void setup(){
   off(right_trigger_pin);}
 
 void stay_on_table(){
+  Serial.print("stay on table");
   int left_speed;
   int right_speed;
 
@@ -139,10 +140,10 @@ void stay_on_table(){
   /* 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);
+  /* 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
@@ -161,51 +162,56 @@ void stay_on_table(){
   // 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");
+    /* Serial.print("backing up"); */
     backup(backup_time);
-    Serial.println("turning around");
+    /* Serial.print("turning around"); */
     turn_around(turn_around_time);}
   else{
-    Serial.println("going");
+    /* Serial.print("going"); */
     go(left_speed, right_speed);}}
 
 void follow() {
-  Serial.println("following!");
+  Serial.print("following!");
   delay(100); }
 
-void loop() {
-  digitalWrite(LED_BUILTIN, digitalRead(button_pin)); }
+enum class Behavior {stay_on_table, follow};
+Behavior behavior = Behavior::stay_on_table;
 
-/*
-enum Behavior {STAY_ON_TABLE, FOLLOW};
-Behavior behavior = STAY_ON_TABLE;
+enum class Button_state {up, down};
 
-enum Button_state {UP, DOWN};
-Button_state prior_button_state = UP;
+Button_state prior_button_state = Button_state::up;
 
-enum Button_state get_button_state() {
-  return
-    (digitalRead(button_pin) == LOW)
-    ? DOWN
-    : UP; }
-void loop_asdf() {
-  static Button_state button_state = getButtonState();
+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(); }
-- 
2.30.2