+ 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) {
+ left_speed = forward_speed; }
+ else {
+ left_speed = stop_speed; }
+ if(right_ping_time <= right_max_ping_time_over_table) {
+ right_speed = forward_speed; }
+ else {
+ right_speed = stop_speed; } }
+ else { // both ping times were above max acceptable ping time
+ left_speed = right_speed = 0;
+ stay_on_table_state = Stay_on_table_state::start_backing; }
+
+ go(left_speed, right_speed); }
+
+void backing(unsigned long start_backing) {
+ Serial.print("backing ");
+ static const unsigned int allowed_backup_duration = 500;
+ unsigned long now = millis();
+ unsigned long backup_duration = now - start_backing;
+
+ go(-250, -250);
+
+ if(backup_duration > allowed_backup_duration) {
+ stay_on_table_state = Stay_on_table_state::start_turning; } }
+
+void turning(unsigned long start_turning_time) {
+ Serial.print("turning ");