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(const int left_motor_speed, const 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(left_led_pin, OUTPUT);
  80   pinMode(right_led_pin, OUTPUT);
  81
  82   pinMode(right_motor_speed_pin, OUTPUT);
  83   pinMode(right_motor_forward_pin, OUTPUT);
  84   pinMode(right_motor_backward_pin, OUTPUT);
  85
  86   pinMode(right_echo_pin, INPUT);
  87   pinMode(right_trigger_pin, OUTPUT);
  88
  89   pinMode(left_motor_speed_pin, OUTPUT);
  90   pinMode(left_motor_forward_pin, OUTPUT);
  91   pinMode(left_motor_backward_pin, OUTPUT);
  92
  93   pinMode(left_echo_pin, INPUT);
  94   pinMode(left_trigger_pin, OUTPUT);
  95
  96   off(left_motor_speed_pin);
  97   off(left_motor_forward_pin);
  98   off(left_motor_backward_pin);
  99   off(left_trigger_pin);
 100
 101   off(right_motor_speed_pin);
 102   off(right_motor_forward_pin);
 103   off(right_motor_backward_pin);
 104   off(right_trigger_pin);}
 105
 106 enum class Stay_on_table_state {
 107   going, start_backing, backing, start_turning, turning };
 108 static Stay_on_table_state stay_on_table_state =
 109   Stay_on_table_state::going;
 110
 111 void going(const unsigned int left_ping_time,
 112            const unsigned int right_ping_time) {
 113   Serial.print("going ");
 114   int forward_speed = 250;
 115   int stop_speed = 0;
 116
 117   int left_speed;
 118   int right_speed;
 119
 120   // adjust this number as necessary for your robot.
 121   // it represents how far the table is from your sonar sensor.
 122   // larger values mean larger distance.  default is 800
 123   const int right_max_ping_time_over_table = 800;
 124   const int left_max_ping_time_over_table = 800;
 125
 126   if (left_ping_time <= left_max_ping_time_over_table
 127       || right_ping_time <= right_max_ping_time_over_table) {
 128     if(left_ping_time <= left_max_ping_time_over_table) {
 129       left_speed = forward_speed; }
 130     else {
 131       left_speed = stop_speed; }
 132     if(right_ping_time <= right_max_ping_time_over_table) {
 133       right_speed = forward_speed; }
 134     else {
 135       right_speed = stop_speed; } }
 136   else { // both ping times were above max acceptable ping time
 137     left_speed = right_speed = 0;
 138     stay_on_table_state = Stay_on_table_state::start_backing; }
 139
 140   go(left_speed, right_speed); }
 141
 142 void backing(unsigned long start_backing) {
 143   Serial.print("backing ");
 144   static const unsigned int allowed_backup_duration = 500;
 145   unsigned long now = millis();
 146   unsigned long backup_duration = now - start_backing;
 147
 148   go(-250, -250);
 149
 150   if(backup_duration > allowed_backup_duration) {
 151     stay_on_table_state = Stay_on_table_state::start_turning; } }
 152
 153 void turning(unsigned long start_turning_time) {
 154   Serial.print("turning ");
 155   // the exact amount of time for turning around might need
 156   // twerking for your robot.  the default value is 3200
 157   static const unsigned int allowed_turning_duration = 2500;
 158   unsigned long now = millis();
 159   unsigned long turning_duration = now - start_turning_time;
 160
 161   go(-250, 250);
 162
 163   if(turning_duration > allowed_turning_duration) {
 164     stay_on_table_state = Stay_on_table_state::going; } }
 165
 166 void stay_on_table(const unsigned int left_ping_time,
 167                    const unsigned int right_ping_time){
 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   switch(stay_on_table_state) {
 174   case Stay_on_table_state::going:
 175     going(left_ping_time, right_ping_time);
 176     break;
 177   case Stay_on_table_state::start_backing:
 178     Serial.print("start backing ");
 179     start_backing_time = millis();
 180     stay_on_table_state = Stay_on_table_state::backing;
 181   case Stay_on_table_state::backing:
 182     backing(start_backing_time);
 183     break;
 184   case Stay_on_table_state::start_turning:
 185     Serial.print("start turning ");
 186     start_turning_time = millis();
 187     stay_on_table_state = Stay_on_table_state::turning;
 188   case Stay_on_table_state::turning:
 189     turning(start_turning_time);
 190     break; } }
 191
 192 void follow(const unsigned int left_ping_time,
 193             const unsigned int right_ping_time) {
 194   // you'll need to adjust these based on your sonar sensor's behavior
 195   const unsigned int desired_right_ping_time = 800;
 196   const unsigned int desired_left_ping_time = 800;
 197
 198   const int
 199     left_speed = left_ping_time - desired_left_ping_time,
 200     right_speed = right_ping_time - desired_right_ping_time;
 201
 202   go(left_speed, right_speed); }
 203
 204 enum class Behavior {stay_on_table, follow};
 205 Behavior behavior = Behavior::stay_on_table;
 206
 207 enum class Button_state {up, down};
 208
 209 Button_state prior_button_state = Button_state::up;
 210
 211 void loop() {
 212   static int count = 0;
 213   Serial.print(count);
 214   Serial.print(" ");
 215   count++;
 216
 217   const unsigned int
 218     left_ping_time = ping(left_trigger_pin, left_echo_pin),
 219     right_ping_time = ping(right_trigger_pin, right_echo_pin);
 220   const unsigned int
 221     left_led_value = map(left_ping_time, 0, 3000, 0, 255),
 222     right_led_value = map(right_ping_time, 0, 3000, 0, 255);
 223   analogWrite(left_led_pin, left_led_value);
 224   analogWrite(right_led_pin, right_led_value);
 225
 226   Button_state button_state =
 227     (digitalRead(button_pin) == HIGH)
 228       ? Button_state::up
 229       : Button_state::down;
 230   switch(button_state) {
 231   case Button_state::up: Serial.print("up "); break;
 232   case Button_state::down: Serial.print("down "); break; }
 233
 234   // if button was just pressed
 235   if (prior_button_state == Button_state::up
 236       &&    button_state == Button_state::down) {
 237     // indicate button press recognized
 238     on(LED_BUILTIN);
 239     // turn off motors, to allow robot to be set down
 240     go(0, 0);
 241     delay(1000);
 242     switch(behavior) {
 243     case Behavior::stay_on_table: behavior = Behavior::follow; break;
 244     case Behavior::follow: behavior = Behavior::stay_on_table; break; }
 245     off(LED_BUILTIN); }
 246
 247   switch(behavior) {
 248   case Behavior::stay_on_table:
 249     stay_on_table(left_ping_time, right_ping_time);
 250     break;
 251   case Behavior::       follow:
 252     follow(left_ping_time, right_ping_time);
 253     break; }
 254
 255   prior_button_state = button_state;
 256
 257   Serial.println(); }