make leds light up based on ping time

Change-Id: I94086a06e5467a492ba2aa20b78b9f03bc3d90ab

diff --git a/build-stages/i_chooser/i_chooser.ino b/build-stages/i_chooser/i_chooser.ino
index 492add5046dc287a8c3bc3f3c8e9921fed8d90ba..d43591fa56618d9ad9cdbefd2f98eb03bc5a3de6 100644 (file)
--- 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(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);
   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;
 
 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;
   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 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) {
   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; } }
 
   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;
 
 
   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) {
   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();
   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; } }
 
     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
   // 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); }
 
 
   go(left_speed, right_speed); }
 


@@ -228,6 +214,15 @@ void loop() {
   Serial.print(" ");
   count++;
 
   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
   Button_state button_state =
     (digitalRead(button_pin) == HIGH)
       ? Button_state::up


@@ -250,8 +245,12 @@ void loop() {
     off(LED_BUILTIN); }
 
   switch(behavior) {
     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;
 
 
   prior_button_state = button_state;