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 int right_motor_speed_pin = 3;
  18 int right_motor_forward_pin = 4;
  19 int right_motor_backward_pin = 5;
  20
  21 int right_echo_pin = 6;
  22 int right_trigger_pin = 7;
  23
  24 int left_motor_speed_pin = 8;
  25 int left_motor_forward_pin = 9;
  26 int left_motor_backward_pin = 10;
  27
  28 int left_echo_pin = 11;
  29 int left_trigger_pin = 12;
  30
  31 void on(int pin){
  32   digitalWrite(pin, HIGH);}
  33
  34 void off(int pin){
  35   digitalWrite(pin, LOW);}
  36
  37 void set_motor(int speed_pin,
  38                int forward_pin,
  39                int backward_pin,
  40                int speed){
  41   if(speed > 0){
  42     off(backward_pin);
  43     on(forward_pin);}
  44   else if(speed < 0){
  45     off(forward_pin);
  46     on(backward_pin);
  47     speed = -speed;}
  48   else{ // speed is 0
  49     off(forward_pin);
  50     off(backward_pin);}
  51   if(speed > 255){
  52     speed = 255;}
  53   analogWrite(speed_pin, speed);}
  54
  55 void go(int left_motor_speed, int right_motor_speed){
  56   set_motor(left_motor_speed_pin,
  57             left_motor_forward_pin,
  58             left_motor_backward_pin,
  59             left_motor_speed);
  60   set_motor(right_motor_speed_pin,
  61             right_motor_forward_pin,
  62             right_motor_backward_pin,
  63             right_motor_speed);}
  64
  65 int ping(int trigger, int echo){
  66   int ping_time = 0;
  67   // turn off trigger
  68   off(trigger);
  69   delayMicroseconds(2);
  70   // turn on the trigger and leave it on long enough for the
  71   // sonar sensor to notice
  72   on(trigger);
  73   delayMicroseconds(10);
  74   off(trigger);
  75   ping_time = pulseIn(echo, HIGH);
  76   if(ping_time <= 0){
  77     ping_time = 3000;}
  78   // sonar needs some time to recover before pinging again,
  79   // so make sure it gets enough sleep right here.  50 milliseconds
  80   delay(50);
  81   return ping_time;}
  82
  83 void backup(int backup_time){
  84   go(-250, -250);
  85   delay(backup_time);}
  86
  87 void turn_around(int turn_around_time){
  88   go(-250, 250);
  89   delay(turn_around_time);}
  90
  91 void setup(){
  92   Serial.begin(9600);
  93
  94   pinMode(right_motor_speed_pin, OUTPUT);
  95   pinMode(right_motor_forward_pin, OUTPUT);
  96   pinMode(right_motor_backward_pin, OUTPUT);
  97
  98   pinMode(right_echo_pin, INPUT);
  99   pinMode(right_trigger_pin, OUTPUT);
 100
 101   pinMode(left_motor_speed_pin, OUTPUT);
 102   pinMode(left_motor_forward_pin, OUTPUT);
 103   pinMode(left_motor_backward_pin, OUTPUT);
 104
 105   pinMode(left_echo_pin, INPUT);
 106   pinMode(left_trigger_pin, OUTPUT);
 107
 108   off(left_motor_speed_pin);
 109   off(left_motor_forward_pin);
 110   off(left_motor_backward_pin);
 111   off(left_trigger_pin);
 112
 113   off(right_motor_speed_pin);
 114   off(right_motor_forward_pin);
 115   off(right_motor_backward_pin);
 116   off(right_trigger_pin);}
 117
 118 void loop(){
 119   int left_speed;
 120   int right_speed;
 121
 122   int forward_speed = 250;
 123   int stop_speed = 0;
 124
 125   // adjust this number as necessary for your robot.
 126   // it represents how far the table is from your sonar sensor.
 127   // larger values mean larger distance.  default is 400
 128   int right_max_ping_time_over_table = 500;
 129   int left_max_ping_time_over_table = 500;
 130   int backup_time = 2000;
 131   // the exact amount of time for turning around might need
 132   // twerking for your robot.  the default value is 3200
 133   int turn_around_time = 2133;
 134
 135   int actual_left_ping_time = ping(left_trigger_pin, left_echo_pin);
 136   int actual_right_ping_time = ping(right_trigger_pin, right_echo_pin);
 137
 138   Serial.print("left ping = ");
 139   Serial.print(actual_left_ping_time);
 140   Serial.print("\tright_ping = ");
 141   Serial.println(actual_right_ping_time);
 142
 143   // if the left sonar senses a table, keep driving left side forward,
 144   // otherwise, stop left side
 145   if(actual_left_ping_time < left_max_ping_time_over_table){
 146     left_speed = forward_speed;}
 147   else{
 148     left_speed = stop_speed;}
 149   // if the right sonar senses a table, keep driving right side forward,
 150   // otherwise, stop right side
 151   if(actual_right_ping_time < right_max_ping_time_over_table){
 152     right_speed = forward_speed;}
 153   else{
 154     right_speed = stop_speed;}
 155
 156   // if both sonars detect being off the table, backup and turn around
 157   // otherwise, go the correct speed for each wheel
 158   if(actual_left_ping_time >= left_max_ping_time_over_table
 159      && actual_right_ping_time >= right_max_ping_time_over_table){
 160     Serial.println("backing up");
 161     backup(backup_time);
 162     Serial.println("turning around");
 163     turn_around(turn_around_time);}
 164   else{
 165     Serial.println("going");
 166     go(left_speed, right_speed);}}