challenge-bot.com Git - challenge-bot/blob - build-stages/i_chooser/i_chooser.ino

   1
   2 /*
   3  This program is free software: you can redistribute it and/or modify
   4     it under the terms of the GNU Affero General Public License as
   5     published by the Free Software Foundation, either version 3 of the
   6     License, or (at your option) any later version.
   7
   8     This program is distributed in the hope that it will be useful,
   9     but WITHOUT ANY WARRANTY; without even the implied warranty of
  10     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  11     GNU Affero General Public License for more details.
  12
  13     You should have received a copy of the GNU Affero General Public License
  14     along with this program.  If not, see <http://www.gnu.org/licenses/>.
  15  */
  16
  17 #include "bot_with_leds_map.h"
  18
  19 void on(int pin){
  20   digitalWrite(pin, HIGH);}
  21
  22 void off(int pin){
  23   digitalWrite(pin, LOW);}
  24
  25 void set_motor(int speed_pin,
  26                int forward_pin,
  27                int backward_pin,
  28                int speed){
  29   if(speed > 0){
  30     off(backward_pin);
  31     on(forward_pin);}
  32   else if(speed < 0){
  33     off(forward_pin);
  34     on(backward_pin);
  35     speed = -speed;}
  36   else{ // speed is 0
  37     off(forward_pin);
  38     off(backward_pin);}
  39   // since speed has been set positive, no need to check if speed < -255.
  40   if(speed > 255){
  41     speed = 255;}
  42   analogWrite(speed_pin, speed);}
  43
  44 void go(int left_motor_speed, int right_motor_speed){
  45   set_motor(left_motor_speed_pin,
  46             left_motor_forward_pin,
  47             left_motor_backward_pin,
  48             left_motor_speed);
  49   set_motor(right_motor_speed_pin,
  50             right_motor_forward_pin,
  51             right_motor_backward_pin,
  52             right_motor_speed);}
  53
  54 int ping(int trigger, int echo){
  55   int ping_time = 0;
  56   // turn off trigger
  57   off(trigger);
  58   delayMicroseconds(2);
  59   // turn on the trigger and leave it on long enough for the
  60   // sonar sensor to notice
  61   on(trigger);
  62   delayMicroseconds(10);
  63   off(trigger);
  64   ping_time = pulseIn(echo, HIGH);
  65   if(ping_time <= 0){
  66     ping_time = 3000;}
  67   // sonar needs some time to recover before pinging again,
  68   // so make sure it gets enough sleep right here.  50 milliseconds
  69   delay(50);
  70   return ping_time;}
  71
  72 void setup(){
  73   Serial.begin(9600);
  74
  75   pinMode(LED_BUILTIN, OUTPUT);
  76
  77   pinMode(button_pin, INPUT_PULLUP);
  78
  79   pinMode(right_motor_speed_pin, OUTPUT);
  80   pinMode(right_motor_forward_pin, OUTPUT);
  81   pinMode(right_motor_backward_pin, OUTPUT);
  82
  83   pinMode(right_echo_pin, INPUT);
  84   pinMode(right_trigger_pin, OUTPUT);
  85
  86   pinMode(left_motor_speed_pin, OUTPUT);
  87   pinMode(left_motor_forward_pin, OUTPUT);
  88   pinMode(left_motor_backward_pin, OUTPUT);
  89
  90   pinMode(left_echo_pin, INPUT);
  91   pinMode(left_trigger_pin, OUTPUT);
  92
  93   off(left_motor_speed_pin);
  94   off(left_motor_forward_pin);
  95   off(left_motor_backward_pin);
  96   off(left_trigger_pin);
  97
  98   off(right_motor_speed_pin);
  99   off(right_motor_forward_pin);
 100   off(right_motor_backward_pin);
 101   off(right_trigger_pin);}
 102
 103 enum class Stay_on_table_state {
 104   going, start_backing, backing, start_turning, turning};
 105 static Stay_on_table_state stay_on_table_state =
 106   Stay_on_table_state::going;
 107
 108 void going() {
 109   Serial.print("going ");
 110   int forward_speed = 250;
 111   int stop_speed = 0;
 112
 113   int left_speed;
 114   int right_speed;
 115
 116   // adjust this number as necessary for your robot.
 117   // it represents how far the table is from your sonar sensor.
 118   // larger values mean larger distance.  default is 800
 119   const int right_max_ping_time_over_table = 800;
 120   const int left_max_ping_time_over_table = 800;
 121
 122   const int left_ping_time =
 123     ping(left_trigger_pin, left_echo_pin);
 124   const int right_ping_time =
 125     ping(right_trigger_pin, right_echo_pin);
 126
 127   if (left_ping_time <= left_max_ping_time_over_table
 128       || right_ping_time <= right_max_ping_time_over_table) {
 129     if(left_ping_time <= left_max_ping_time_over_table) {
 130       left_speed = forward_speed; }
 131     else {
 132       left_speed = stop_speed; }
 133     if(right_ping_time <= right_max_ping_time_over_table) {
 134       right_speed = forward_speed; }
 135     else {
 136       right_speed = stop_speed; } }
 137   else { // both ping times were above max acceptable ping time
 138     left_speed = right_speed = 0;
 139     stay_on_table_state = Stay_on_table_state::start_backing; }
 140
 141   go(left_speed, right_speed); }
 142
 143 void backing(unsigned long start_backing) {
 144   Serial.print("backing ");
 145   static const unsigned int allowed_backup_duration = 500;
 146   unsigned long now = millis();
 147   unsigned long backup_duration = now - start_backing;
 148
 149   go(-250, -250);
 150
 151   if(backup_duration > allowed_backup_duration) {
 152     stay_on_table_state = Stay_on_table_state::start_turning; } }
 153
 154 void turning(unsigned long start_turning_time) {
 155   Serial.print("turning ");
 156   // the exact amount of time for turning around might need
 157   // twerking for your robot.  the default value is 3200
 158   static const unsigned int allowed_turning_duration = 2500;
 159   unsigned long now = millis();
 160   unsigned long turning_duration = now - start_turning_time;
 161
 162   go(-250, 250);
 163
 164   if(turning_duration > allowed_turning_duration) {
 165     stay_on_table_state = Stay_on_table_state::going; } }
 166
 167 void stay_on_table(){
 168   Serial.print("stay on table ");
 169   static unsigned long start_backing_time = 0;
 170   static unsigned long start_turning_time = 0;
 171
 172
 173   int actual_left_ping_time = ping(left_trigger_pin, left_echo_pin);
 174   int actual_right_ping_time = ping(right_trigger_pin, right_echo_pin);
 175
 176   switch(stay_on_table_state) {
 177   case Stay_on_table_state::going: going(); break;
 178   case Stay_on_table_state::start_backing:
 179     Serial.print("start backing ");
 180     start_backing_time = millis();
 181     stay_on_table_state = Stay_on_table_state::backing;
 182   case Stay_on_table_state::backing:
 183     backing(start_backing_time);
 184     break;
 185   case Stay_on_table_state::start_turning:
 186     Serial.print("start turning ");
 187     start_turning_time = millis();
 188     stay_on_table_state = Stay_on_table_state::turning;
 189   case Stay_on_table_state::turning:
 190     turning(start_turning_time);
 191     break; } }
 192
 193 void follow() {
 194   int left_speed;
 195   int right_speed;
 196
 197   // you'll need to adjust these based on your sonar sensor's behavior
 198   int desired_right_ping_time = 800;
 199   int desired_left_ping_time = 800;
 200
 201   unsigned int left_ping_time = ping(left_trigger_pin, left_echo_pin);
 202   unsigned int right_ping_time = ping(right_trigger_pin, right_echo_pin);
 203
 204   left_speed = left_ping_time - desired_left_ping_time;
 205   right_speed = right_ping_time - desired_right_ping_time;
 206
 207   Serial.print(", left: ping = ");
 208   Serial.print(left_ping_time);
 209   Serial.print(" speed = ");
 210   Serial.print(left_speed);
 211   Serial.print(" right: ping = ");
 212   Serial.print(right_ping_time);
 213   Serial.print(" speed = ");
 214   Serial.print(right_speed);
 215
 216   go(left_speed, right_speed); }
 217
 218 enum class Behavior {stay_on_table, follow};
 219 Behavior behavior = Behavior::stay_on_table;
 220
 221 enum class Button_state {up, down};
 222
 223 Button_state prior_button_state = Button_state::up;
 224
 225 void loop() {
 226   static int count = 0;
 227   Serial.print(count);
 228   Serial.print(" ");
 229   count++;
 230
 231   Button_state button_state =
 232     (digitalRead(button_pin) == HIGH)
 233       ? Button_state::up
 234       : Button_state::down;
 235   switch(button_state) {
 236   case Button_state::up: Serial.print("up "); break;
 237   case Button_state::down: Serial.print("down "); break; }
 238
 239   // if button was just pressed
 240   if (prior_button_state == Button_state::up
 241       &&    button_state == Button_state::down) {
 242     // indicate button press recognized
 243     on(LED_BUILTIN);
 244     // turn off motors, to allow robot to be set down
 245     go(0, 0);
 246     delay(1000);
 247     switch(behavior) {
 248     case Behavior::stay_on_table: behavior = Behavior::follow; break;
 249     case Behavior::follow: behavior = Behavior::stay_on_table; break; }
 250     off(LED_BUILTIN); }
 251
 252   switch(behavior) {
 253   case Behavior::stay_on_table: stay_on_table(); break;
 254   case Behavior::       follow:        follow(); break; }
 255
 256   prior_button_state = button_state;
 257
 258   Serial.println(); }