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

diff --git a/build-stages/i_chooser/i_chooser.ino b/build-stages/i_chooser/i_chooser.ino
index 492add5..d43591f 100644
--- a/build-stages/i_chooser/i_chooser.ino
+++ b/build-stages/i_chooser/i_chooser.ino
@@ -76,6 +76,9 @@ void setup(){
 
   pinMode(button_pin, INPUT_PULLUP);
 
+  pinMode(left_led_pin, OUTPUT);
+  pinMode(right_led_pin, OUTPUT);
+
   pinMode(right_motor_speed_pin, OUTPUT);
   pinMode(right_motor_forward_pin, OUTPUT);
   pinMode(right_motor_backward_pin, OUTPUT);
@@ -105,7 +108,8 @@ enum class Stay_on_table_state {
 static Stay_on_table_state stay_on_table_state =
   Stay_on_table_state::going;
 
-void going() {
+void going(const unsigned int left_ping_time,
+           const unsigned int right_ping_time) {
   Serial.print("going ");
   int forward_speed = 250;
   int stop_speed = 0;
@@ -119,11 +123,6 @@ void going() {
   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) {
@@ -164,17 +163,17 @@ void turning(unsigned long start_turning_time) {
   if(turning_duration > allowed_turning_duration) {
     stay_on_table_state = Stay_on_table_state::going; } }
 
-void stay_on_table(){
+void stay_on_table(const unsigned int left_ping_time,
+                   const unsigned int right_ping_time){
   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);
-
   switch(stay_on_table_state) {
-  case Stay_on_table_state::going: going(); break;
+  case Stay_on_table_state::going:
+    going(left_ping_time, right_ping_time);
+    break;
   case Stay_on_table_state::start_backing:
     Serial.print("start backing ");
     start_backing_time = millis();
@@ -190,28 +189,15 @@ void stay_on_table(){
     turning(start_turning_time);
     break; } }
 
-void follow() {
-  int left_speed;
-  int right_speed;
-
+void follow(const unsigned int left_ping_time,
+            const unsigned int right_ping_time) {
   // 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 left_ping_time = ping(left_trigger_pin, left_echo_pin);
-  unsigned int right_ping_time = ping(right_trigger_pin, right_echo_pin);
-
-  left_speed = left_ping_time - desired_left_ping_time;
-  right_speed = right_ping_time - desired_right_ping_time;
+  const unsigned int desired_right_ping_time = 800;
+  const unsigned int desired_left_ping_time = 800;
 
-  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);
+  const int
+    left_speed = left_ping_time - desired_left_ping_time,
+    right_speed = right_ping_time - desired_right_ping_time;
 
   go(left_speed, right_speed); }
 
@@ -228,6 +214,15 @@ void loop() {
   Serial.print(" ");
   count++;
 
+  const unsigned int
+    left_ping_time = ping(left_trigger_pin, left_echo_pin),
+    right_ping_time = ping(right_trigger_pin, right_echo_pin);
+  const unsigned int
+    left_led_value = map(left_ping_time, 0, 3000, 0, 255),
+    right_led_value = map(right_ping_time, 0, 3000, 0, 255);
+  analogWrite(left_led_pin, left_led_value);
+  analogWrite(right_led_pin, right_led_value);
+
   Button_state button_state =
     (digitalRead(button_pin) == HIGH)
       ? Button_state::up
@@ -250,8 +245,12 @@ void loop() {
     off(LED_BUILTIN); }
 
   switch(behavior) {
-  case Behavior::stay_on_table: stay_on_table(); break;
-  case Behavior::       follow:        follow(); break; }
+  case Behavior::stay_on_table:
+    stay_on_table(left_ping_time, right_ping_time);
+    break;
+  case Behavior::       follow:
+    follow(left_ping_time, right_ping_time);
+    break; }
 
   prior_button_state = button_state;