challenge-bot.com Git - challenge-bot/blob - phase2/phase2.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   Motor.
  17   Runs both motors back and forth.
  18 n
  19   This example code is in the public domain.
  20  */
  21
  22 int motor1_enable = 10;
  23 int motor1a = 9;
  24 int motor1b = 8;
  25 int motor2_enable = 3;
  26 int motor2a = 4;
  27 int motor2b = 5;
  28
  29 // ping sensors
  30 int left_trigger = 12;
  31 int left_echo = 11;
  32 int right_trigger = 7;
  33 int right_echo = 6;
  34
  35 // the setup routine runs once when you press reset:
  36 void setup() {
  37   Serial.begin(9600);
  38
  39   // initialize the digital pin as an output.
  40   pinMode(motor1_enable, OUTPUT);
  41   pinMode(motor1a, OUTPUT);
  42   pinMode(motor1b, OUTPUT);
  43   pinMode(motor2_enable, OUTPUT);
  44   pinMode(motor2a, OUTPUT);
  45   pinMode(motor2b, OUTPUT);
  46
  47   // ping sensors
  48   pinMode(left_trigger, OUTPUT);
  49   pinMode(left_echo, INPUT);
  50   pinMode(right_trigger, OUTPUT);
  51   pinMode(right_echo, INPUT);
  52
  53   digitalWrite(motor1_enable, LOW);
  54   digitalWrite(motor1a, LOW);
  55   digitalWrite(motor1b, LOW);
  56   digitalWrite(motor2_enable, LOW);
  57   digitalWrite(motor2a, LOW);
  58   digitalWrite(motor2b, LOW);
  59 }
  60
  61 void
  62 on(int pin)
  63 {
  64   digitalWrite (pin, HIGH);
  65 }
  66
  67 void
  68 off(int pin)
  69 {
  70   digitalWrite (pin, LOW);
  71 }
  72
  73 int
  74 ping(int trigger, int echo)
  75 {
  76   int ping_time;
  77   on(trigger);
  78   delayMicroseconds(12);
  79   off(trigger);
  80   ping_time = pulseIn (echo, HIGH);
  81   if (ping_time <= 0)
  82     ping_time = 3000;
  83   return ping_time;
  84 }
  85
  86 void motorsRun(int left, int right, int ms_delay) {
  87   // Set left motor direction:
  88   if (left > 0) {
  89     // Set left motor to go forward:
  90     digitalWrite(motor1a, HIGH);
  91     digitalWrite(motor1b, LOW);
  92   } else {
  93     // Set left motor to go backward:
  94     digitalWrite(motor1a, LOW);
  95     digitalWrite(motor1b, HIGH);
  96     left = -left;                   // Make left a positive value:
  97   }
  98   analogWrite(motor1_enable, left); // Start motor in right direction
  99
 100   // Set left motor direction:
 101   if (right > 0) {
 102     // Set right motor to go forward:
 103     digitalWrite(motor2a, HIGH);
 104     digitalWrite(motor2b, LOW);
 105   } else {
 106     // Set right motor to go backward:
 107     digitalWrite(motor2a, LOW);
 108     digitalWrite(motor2b, HIGH);
 109     right = -right;                 // Make right a positive value:
 110   }
 111   analogWrite(motor2_enable, right); // Start motor in right direction
 112
 113   delay(ms_delay);                  // Wait the specified amount of time
 114 }
 115
 116 // the loop routine runs over and over again forever:
 117 void
 118 loop()
 119 {
 120   int left_speed = 0;
 121   int right_speed = 0;
 122
 123   int left_ping = ping (left_trigger, left_echo);
 124   int right_ping = ping (right_trigger, right_echo);
 125
 126   /*
 127   Serial.print ("left ping = ");
 128   Serial.print (left_ping);
 129
 130   Serial.print ("  right ping = ");
 131   Serial.println (right_ping);
 132   */
 133
 134   if (left_ping < 400)
 135     {
 136       left_speed = 250;
 137     }
 138
 139   if (right_ping < 400)
 140     {
 141       right_speed = 250;
 142     }
 143
 144   if (left_speed == 0 && right_speed == 0)
 145     {
 146       // backup
 147       motorsRun(-250, -250, 2000);
 148       // turn around
 149       motorsRun(-250, 250, 3200);
 150     }
 151   else
 152     {
 153       motorsRun(left_speed, right_speed, 0);
 154     }
 155 }