cleaned up robot code

wiichuckControlledIRAvoidingLibrary/wiichuckControlledIRAvoidingLibrary.pde

@@ -1,48 +1,55 @@
-#include <Wire.h>
-#include <Servo.h>
-#include <wiichuck.h>
-#include <pmc.h>
-
-
-#define READ_DELAY 50 
-
+#include "Wire.h"
+#include "Servo.h"
+#include "wiichuck.h"
+#include "pmc.h"
 
+//constants
+#define readDelay 50 
 #define maxAngle 115
+#define midAngle 90
 #define minAngle 65
-
-
 #define left 1
 #define right 0
-
 #define forward 255
 #define backward 0
 #define stopped 128
 
-unsigned long previous_read_time = 0;
-
+//pins
+#define tuningPot 0
+#define irSensor 2
+#define underControlLED 10
+#define autonomousLED 11
+#define obstacleLED 13
+#define servoPin 4
+#define motorControlPin 7
+#define motorResetPin 8
+
+//controllers
 Servo myservo;
 MotorController mc;
 WiiChuck chuck;
 
-boolean obstacle = false;
-int increment = 10;
-int motorState = 2;
+//globals
+unsigned long previous_read_time = 0;
+int sweepIncrement = 10;
 boolean autonomous = false;
 
-
 void setup()
 {
-  pinMode(0, INPUT);
-  pinMode(2, INPUT);
-  pinMode(13, OUTPUT);
-  pinMode(11, OUTPUT);
-  pinMode(10, OUTPUT);
-  myservo.attach(4); 
-  myservo.write(90);
-  mc.init(7, 8);
+  pinMode(tuningPot, INPUT);
+  pinMode(irSensor, INPUT);
+  pinMode(obstacleLED, OUTPUT);
+  pinMode(autonomousLED, OUTPUT);
+  pinMode(underControlLED, OUTPUT);
+
+  myservo.attach(servoPin); 
+  myservo.write(midAngle);
+
+  mc.init(motorControlPin, motorResetPin);
+
   Wire.begin();
-  previous_read_time = millis();
   Serial.begin(9600);
+  previous_read_time = millis();
 }
 
 void loop()
@@ -52,7 +59,7 @@ void loop()
     chuck.init();
   }
 
-  if (chuck.inSync && (millis() - previous_read_time > READ_DELAY))
+  if (chuck.inSync && (millis() - previous_read_time > readDelay))
   {
     previous_read_time = millis();
     if(chuck.readData())
@@ -71,89 +78,65 @@ void loop()
         autonomous = false;
       } 
 
-      if(chuck.btn_z!= 0)
-      {
-        autonomous = true;
-      }
-
       if(chuck.btn_c!= 0)
       {
         //stopped
         mc.SetMotor(left, stopped);
         mc.SetMotor(right, stopped);
         autonomous = false;
       }
+
+      if(chuck.btn_z!= 0)
+      {
+        autonomous = true;
+      }
     }
 
     if(autonomous)
     {
-      motorState = 2;
       sweep();
     } 
-    readSensor();
-    digitalWrite(11,  autonomous ? HIGH : LOW);
-    digitalWrite(10,  autonomous ? LOW : HIGH);
+    else
+    {
+      myservo.write(midAngle);
+    }
+
+    digitalWrite(autonomousLED,  autonomous ? HIGH : LOW);
+    digitalWrite(underControlLED,  autonomous ? LOW : HIGH);
   }
 }
 
-
-
-void go()
-{
-  if(motorState == 0) return;
-  motorState = 0;
-  mc.SetMotor(left, forward);
-  mc.SetMotor(right, forward);
-}
-
-void goLeft()
-{
-  if(motorState == -1) return;
-  motorState = -1;
-  mc.SetMotor(left, backward);
-  mc.SetMotor(right, forward);
-}
-
-void goRight()
-{
-  if(motorState == 1) return;
-  motorState = 1;
-  mc.SetMotor(left, forward);
-  mc.SetMotor(right, backward);
-}
-
 void sweep()
 {
+  int sensor = analogRead(irSensor);
+  int thresh = analogRead(tuningPot);
+  boolean obstacle = sensor > thresh;
+  digitalWrite(obstacleLED,  obstacle ? HIGH : LOW);
+
   int pos = myservo.read();
   if(obstacle)
   {
-    if(pos-90 < 0)
+    if(pos < midAngle)
     {
-      goRight();
+      mc.SetMotor(left, forward);
+      mc.SetMotor(right, backward);
     }
     else
     {
-      goLeft();
+      mc.SetMotor(left, backward);
+      mc.SetMotor(right, forward);
     }
   }
   else
   {
-    go();
-    pos +=  increment;
-    if(pos >= maxAngle || pos <= minAngle) {increment *= -1;}
-
+    mc.SetMotor(left, forward);
+    mc.SetMotor(right, forward);
+    pos +=  sweepIncrement;
+    if(pos >= maxAngle || pos <= minAngle) {sweepIncrement *= -1;}
     myservo.write(pos);
   }
 }
 
-void readSensor() 
-{  
-  int sensor = analogRead(2);
-  int thresh = analogRead(0);
-  obstacle = sensor > thresh;
-  digitalWrite(13,  obstacle ? HIGH : LOW);
-}
-