Diff: DriveController.cpp

Revision:

6:83c1f8d6a65a

Parent:

5:f53f06a866e9

Child:

7:2f3e841ee0ff

--- a/DriveController.cpp	Thu Oct 16 04:10:32 2014 +0000
+++ b/DriveController.cpp	Fri Oct 17 17:54:16 2014 +0000
@@ -1,5 +1,4 @@
 #include "DriveController.h"
-#include <math>
 
 DriveController::DriveController() : i2c(PTC2, PTC1), wheelSpeed1(PTB0), wheelSpeed2(PTB1), wheelSpeed3(PTB2),
     wheelSpeed4(PTB3), wheelDirection1(PTE20), wheelDirection2(PTE21), wheelDirection3(PTE22), wheelDirection4(PTE23),
@@ -11,7 +10,15 @@
 
 void DriveController::go()
 {
-    while(true)
+    while(true) //test loop
+    {
+        move();
+        rotate('R');
+        rotate('R');
+        move();
+    }
+    
+    /*while(true)
     {
         getCommand();
         
@@ -27,7 +34,7 @@
         }
             
         sendComplete();
-    }    
+    }*/    
 }
 
 void DriveController::move(char direction)
@@ -42,6 +49,9 @@
         
     do
     {
+        sensors.read();
+        sensors.lineDetect(sensorStates);
+        
         if(intersection())
         {
             wheelSpeed1 = wheelSpeed3 = 1;
@@ -49,15 +59,13 @@
         }
         else
         {
-            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;
@@ -67,8 +75,7 @@
             if(atLine)
                 atLine = false;
         }       
-    }
-    while(!intersection() || atLine)
+    } while(!intersection() || atLine);
     
     wheelSpeed1 = wheelSpeed2 = wheelSpeed3 = wheelSpeed4 = 0;
 }
@@ -80,8 +87,7 @@
     
     for(int i = 7; i >= 0; i--)
     {
-        if(sensorStates[i][1] == 1)
-            bin += pow(2, 7-i);
+            bin += sensorStates[i][1]<<(7-i);
     }
     
     switch(bin)
@@ -122,13 +128,41 @@
 
 bool DriveController::intersection()
 {
+    
+    // add speed bump check later
+    
     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(char direction)
 {
+    bool atLine = true;
     
+    if(direction == 'L')
+    {
+        wheelDirection1 = wheelDirection3 = 0;
+        wheelDirection2 = wheelDirection4 = 1;
+    }
+    else
+    {
+        wheelDirection1 = wheelDirection3 = 1;
+        wheelDirection2 = wheelDirection3 = 0;
+    }
+    
+    wheelSpeed1 = wheelSpeed2 = wheelSpeed3 = wheelSpeed4 = 1;
+    
+    do
+    {
+        sensors.read();
+        sensors.lineDetect(sensorStates);
+        
+        if(!intersection())
+            atLine = false;
+            
+    } while(!intersection() || atLine);
+    
+    wheelSpeed1 = wheelSpeed2 = wheelSpeed3 = wheelSpeed4 = 0;
 }
 
 void DriveController::getCommand()