From 73ad88453aa7e7f04596f2a3a3f8642cb9cf010d Mon Sep 17 00:00:00 2001 From: ozzloy Date: Sun, 12 May 2019 19:41:58 -0700 Subject: [PATCH] start button chooser to switch between follow and stay on table Change-Id: Ib6184e7bbb3e00ccd82ef8754def1707c30c1f00 --- build-stages/i_chooser/bot_map.h | 63 ++++++ build-stages/i_chooser/bot_with_leds_map.h | 48 +++++ build-stages/i_chooser/i_chooser.ino | 211 +++++++++++++++++++++ 3 files changed, 322 insertions(+) create mode 100644 build-stages/i_chooser/bot_map.h create mode 100644 build-stages/i_chooser/bot_with_leds_map.h create mode 100644 build-stages/i_chooser/i_chooser.ino diff --git a/build-stages/i_chooser/bot_map.h b/build-stages/i_chooser/bot_map.h new file mode 100644 index 0000000..8cf4ebb --- /dev/null +++ b/build-stages/i_chooser/bot_map.h @@ -0,0 +1,63 @@ +// license info at bottom of file +// tl;dr AGPLv3+ + +/* + define which pins are connected to which components +*/ + +////////////////////// +// button +////////////////////// +// purple wire +const int button_pin = 2; + +////////////////////// +// right motor +////////////////////// +// blue wire +const int right_motor_forward_pin = 3; +// orange wire +const int right_motor_backward_pin = 4; +// white wire +const int right_motor_speed_pin = 5; + +////////////////////// +// right sonar sensor +////////////////////// +// yellow wire +const int right_echo_pin = 6; +// blue wire +const int right_trigger_pin = 7; + +////////////////////// +// left motor +////////////////////// +// blue wire +const int left_motor_forward_pin = 8; +// orange wire +const int left_motor_backward_pin = 9; +// white wire +const int left_motor_speed_pin = 10; + +////////////////////// +// left sonar sensor +////////////////////// +// yellow wire +const int left_echo_pin = 11; +// blue wire +const int left_trigger_pin = 12; + +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU Affero General Public License as + published by the Free Software Foundation, either version 3 of the + License, or (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU Affero General Public License for more details. + + You should have received a copy of the GNU Affero General Public License + along with this program. If not, see . + */ diff --git a/build-stages/i_chooser/bot_with_leds_map.h b/build-stages/i_chooser/bot_with_leds_map.h new file mode 100644 index 0000000..ebb554c --- /dev/null +++ b/build-stages/i_chooser/bot_with_leds_map.h @@ -0,0 +1,48 @@ +// license info at bottom of file +// tl;dr AGPLv3+ + +// right side LED, white wire +int right_led_pin = 3; + +// define which pins are connected to which components +// blue wire +int right_motor_forward_pin = 2; +// orange wire +int right_motor_backward_pin = 4; +// white wire +int right_motor_speed_pin = 5; + +// yellow wire +int right_echo_pin = 6; +// blue wire +int right_trigger_pin = 7; + +// blue wire +int left_motor_forward_pin = 8; +// orange wire +int left_motor_backward_pin = 13; +// white wire +int left_motor_speed_pin = 10; + +// yellow wire +int left_echo_pin = 11; +// blue wire +int left_trigger_pin = 12; + +// left side LED, blue wire +int left_led_pin = 9; + +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU Affero General Public License as + published by the Free Software Foundation, either version 3 of the + License, or (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU Affero General Public License for more details. + + You should have received a copy of the GNU Affero General Public License + along with this program. If not, see . + */ diff --git a/build-stages/i_chooser/i_chooser.ino b/build-stages/i_chooser/i_chooser.ino new file mode 100644 index 0000000..7948767 --- /dev/null +++ b/build-stages/i_chooser/i_chooser.ino @@ -0,0 +1,211 @@ + +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU Affero General Public License as + published by the Free Software Foundation, either version 3 of the + License, or (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU Affero General Public License for more details. + + You should have received a copy of the GNU Affero General Public License + along with this program. If not, see . + */ + +#include "bot_map.h" + +void on(int pin){ + digitalWrite(pin, HIGH);} + +void off(int pin){ + digitalWrite(pin, LOW);} + +void set_motor(int speed_pin, + int forward_pin, + int backward_pin, + int speed){ + if(speed > 0){ + off(backward_pin); + on(forward_pin);} + else if(speed < 0){ + off(forward_pin); + on(backward_pin); + speed = -speed;} + else{ // speed is 0 + off(forward_pin); + off(backward_pin);} + // since speed has been set positive, no need to check if speed < -255. + if(speed > 255){ + speed = 255;} + analogWrite(speed_pin, speed);} + +void go(int left_motor_speed, int right_motor_speed){ + set_motor(left_motor_speed_pin, + left_motor_forward_pin, + left_motor_backward_pin, + left_motor_speed); + set_motor(right_motor_speed_pin, + right_motor_forward_pin, + right_motor_backward_pin, + right_motor_speed);} + +int ping(int trigger, int echo){ + int ping_time = 0; + // turn off trigger + off(trigger); + delayMicroseconds(2); + // turn on the trigger and leave it on long enough for the + // sonar sensor to notice + on(trigger); + delayMicroseconds(10); + off(trigger); + ping_time = pulseIn(echo, HIGH); + if(ping_time <= 0){ + ping_time = 3000;} + // sonar needs some time to recover before pinging again, + // so make sure it gets enough sleep right here. 50 milliseconds + delay(50); + return ping_time;} + +void backup(int backup_time){ + go(-250, -250); + delay(backup_time);} + +void turn_around(int turn_around_time){ + go(-250, 250); + delay(turn_around_time);} + +void setup(){ + Serial.begin(9600); + + pinMode(LED_BUILTIN, OUTPUT); + + pinMode(button_pin, INPUT_PULLUP); + + pinMode(right_motor_speed_pin, OUTPUT); + pinMode(right_motor_forward_pin, OUTPUT); + pinMode(right_motor_backward_pin, OUTPUT); + + pinMode(right_echo_pin, INPUT); + pinMode(right_trigger_pin, OUTPUT); + + pinMode(left_motor_speed_pin, OUTPUT); + pinMode(left_motor_forward_pin, OUTPUT); + pinMode(left_motor_backward_pin, OUTPUT); + + pinMode(left_echo_pin, INPUT); + pinMode(left_trigger_pin, OUTPUT); + + off(left_motor_speed_pin); + off(left_motor_forward_pin); + off(left_motor_backward_pin); + off(left_trigger_pin); + + off(right_motor_speed_pin); + off(right_motor_forward_pin); + off(right_motor_backward_pin); + off(right_trigger_pin);} + +void stay_on_table(){ + int left_speed; + int right_speed; + + int forward_speed = 250; + int stop_speed = 0; + + // adjust this number as necessary for your robot. + // it represents how far the table is from your sonar sensor. + // larger values mean larger distance. default is 800 + int right_max_ping_time_over_table = 800; + int left_max_ping_time_over_table = 800; + int backup_time = 500; + // the exact amount of time for turning around might need + // twerking for your robot. the default value is 3200 + 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.println(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.println("backing up"); + backup(backup_time); + Serial.println("turning around"); + turn_around(turn_around_time);} + else{ + Serial.println("going"); + go(left_speed, right_speed);}} + +void follow() { + Serial.println("following!"); + delay(100); } + +void loop() { + digitalWrite(LED_BUILTIN, digitalRead(button_pin)); } + +/* +enum Behavior {STAY_ON_TABLE, FOLLOW}; +Behavior behavior = STAY_ON_TABLE; + +enum Button_state {UP, DOWN}; +Button_state prior_button_state = UP; + +enum Button_state get_button_state() { + return + (digitalRead(button_pin) == LOW) + ? DOWN + : UP; } +void loop_asdf() { + static Button_state button_state = getButtonState(); + + // if button was just pressed + if (prior_button_state == UP && button_state == DOWN) { + // indicate button press recognized + on(LED_BUILTIN); + // turn off motors, to allow robot to be set down + go(0, 0); + delay(1000); + switch(behavior) { + case STAY_ON_TABLE: behavior = FOLLOW; break; + case FOLLOW: behavior = STAY_ON_TABLE; break; } + off(LED_BUILTIN); } + + switch(behavior) { + case STAY_ON_TABLE: stay_on_table(); break; + case FOLLOW: follow(); break; } + + prior_button_state = button_state; } +*/ -- 2.30.2