switch to camelCase to match arduino style
[challenge-bot] / phase1 / phase1.ino
index 9961a366c4535f4b0b1aec734b9e11440e2f672c..7c183734726c2a61ee3b58920dc8c9ea084ca635 100644 (file)
     along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-// Pin 13 has an LED connected on most Arduino boards.
-// give it a name:
+// use pin 13's LED to indicate intended travel direction.
+//  on == forward, off == backward
 int led = 13;
-int motor1_enable = 10;
-int motor1a = 9;
-int motor1b = 8;
-int motor2_enable = 3;
-int motor2a = 4;
-int motor2b = 5;
+
+int leftMotorEnable = 10;
+int leftMotorA = 9;
+int leftMotorB = 8;
+
+int rightMotorEnable = 3;
+int rightMotorA = 4;
+int rightMotorB = 5;
+
+void setupMotor(int motorEnable, int motorA, int motorB){
+    pinMode(motorEnable, OUTPUT);
+    pinMode(motorA, OUTPUT);
+    pinMode(motorB, OUTPUT);
+
+    digitalWrite(motorEnable, LOW);
+    digitalWrite(motorA, LOW);
+    digitalWrite(motorB, LOW);
+}
 
 // the setup routine runs once when you press reset:
 void setup() {
   // initialize the digital pin as an output.
-  pinMode(led, OUTPUT);
-  pinMode(motor1_enable, OUTPUT);
-  pinMode(motor1a, OUTPUT);
-  pinMode(motor1b, OUTPUT);
-  pinMode(motor2_enable, OUTPUT);
-  pinMode(motor2a, OUTPUT);
-  pinMode(motor2b, OUTPUT);
+  setupMotor(leftMotorEnable, leftMotorA, leftMotorB);
+  setupMotor(rightMotorEnable, rightMotorA, rightMotorB);
 
-  digitalWrite(led, HIGH);
-  digitalWrite(motor1_enable, LOW);
-  digitalWrite(motor1a, LOW);
-  digitalWrite(motor1b, LOW);
-  digitalWrite(motor2_enable, LOW);
-  digitalWrite(motor2a, LOW);
-  digitalWrite(motor2b, LOW);
+  pinMode(led, OUTPUT);
 }
 
-void motorsRun(int left, int right, int ms_delay) {
+void motorsRun(int left, int right, int msDelay) {
   // Set left motor direction:
   if (left > 0) {
     // Set left motor to go forward:
-    digitalWrite(motor1a, HIGH);
-    digitalWrite(motor1b, LOW);
+    digitalWrite(leftMotorA, HIGH);
+    digitalWrite(leftMotorB, LOW);
   } else {
     // Set left motor to go backward:
-    digitalWrite(motor1a, LOW);
-    digitalWrite(motor1b, HIGH);
+    digitalWrite(leftMotorA, LOW);
+    digitalWrite(leftMotorB, HIGH);
     left = -left;                   // Make left a positive value:
   }
 
-  analogWrite(motor2_enable, left); // Start motor in right direction
+  analogWrite(rightMotorEnable, left); // Start motor in right direction
   // Set left motor direction:
   if (right > 0) {
     // Set right motor to go forward:
-    digitalWrite(motor2a, HIGH);
-    digitalWrite(motor2b, LOW);
+    digitalWrite(rightMotorA, HIGH);
+    digitalWrite(rightMotorB, LOW);
   } else {
     // Set right motor to go backward:
-    digitalWrite(motor2a, LOW);
-    digitalWrite(motor2b, HIGH);
+    digitalWrite(rightMotorA, LOW);
+    digitalWrite(rightMotorB, HIGH);
     right = -right;                 // Make right a positive value:
   }
-  analogWrite(motor1_enable, left); // Start motor in right direction
+  analogWrite(leftMotorEnable, left); // Start motor in right direction
 
-  delay(ms_delay);                  // Wait the specified amount of time
+  delay(msDelay);                  // Wait the specified amount of time
 
   // Stop both motors:
-  analogWrite(motor1_enable, 0);
-  analogWrite(motor2_enable, 0);
+  analogWrite(leftMotorEnable, 0);
+  analogWrite(rightMotorEnable, 0);
 }
 
 // the loop routine runs over and over again forever:
 void loop() {
-
   digitalWrite(led, HIGH);   // turn the LED on (HIGH is the voltage level)
   motorsRun(100, 100, 2000);// Run the robot forward
   digitalWrite(led, LOW);    // turn the LED off by making the voltage LOW