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