challenge-bot.com Git - challenge-bot/blob - phase3/phase3.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 // This program implements table top challenge level 1 --
  16 // The robot stays on the table without falling off.
  17
  18 //  This example code is in the public domain.
  19
  20 // Set *debug* to 1 to enable debugging statements:
  21 int debug = 1;
  22
  23 // Set *stop_at_edge* to 1 to have robot stop at the edge;
  24 // otherwise, it will back away from edge:
  25 int stop_at_edge = 0;
  26
  27 // Set *table_top_threshold* to a value that captures the
  28 // tabletop distance for *your* robot:
  29 int table_top_threshold = 600;
  30
  31 // Set *sonar_timeout* to a value a 200-300 uSec longer
  32 // that your table top threshold:
  33
  34 int sonar_timeout = 1600;
  35
  36 // Set *sonar_set_point* to a value that specifies the approximate
  37 // follow distance:
  38
  39 int sonar_follow_distance = 800;
  40
  41 // Pin definitions:
  42 int led = 13;
  43 int left_motor_enable = 3;
  44 int left_motor_a = 4;
  45 int left_motor_b = 5;
  46 int right_motor_enable = 10;
  47 int right_motor_a = 9;
  48 int right_motor_b = 8;
  49 int right_sonar_trigger = 7;
  50 int right_sonar_echo = 6;
  51 int left_sonar_trigger = 12;
  52 int left_sonar_echo = 11;
  53
  54 // the setup routine runs once when you press reset:
  55 void setup() {
  56   // If debugging is enabled,
  57   if (debug) {
  58     // 115200 is the fastest "standard" baud rate for debugging.
  59     // Be sure to set the baud Tools=>Serial Monitor to 115200:
  60     Serial.begin(115200);
  61   }
  62
  63   // Set LED output pin:
  64   pinMode(led, OUTPUT);
  65
  66   // Set H-bridge control pins:
  67   pinMode(left_motor_enable, OUTPUT);
  68   pinMode(left_motor_a, OUTPUT);
  69   pinMode(left_motor_b, OUTPUT);
  70   pinMode(right_motor_enable, OUTPUT);
  71   pinMode(right_motor_a, OUTPUT);
  72   pinMode(right_motor_b, OUTPUT);
  73
  74   // Set control pins for both left and right sonars:
  75   pinMode(left_sonar_trigger, OUTPUT);
  76   pinMode(left_sonar_echo, INPUT);
  77   pinMode(right_sonar_trigger, OUTPUT);
  78   pinMode(right_sonar_echo, INPUT);
  79
  80   // Set all the pin values to predefined values:
  81   digitalWrite(led, HIGH);
  82   digitalWrite(left_motor_enable, LOW);
  83   digitalWrite(left_motor_a, LOW);
  84   digitalWrite(left_motor_b, LOW);
  85   digitalWrite(right_motor_enable, LOW);
  86   digitalWrite(right_motor_a, LOW);
  87   digitalWrite(right_motor_b, LOW);
  88   digitalWrite(left_sonar_trigger, LOW);
  89   digitalWrite(right_sonar_trigger, LOW);
  90   digitalWrite(left_sonar_trigger, LOW);
  91   digitalWrite(right_sonar_trigger, LOW);
  92 }
  93
  94 void motorsRun(int left, int right, int ms_delay) {
  95   // Set left motor direction:
  96   if (left > 0) {
  97     // Set left motor to go forward:
  98     digitalWrite(left_motor_a, HIGH);
  99     digitalWrite(left_motor_b, LOW);
 100   } else {
 101     // Set left motor to go backward:
 102     digitalWrite(left_motor_a, LOW);
 103     digitalWrite(left_motor_b, HIGH);
 104     left = -left;                   // Make left a positive value:
 105   }
 106   // Make sure we cap the speed at 255:
 107   if (left > 255) {
 108     left = 255;
 109   }
 110   analogWrite(left_motor_enable, left); // Start motor in right direction
 111
 112   // Set right motor direction:
 113   if (right > 0) {
 114     // Set right motor to go forward:
 115     digitalWrite(right_motor_a, HIGH);
 116     digitalWrite(right_motor_b, LOW);
 117   } else {
 118     // Set right motor to go backward:
 119     digitalWrite(right_motor_a, LOW);
 120     digitalWrite(right_motor_b, HIGH);
 121     right = -right;                 // Make right a positive value:
 122   }
 123   // Make sure we cap the speed at 255:
 124   if (right > 255) {
 125     right = 255;
 126   }
 127   analogWrite(right_motor_enable, right); // Start motor in right direction
 128
 129   delay(ms_delay);                  // Wait the specified amount of time
 130 }
 131
 132 // This routine will trigger a HC-SR04 sonar that has its
 133 // trigger pin attached to *sonar_trigger* and its echo
 134 // pin attached to *sonar_echo*.  If the echo response
 135 // takes longer than *timeout* microseconds, *timeout* is
 136 // returned.
 137 int ping(int sonar_trigger, int sonar_echo, int timeout) {
 138   int result = timeout;
 139   if (!digitalRead(sonar_echo)) {
 140     // *sonar_echo* is LOW, so we can safely trigger a 12uS
 141     // trigger pulse:
 142     digitalWrite(sonar_trigger, HIGH);
 143     delayMicroseconds(12);
 144     digitalWrite(sonar_trigger, LOW);
 145
 146     // Time the resulting echo pulse.  If the resulting echo pulse
 147     // is longer than *timeout*, *pulseIn* returns 0.
 148     result = pulseIn(sonar_echo, HIGH, timeout);
 149
 150     if (result == 0) {
 151       // We timed out, so return *timeout*:
 152       result = timeout;
 153     }
 154   } // else *sonar_echo* is still high from the previous trigger;
 155     // return *timeout* when this occurs.
 156   return result;
 157 }
 158
 159 // the loop routine runs over and over again forever:
 160 void loop() {
 161   // Get the left and right sonar values:
 162   int left_ping = ping(left_sonar_trigger, left_sonar_echo, sonar_timeout);
 163   int right_ping = ping(right_sonar_trigger, right_sonar_echo, sonar_timeout);
 164
 165   // Set the left and right motor speed:
 166   int left_speed = left_ping - sonar_follow_distance;
 167   int right_speed = right_ping - sonar_follow_distance;
 168
 169   // Debugging code:
 170   if (debug) {
 171     // Print some debugging information:
 172     Serial.print("lp=");
 173     Serial.print(left_ping);
 174     Serial.print(" rp=");
 175     Serial.print(right_ping);
 176     Serial.print(" ls=");
 177     Serial.print(left_speed);
 178     Serial.print(" rs=");
 179     Serial.println(right_speed);
 180
 181     // Delay a little so that the serial port does not get
 182     // swamped with debugging information:
 183     delay(100);
 184   }
 185
 186   // Set the motor speeds:
 187   motorsRun(left_speed, right_speed, 0);
 188 }