- int turn_around_time = 2500;
-
- 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.print(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.print("backing up"); */
- backup(backup_time);
- /* Serial.print("turning around"); */
- turn_around(turn_around_time);}
- else{
- /* Serial.print("going"); */
- go(left_speed, right_speed);}}
-
-void follow() {
- Serial.print("following!");
- delay(100); }
+ 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(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;
+
+
+ switch(stay_on_table_state) {
+ 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();
+ 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(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
+ const unsigned int desired_right_ping_time = 800;
+ const unsigned int desired_left_ping_time = 800;
+
+ 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); }