challenge-bot.com Git - challenge-bot/blob - arduino-sketches/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 // use pin 13's LED to indicate intended travel direction.
  17 //  on == forward, off == backward
  18 int led = 13;
  19
  20 int leftMotorEnable = 10;
  21 int leftMotorA = 9;
  22 int leftMotorB = 8;
  23
  24 int rightMotorEnable = 3;
  25 int rightMotorA = 4;
  26 int rightMotorB = 5;
  27
  28 void setupMotor(int motorEnable, int motorA, int motorB){
  29     pinMode(motorEnable, OUTPUT);
  30     pinMode(motorA, OUTPUT);
  31     pinMode(motorB, OUTPUT);
  32
  33     digitalWrite(motorEnable, LOW);
  34     digitalWrite(motorA, LOW);
  35     digitalWrite(motorB, LOW);
  36 }
  37
  38 // the setup routine runs once when you press reset:
  39 void setup() {
  40   // initialize the digital pin as an output.
  41   setupMotor(leftMotorEnable, leftMotorA, leftMotorB);
  42   setupMotor(rightMotorEnable, rightMotorA, rightMotorB);
  43
  44   pinMode(led, OUTPUT);
  45 }
  46
  47 void motorsRun(int left, int right, int msDelay) {
  48   // Set left motor direction:
  49   if (left > 0) {
  50     // Set left motor to go forward:
  51     digitalWrite(leftMotorA, HIGH);
  52     digitalWrite(leftMotorB, LOW);
  53   } else {
  54     // Set left motor to go backward:
  55     digitalWrite(leftMotorA, LOW);
  56     digitalWrite(leftMotorB, HIGH);
  57     left = -left;                   // Make left a positive value:
  58   }
  59
  60   analogWrite(rightMotorEnable, left); // Start motor in right direction
  61   // Set left motor direction:
  62   if (right > 0) {
  63     // Set right motor to go forward:
  64     digitalWrite(rightMotorA, HIGH);
  65     digitalWrite(rightMotorB, LOW);
  66   } else {
  67     // Set right motor to go backward:
  68     digitalWrite(rightMotorA, LOW);
  69     digitalWrite(rightMotorB, HIGH);
  70     right = -right;                 // Make right a positive value:
  71   }
  72   analogWrite(leftMotorEnable, left); // Start motor in right direction
  73
  74   delay(msDelay);                  // Wait the specified amount of time
  75
  76   // Stop both motors:
  77   analogWrite(leftMotorEnable, 0);
  78   analogWrite(rightMotorEnable, 0);
  79 }
  80
  81 // the loop routine runs over and over again forever:
  82 void loop() {
  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 }