challenge-bot.com Git - challenge-bot/blob - phase1/phase1.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 // Pin 13 has an LED connected on most Arduino boards.
  17 // give it a name:
  18 int led = 13;
  19 int motor1_enable = 10;
  20 int motor1a = 9;
  21 int motor1b = 8;
  22 int motor2_enable = 3;
  23 int motor2a = 4;
  24 int motor2b = 5;
  25
  26 // the setup routine runs once when you press reset:
  27 void setup() {
  28   // initialize the digital pin as an output.
  29   pinMode(led, OUTPUT);
  30   pinMode(motor1_enable, OUTPUT);
  31   pinMode(motor1a, OUTPUT);
  32   pinMode(motor1b, OUTPUT);
  33   pinMode(motor2_enable, OUTPUT);
  34   pinMode(motor2a, OUTPUT);
  35   pinMode(motor2b, OUTPUT);
  36
  37   digitalWrite(led, HIGH);
  38   digitalWrite(motor1_enable, LOW);
  39   digitalWrite(motor1a, LOW);
  40   digitalWrite(motor1b, LOW);
  41   digitalWrite(motor2_enable, LOW);
  42   digitalWrite(motor2a, LOW);
  43   digitalWrite(motor2b, LOW);
  44 }
  45
  46 void motorsRun(int left, int right, int ms_delay) {
  47   // Set left motor direction:
  48   if (left > 0) {
  49     // Set left motor to go forward:
  50     digitalWrite(motor1a, HIGH);
  51     digitalWrite(motor1b, LOW);
  52   } else {
  53     // Set left motor to go backward:
  54     digitalWrite(motor1a, LOW);
  55     digitalWrite(motor1b, HIGH);
  56     left = -left;                   // Make left a positive value:
  57   }
  58
  59   analogWrite(motor2_enable, left); // Start motor in right direction
  60   // Set left motor direction:
  61   if (right > 0) {
  62     // Set right motor to go forward:
  63     digitalWrite(motor2a, HIGH);
  64     digitalWrite(motor2b, LOW);
  65   } else {
  66     // Set right motor to go backward:
  67     digitalWrite(motor2a, LOW);
  68     digitalWrite(motor2b, HIGH);
  69     right = -right;                 // Make right a positive value:
  70   }
  71   analogWrite(motor1_enable, left); // Start motor in right direction
  72
  73   delay(ms_delay);                  // Wait the specified amount of time
  74
  75   // Stop both motors:
  76   analogWrite(motor1_enable, 0);
  77   analogWrite(motor2_enable, 0);
  78 }
  79
  80 // the loop routine runs over and over again forever:
  81 void loop() {
  82
  83   digitalWrite(led, HIGH);   // turn the LED on (HIGH is the voltage level)
  84   motorsRun(100, 100, 2000);// Run the robot forward
  85   digitalWrite(led, LOW);    // turn the LED off by making the voltage LOW
  86   motorsRun(-100, -100, 2000); // Run the robot backward
  87 }