Diff: DriveController.cpp

Revision:

3:0d7687b6ef14

Parent:

2:d0ce8e26cbc4

Child:

4:ac6b2e5b240b

--- a/DriveController.cpp	Fri Oct 03 03:48:39 2014 +0000
+++ b/DriveController.cpp	Fri Oct 03 18:54:42 2014 +0000
@@ -1,12 +1,26 @@
 #include "DriveController.h"
 
-#define INTERSECTION_THRESHOLD = 8
+#define REFLECTION_THRESHOLD = 0
 
-DriveController::DriveController()
+/*DriveController::DriveController() : wheel1(PTC7), wheel2(PTC2), wheel3(PTE29), wheel4(PTE30), sensor01(PTC7),
+    sensor02(PTC0), sensor03(PTC3), sensor04(PTC4), sensor05(PTC5), sensor06(PTC6), sensor07(PTC10), sensor08(PTC11), 
+    sensor09(PTC9), sensor10(PTC8), sensor11(PTA5), sensor12(PTA4), sensor13(PTA12), sensor14(PTD4), sensor15(PTA2),
+    sensor16(PTA1), sensor17(PTC12), sensor18(PTC13), sensor19(PTC16), sensor20(PTC17), sensor21(PTD2), sensor22(PTD0),
+    sensor23(PTD5), sensor24(PTD13), orientation(NORTH)
 {
-    direction = NORTH;
     command.direction = NORTH;
     command.distance = 0;
+}*/
+
+DriveController::DriveController() : wheel1(PTC7), wheel2(PTC2), wheel3(PTE29), wheel4(PTE30), orientation(NORTH)
+{
+    command.direction = NORTH;
+    command.distance = 0;
+    
+    for(int i = 0; i < 24; i++)
+    {
+        sensors.push_back(new DigitalInOut(sensorPinNames[i]));
+    }
 }
 
 DriveController::void go()
@@ -14,14 +28,18 @@
     while(true)
     {
         getCommand();
+        
         if(currentCommand.distance != 0)
             move();
+            
+        sendComplete();
     }    
 }
 
 DriveController::void move()
 {
     int travelled = 0;
+    bool atIntersection = true;
     
     if(orientation != command.direction)
         rotate(command.direction);
@@ -29,9 +47,17 @@
     while(travelled < command.distance)
     {
         forward(calculateError());
-        if(intersection())
+        
+        if(intersection() && !atIntersection)
+        {
             travelled++;
+            atIntersection = true;
+        }
+        else if(!intersection())
+            atIntersection = false;
     }
+    
+    command.distance = 0;
 }
 
 DriveController::double calculateError()
@@ -46,19 +72,10 @@
 
 DriveController::bool intersection()
 {
-    int sensorTally = 0;
-    
-    readSensors();
-    for(int i = 0; i < 24; i++)
-    {
-        if(sensorStates[i]==true)
-            sensorTally++;
-    }
-    
-    if(sensorTally > INTERSECTION_THRESHOLD)
-        return true;
-    else
-        return false;
+    return (sensorStates[0]||sensorStates[8]||sensorStates[16])&&(sensorStates[1]||sensorStates[9]||sensorStates[17])
+        &&(sensorStates[2]||sensorStates[10]||sensorStates[18])&&(sensorStates[4]||sensorStates[12]||sensorStates[20])
+        &&(sensorStates[5]||sensorStates[13]||sensorStates[21])&&(sensorStates[6]||sensorStates[14]||sensorStates[22])
+        &&(sensorStates[7]||sensorStates[15]||sensorStates[23]);
 }
 
 DriveController::void rotate(Direction diretion)