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   int left_speed;
 113   int right_speed;
 114
 115   int forward_speed = 250;
 116   int stop_speed = 0;
 117
 118   // adjust this number as necessary for your robot.
 119   // it represents how far the table is from your sonar sensor.
 120   // larger values mean larger distance.  default is 800
 121   int right_max_ping_time_over_table = 800;
 122   int left_max_ping_time_over_table = 800;
 123   int backup_time = 500;
 124   // the exact amount of time for turning around might need
 125   // twerking for your robot.  the default value is 3200
 126   int turn_around_time = 2500;
 127
 128   int actual_left_ping_time = ping(left_trigger_pin, left_echo_pin);
 129   int actual_right_ping_time = ping(right_trigger_pin, right_echo_pin);
 130
 131   /* int left_led_value = (int)(actual_left_ping_time / 16.0); */
 132   /* int right_led_value = (int)(actual_right_ping_time / 16.0); */
 133
 134   /* Serial.print("left led value = "); */
 135   /* Serial.print(left_led_value); */
 136   /* Serial.print(", right led value = "); */
 137   /* Serial.print(right_led_value); */
 138
 139   /* analogWrite(left_led_pin, left_led_value); */
 140   /* analogWrite(right_led_pin, right_led_value); */
 141
 142   Serial.print("left ping = ");
 143   Serial.print(actual_left_ping_time);
 144   Serial.print("\tright_ping = ");
 145   Serial.println(actual_right_ping_time);
 146
 147   // if the left sonar senses a table, keep driving left side forward,
 148   // otherwise, stop left side
 149   if(actual_left_ping_time < left_max_ping_time_over_table){
 150     left_speed = forward_speed;}
 151   else{
 152     left_speed = stop_speed;}
 153   // if the right sonar senses a table, keep driving right side forward,
 154   // otherwise, stop right side
 155   if(actual_right_ping_time < right_max_ping_time_over_table){
 156     right_speed = forward_speed;}
 157   else{
 158     right_speed = stop_speed;}
 159
 160   // if both sonars detect being off the table, backup and turn around
 161   // otherwise, go the correct speed for each wheel
 162   if(actual_left_ping_time >= left_max_ping_time_over_table
 163      && actual_right_ping_time >= right_max_ping_time_over_table){
 164     Serial.println("backing up");
 165     backup(backup_time);
 166     Serial.println("turning around");
 167     turn_around(turn_around_time);}
 168   else{
 169     Serial.println("going");
 170     go(left_speed, right_speed);}}
 171
 172 void follow() {
 173   Serial.println("following!");
 174   delay(100); }
 175
 176 void loop() {
 177   digitalWrite(LED_BUILTIN, digitalRead(button_pin)); }
 178
 179 /*
 180 enum Behavior {STAY_ON_TABLE, FOLLOW};
 181 Behavior behavior = STAY_ON_TABLE;
 182
 183 enum Button_state {UP, DOWN};
 184 Button_state prior_button_state = UP;
 185
 186 enum Button_state get_button_state() {
 187   return
 188     (digitalRead(button_pin) == LOW)
 189     ? DOWN
 190     : UP; }
 191 void loop_asdf() {
 192   static Button_state button_state = getButtonState();
 193
 194   // if button was just pressed
 195   if (prior_button_state == UP && button_state == DOWN) {
 196     // indicate button press recognized
 197     on(LED_BUILTIN);
 198     // turn off motors, to allow robot to be set down
 199     go(0, 0);
 200     delay(1000);
 201     switch(behavior) {
 202     case STAY_ON_TABLE: behavior =        FOLLOW; break;
 203     case        FOLLOW: behavior = STAY_ON_TABLE; break; }
 204     off(LED_BUILTIN); }
 205
 206   switch(behavior) {
 207   case STAY_ON_TABLE: stay_on_table(); break;
 208   case        FOLLOW:        follow(); break; }
 209
 210   prior_button_state = button_state; }
 211 */