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,
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);
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;
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(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();
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); }
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
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);
-}