Diff: DriveController.cpp

Revision:

2:d0ce8e26cbc4

Parent:

1:fa6eb0c33b2f

Child:

3:0d7687b6ef14

--- a/DriveController.cpp	Wed Sep 24 21:03:56 2014 +0000
+++ b/DriveController.cpp	Fri Oct 03 03:48:39 2014 +0000
@@ -1,16 +1,77 @@
 #include "DriveController.h"
 
+#define INTERSECTION_THRESHOLD = 8
+
+DriveController::DriveController()
+{
+    direction = NORTH;
+    command.direction = NORTH;
+    command.distance = 0;
+}
+
 DriveController::void go()
 {
+    while(true)
+    {
+        getCommand();
+        if(currentCommand.distance != 0)
+            move();
+    }    
+}
+
+DriveController::void move()
+{
+    int travelled = 0;
+    
+    if(orientation != command.direction)
+        rotate(command.direction);
+        
+    while(travelled < command.distance)
+    {
+        forward(calculateError());
+        if(intersection())
+            travelled++;
+    }
+}
+
+DriveController::double calculateError()
+{
     
 }
 
-DriveController::void move(Direction direction)
+DriveController::void forward(double correction)
 {
-    if(orientation != direction)
+    
+}
+
+DriveController::bool intersection()
+{
+    int sensorTally = 0;
+    
+    readSensors();
+    for(int i = 0; i < 24; i++)
     {
-        bool divert;
-        while(true)
-        {
-            readSensors();
+        if(sensorStates[i]==true)
+            sensorTally++;
+    }
+    
+    if(sensorTally > INTERSECTION_THRESHOLD)
+        return true;
+    else
+        return false;
+}
 
+DriveController::void rotate(Direction diretion)
+{
+    
+}
+
+DriveController::void getCommand()
+{
+    
+}
+
+DriveControler::void readSensors()
+{
+    
+}
\ No newline at end of file