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_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 backup(int backup_time){
  73   go(-250, -250);
  74   delay(backup_time);}
  75
  76 void turn_around(int turn_around_time){
  77   go(-250, 250);
  78   delay(turn_around_time);}
  79
  80 void setup(){
  81   Serial.begin(9600);
  82
  83   pinMode(LED_BUILTIN, OUTPUT);
  84
  85   pinMode(button_pin, INPUT_PULLUP);
  86
  87   pinMode(right_motor_speed_pin, OUTPUT);
  88   pinMode(right_motor_forward_pin, OUTPUT);
  89   pinMode(right_motor_backward_pin, OUTPUT);
  90
  91   pinMode(right_echo_pin, INPUT);
  92   pinMode(right_trigger_pin, OUTPUT);
  93
  94   pinMode(left_motor_speed_pin, OUTPUT);
  95   pinMode(left_motor_forward_pin, OUTPUT);
  96   pinMode(left_motor_backward_pin, OUTPUT);
  97
  98   pinMode(left_echo_pin, INPUT);
  99   pinMode(left_trigger_pin, OUTPUT);
 100
 101   off(left_motor_speed_pin);
 102   off(left_motor_forward_pin);
 103   off(left_motor_backward_pin);
 104   off(left_trigger_pin);
 105
 106   off(right_motor_speed_pin);
 107   off(right_motor_forward_pin);
 108   off(right_motor_backward_pin);
 109   off(right_trigger_pin);}
 110
 111 void stay_on_table(){
 112   Serial.print("stay on table");
 113   int left_speed;
 114   int right_speed;
 115
 116   int forward_speed = 250;
 117   int stop_speed = 0;
 118
 119   // adjust this number as necessary for your robot.
 120   // it represents how far the table is from your sonar sensor.
 121   // larger values mean larger distance.  default is 800
 122   int right_max_ping_time_over_table = 800;
 123   int left_max_ping_time_over_table = 800;
 124   int backup_time = 500;
 125   // the exact amount of time for turning around might need
 126   // twerking for your robot.  the default value is 3200
 127   int turn_around_time = 2500;
 128
 129   int actual_left_ping_time = ping(left_trigger_pin, left_echo_pin);
 130   int actual_right_ping_time = ping(right_trigger_pin, right_echo_pin);
 131
 132   /* int left_led_value = (int)(actual_left_ping_time / 16.0); */
 133   /* int right_led_value = (int)(actual_right_ping_time / 16.0); */
 134
 135   /* Serial.print("left led value = "); */
 136   /* Serial.print(left_led_value); */
 137   /* Serial.print(", right led value = "); */
 138   /* Serial.print(right_led_value); */
 139
 140   /* analogWrite(left_led_pin, left_led_value); */
 141   /* analogWrite(right_led_pin, right_led_value); */
 142
 143   /* Serial.print("left ping = "); */
 144   /* Serial.print(actual_left_ping_time); */
 145   /* Serial.print("\tright_ping = "); */
 146   /* Serial.print(actual_right_ping_time); */
 147
 148   // if the left sonar senses a table, keep driving left side forward,
 149   // otherwise, stop left side
 150   if(actual_left_ping_time < left_max_ping_time_over_table){
 151     left_speed = forward_speed;}
 152   else{
 153     left_speed = stop_speed;}
 154   // if the right sonar senses a table, keep driving right side forward,
 155   // otherwise, stop right side
 156   if(actual_right_ping_time < right_max_ping_time_over_table){
 157     right_speed = forward_speed;}
 158   else{
 159     right_speed = stop_speed;}
 160
 161   // if both sonars detect being off the table, backup and turn around
 162   // otherwise, go the correct speed for each wheel
 163   if(actual_left_ping_time >= left_max_ping_time_over_table
 164      && actual_right_ping_time >= right_max_ping_time_over_table){
 165     /* Serial.print("backing up"); */
 166     backup(backup_time);
 167     /* Serial.print("turning around"); */
 168     turn_around(turn_around_time);}
 169   else{
 170     /* Serial.print("going"); */
 171     go(left_speed, right_speed);}}
 172
 173 void follow() {
 174   Serial.print("following!");
 175   delay(100); }
 176
 177 enum class Behavior {stay_on_table, follow};
 178 Behavior behavior = Behavior::stay_on_table;
 179
 180 enum class Button_state {up, down};
 181
 182 Button_state prior_button_state = Button_state::up;
 183
 184 void loop() {
 185   static int count = 0;
 186   Serial.print(count);
 187   Serial.print(" ");
 188   count++;
 189
 190   Button_state button_state =
 191     (digitalRead(button_pin) == HIGH)
 192       ? Button_state::up
 193       : Button_state::down;
 194   switch(button_state) {
 195   case Button_state::up: Serial.print("up "); break;
 196   case Button_state::down: Serial.print("down "); break; }
 197
 198   // if button was just pressed
 199   if (prior_button_state == Button_state::up
 200       &&    button_state == Button_state::down) {
 201     // indicate button press recognized
 202     on(LED_BUILTIN);
 203     // turn off motors, to allow robot to be set down
 204     go(0, 0);
 205     delay(1000);
 206     switch(behavior) {
 207     case Behavior::stay_on_table: behavior = Behavior::follow; break;
 208     case Behavior::follow: behavior = Behavior::stay_on_table; break; }
 209     off(LED_BUILTIN); }
 210
 211   switch(behavior) {
 212   case Behavior::stay_on_table: stay_on_table(); break;
 213   case Behavior::       follow:        follow(); break; }
 214
 215   prior_button_state = button_state;
 216
 217   Serial.println(); }