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=6eabcaf90f157a2ce79a7bceef243165b27ee5cb;hb=f9290e379cc65d97e76bc5650066bdbe27ee4883;hpb=dad0fdb28c89f7db2872178655f30f281c6f9d13

diff --git a/build-stages/i_chooser/i_chooser.ino b/build-stages/i_chooser/i_chooser.ino
index 6eabcaf..d43591f 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,
@@ -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);
@@ -101,11 +104,12 @@ void setup(){
   off(right_trigger_pin);}
 
 enum class Stay_on_table_state {
-  going, start_backing, backing, start_turning, turning};
+  going, start_backing, backing, start_turning, turning };
 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 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;
+  const unsigned int desired_right_ping_time = 800;
+  const unsigned int desired_left_ping_time = 800;
 
-  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);
+  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,13 +245,13 @@ 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;
 
   Serial.println(); }
-
-void loop_asdf() {
-  go(250, 250);
-}