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

   1 /*
   2  This program is free software: you can redistribute it and/or modify
   3     it under the terms of the GNU Affero General Public License as
   4     published by the Free Software Foundation, either version 3 of the
   5     License, or (at your option) any later version.
   6
   7     This program is distributed in the hope that it will be useful,
   8     but WITHOUT ANY WARRANTY; without even the implied warranty of
   9     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  10     GNU Affero General Public License for more details.
  11
  12     You should have received a copy of the GNU Affero General Public License
  13     along with this program.  If not, see <http://www.gnu.org/licenses/>.
  14  */
  15
  16 // define which pins are connected to which components
  17 // blue wire
  18 int right_motor_forward_pin = 3;
  19 // orange wire
  20 int right_motor_backward_pin = 4;
  21 // white wire
  22 int right_motor_speed_pin = 5;
  23
  24 // yellow wire
  25 int right_echo_pin = 6;
  26 // blue wire
  27 int right_trigger_pin = 7;
  28
  29 // blue wire
  30 int left_motor_forward_pin = 8;
  31 // orange wire
  32 int left_motor_backward_pin = 9;
  33 // white wire
  34 int left_motor_speed_pin = 10;
  35
  36 // yellow wire
  37 int left_echo_pin = 11;
  38 // blue wire
  39 int left_trigger_pin = 12;
  40
  41 void on(int pin){
  42   digitalWrite(pin, HIGH);}
  43
  44 void off(int pin){
  45   digitalWrite(pin, LOW);}
  46
  47 void set_motor(int speed_pin,
  48                int forward_pin,
  49                int backward_pin,
  50                int speed){
  51   if(speed > 0){
  52     off(backward_pin);
  53     on(forward_pin);}
  54   else if(speed < 0){
  55     off(forward_pin);
  56     on(backward_pin);
  57     speed = -speed;}
  58   else{ // speed is 0
  59     off(forward_pin);
  60     off(backward_pin);}
  61   // since speed has been set positive, no need to check if speed < -255.
  62   if(speed > 255){
  63     speed = 255;}
  64   analogWrite(speed_pin, speed);}
  65
  66 void go(int left_motor_speed, int right_motor_speed){
  67   set_motor(left_motor_speed_pin,
  68             left_motor_forward_pin,
  69             left_motor_backward_pin,
  70             left_motor_speed);
  71   set_motor(right_motor_speed_pin,
  72             right_motor_forward_pin,
  73             right_motor_backward_pin,
  74             right_motor_speed);}
  75
  76 int ping(int trigger, int echo){
  77   int ping_time = 0;
  78   // turn off trigger
  79   off(trigger);
  80   delayMicroseconds(2);
  81   // turn on the trigger and leave it on long enough for the
  82   // sonar sensor to notice
  83   on(trigger);
  84   delayMicroseconds(10);
  85   off(trigger);
  86   ping_time = pulseIn(echo, HIGH);
  87   if(ping_time <= 0){
  88     ping_time = 3000;}
  89   // sonar needs some time to recover before pinging again,
  90   // so make sure it gets enough sleep right here.  50 milliseconds
  91   delay(50);
  92   return ping_time;}
  93
  94 void backup(int backup_time){
  95   go(-250, -250);
  96   delay(backup_time);}
  97
  98 void turn_around(int turn_around_time){
  99   go(-250, 250);
 100   delay(turn_around_time);}
 101
 102 void setup(){
 103   Serial.begin(9600);
 104
 105   pinMode(right_motor_speed_pin, OUTPUT);
 106   pinMode(right_motor_forward_pin, OUTPUT);
 107   pinMode(right_motor_backward_pin, OUTPUT);
 108
 109   pinMode(right_echo_pin, INPUT);
 110   pinMode(right_trigger_pin, OUTPUT);
 111
 112   pinMode(left_motor_speed_pin, OUTPUT);
 113   pinMode(left_motor_forward_pin, OUTPUT);
 114   pinMode(left_motor_backward_pin, OUTPUT);
 115
 116   pinMode(left_echo_pin, INPUT);
 117   pinMode(left_trigger_pin, OUTPUT);
 118
 119   off(left_motor_speed_pin);
 120   off(left_motor_forward_pin);
 121   off(left_motor_backward_pin);
 122   off(left_trigger_pin);
 123
 124   off(right_motor_speed_pin);
 125   off(right_motor_forward_pin);
 126   off(right_motor_backward_pin);
 127   off(right_trigger_pin);}
 128
 129 void loop(){
 130   int left_speed;
 131   int right_speed;
 132
 133   int forward_speed = 250;
 134   int stop_speed = 0;
 135
 136   // adjust this number as necessary for your robot.
 137   // it represents how far the table is from your sonar sensor.
 138   // larger values mean larger distance.  default is 800
 139   int right_max_ping_time_over_table = 800;
 140   int left_max_ping_time_over_table = 800;
 141   int backup_time = 2000;
 142   // the exact amount of time for turning around might need
 143   // twerking for your robot.  the default value is 3200
 144   int turn_around_time = 3200;
 145
 146   int actual_left_ping_time = ping(left_trigger_pin, left_echo_pin);
 147   int actual_right_ping_time = ping(right_trigger_pin, right_echo_pin);
 148
 149   Serial.print("left ping = ");
 150   Serial.print(actual_left_ping_time);
 151   Serial.print("\tright_ping = ");
 152   Serial.println(actual_right_ping_time);
 153
 154   // if the left sonar senses a table, keep driving left side forward,
 155   // otherwise, stop left side
 156   if(actual_left_ping_time < left_max_ping_time_over_table){
 157     left_speed = forward_speed;}
 158   else{
 159     left_speed = stop_speed;}
 160   // if the right sonar senses a table, keep driving right side forward,
 161   // otherwise, stop right side
 162   if(actual_right_ping_time < right_max_ping_time_over_table){
 163     right_speed = forward_speed;}
 164   else{
 165     right_speed = stop_speed;}
 166
 167   // if both sonars detect being off the table, backup and turn around
 168   // otherwise, go the correct speed for each wheel
 169   if(actual_left_ping_time >= left_max_ping_time_over_table
 170      && actual_right_ping_time >= right_max_ping_time_over_table){
 171     Serial.println("backing up");
 172     backup(backup_time);
 173     Serial.println("turning around");
 174     turn_around(turn_around_time);}
 175   else{
 176     Serial.println("going");
 177     go(left_speed, right_speed);}}