Diff: DriveController.cpp

Revision:

5:f53f06a866e9

Parent:

4:ac6b2e5b240b

Child:

6:83c1f8d6a65a

--- a/DriveController.cpp	Tue Oct 14 02:39:29 2014 +0000
+++ b/DriveController.cpp	Thu Oct 16 04:10:32 2014 +0000
@@ -1,20 +1,12 @@
 #include "DriveController.h"
+#include <math>
 
-#define REFLECTION_THRESHOLD = 0
-
-DriveController::DriveController() : orientation(NORTH), i2c(PTC2, PTC1), wheelSpeed1(PTB0), wheelSpeed2(PTB1), wheelSpeed3(PTB2),
-    wheelSpeed4(PTB3), wheelDirection1(PTE20), wheelDirection2(PTE21), wheelDirection3(PTE22), wheelDirection4(PTE23)
+DriveController::DriveController() : i2c(PTC2, PTC1), wheelSpeed1(PTB0), wheelSpeed2(PTB1), wheelSpeed3(PTB2),
+    wheelSpeed4(PTB3), wheelDirection1(PTE20), wheelDirection2(PTE21), wheelDirection3(PTE22), wheelDirection4(PTE23),
+    sensors(PTC7, PTC0, PTC3, PTC4, PTC5, PTC6, PTC10, PTC11, PTC9, PTC8, PTA5, PTA4, PTA12, PTD4, PTA2, PTA1, PTC12, PTC13,
+    PTC16, PTC17, PTD1, PTD0, PTD5, PTD13)
 {
     
-    PinName sensorPins[] = {PTC7, PTC0, PTC3, PTC4, PTC5, PTC6, PTC10, PTC11, PTC9, PTC8, PTA5, PTA4, PTA12, PTD4, PTA2, PTA1,
-        PTC12, PTC13, PTC16, PTC17, PTD2, PTD0, PTD5, PTD13}; 
-    command.direction = NORTH;
-    command.distance = 0;
-    
-    for(int i = 0; i < 24; i++)
-    {
-        sensors.push_back(DigitalInOut(sensorPins[i]));
-    }    
 }
 
 void DriveController::go()
@@ -23,53 +15,118 @@
     {
         getCommand();
         
-        if(command.distance != 0)
-            move();
+        switch(command)
+        {
+            case 0: move(); //move forward
+                break;
+            case 1: move('B'); //move backward
+                break;
+            case 2: rotate('R'); //rotate right
+                break;
+            case 3: rotate('L'); //rotate left
+        }
             
         sendComplete();
     }    
 }
 
-void DriveController::move()
+void DriveController::move(char direction)
 {
-    int travelled = 0;
-    bool atIntersection = true;
+    bool atLine = true; //tracks whether the bot is still at the starting intersection
+    int error = 0;
     
-    if(orientation != command.direction)
-        rotate(command.direction);
+    if(direction == 'B')
+        wheelDirection1 = wheelDirection2 = wheelDirection3 = wheelDirection4 = 0;
+    else
+        wheelDirection1 = wheelDirection2 = wheelDirection3 = wheelDirection4 = 1;
         
-    while(travelled < command.distance)
+    do
     {
-        forward(calculateError());
-        
-        if(intersection() && !atIntersection)
+        if(intersection())
+        {
+            wheelSpeed1 = wheelSpeed3 = 1;
+            wheelSpeed2 = wheelSpeed4 = 1;
+        }
+        else
         {
-            travelled++;
-            atIntersection = true;
-        }
-        else if(!intersection())
-            atIntersection = false;
+            sensors.read();
+            sensors.lineDetect(sensorStates);
+            error = calculateError();
+            
+            if(error < 0)
+            {
+                wheelSpeed1 = wheelSpeed3 = 1.0 - abs(error)*0.09;
+                wheelSpeed2 = wheelSpeed4 = 1;
+            {
+            else
+            {
+                wheelSpeed1 = wheelSpeed3 = 1;
+                wheelSpeed2 = wheelSpeed4 = 1.0 - error*0.09;
+            }
+            
+            if(atLine)
+                atLine = false;
+        }       
+    }
+    while(!intersection() || atLine)
+    
+    wheelSpeed1 = wheelSpeed2 = wheelSpeed3 = wheelSpeed4 = 0;
+}
+
+int DriveController::calculateError()
+{
+    int error;
+    int bin = 0;
+    
+    for(int i = 7; i >= 0; i--)
+    {
+        if(sensorStates[i][1] == 1)
+            bin += pow(2, 7-i);
     }
     
-    command.distance = 0;
-}
-
-double DriveController::calculateError()
-{
-    return 0;
-}
-
-void DriveController::forward(double correction)
-{
-    
+    switch(bin)
+    {
+        case 1: error = 7;
+            break;
+        case 3: error = 6;
+            break;
+        case 2: error = 5;
+            break;
+        case 6: error = 4;
+            break;
+        case 4: error = 3;
+            break;
+        case 12: error = 2;
+            break;
+        case 8: error = 1;
+            break;
+        case 24: error = 0;
+            break;
+        case 16: error = -1;
+            break;
+        case 48: error = -2;
+            break;  
+        case 32: error = -3;
+            break;
+        case 96: error = -4;
+            break;
+        case 64: error = -5;
+            break;
+        case 192: error = -6;
+            break;
+        case 128: error = -7;
+    }
+                
+    return error;
 }
 
 bool DriveController::intersection()
 {
-    return sensorStates[8]&&sensorStates[9]&&sensorStates[10]&&sensorStates[12]&&sensorStates[13]&&sensorStates[14]&&sensorStates[15];
+    return (sensorStates[0][1]&&sensorStates[1][1]&&sensorStates[2][1]&&sensorStates[3][1]) 
+            || (sensorStates[4][1]&&sensorStates[5][1]&&sensorStates[6][1]&&sensorStates[7][1]);
 }
 
-void DriveController::rotate(Direction diretion)
+void DriveController::rotate(char direction)
 {
     
 }
@@ -83,8 +140,3 @@
 {
     
 }
-
-void DriveController::readSensors()
-{
-    
-}
\ No newline at end of file