Diff: DriveController.cpp

Revision:

9:01c17b286a99

Parent:

8:9030d2e3a1e8

Child:

10:210c8f1e3a92

--- a/DriveController.cpp	Sun Oct 19 02:39:17 2014 +0000
+++ b/DriveController.cpp	Fri Oct 24 22:46:24 2014 +0000
@@ -1,6 +1,9 @@
 #include "DriveController.h"
 
-DriveController::DriveController() : i2c(PTC2, PTC1), wheelSpeed1(PTB0), wheelSpeed2(PTB1), wheelSpeed3(PTB2),
+#define MOTOR_SCALE 0.5
+#define CORRECTION_SCALE 0.09
+
+DriveController::DriveController() : debugLog("log.txt"), 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)
@@ -10,6 +13,7 @@
 
 void DriveController::go()
 {
+    debugLog << "Starting Drive Controller!" << endl;
     while(true) //test loop
     {
         move();
@@ -43,41 +47,62 @@
     int error = 0;
     
     if(direction == 'B')
+    {   
+        debugLog << "Moving Backward" << endl;
         wheelDirection1 = wheelDirection2 = wheelDirection3 = wheelDirection4 = 0;
+    }
     else
+    {
+        debugLog << "Moving Forward" << endl;
         wheelDirection1 = wheelDirection2 = wheelDirection3 = wheelDirection4 = 1;
+    }
         
     do
     {
         sensors.read();
         sensors.lineDetect(sensorStates);
         
+        debugLog << "Updating Sensors" << endl;
+        debugLog << "    1   2   3   4   5   6   7   8" << endl;
+        debugLog << "----------------------------------" << endl;
+        for(int i = 0; i < 3; i++)
+        {
+            debugLog << i + 1 << " | ";
+            for(int j = 0; j < 8; j++)
+            {
+                debugLog << int(sensorStates[j][i]) << "   ";
+            }
+            debugLog << endl;
+        }
+        
         if(intersection())
         {
-            wheelSpeed1 = wheelSpeed3 = 1;
-            wheelSpeed2 = wheelSpeed4 = 1;
+            debugLog << "At Intersection" << endl;
+            wheelSpeed1 = wheelSpeed2 = wheelSpeed3 = wheelSpeed4 = MOTOR_SCALE;
         }
         else
         {
             error = calculateError();
+            debugLog << "Error Calculated : " << error << endl;
             
             if(error < 0)
             {
-                wheelSpeed1 = wheelSpeed3 = 1.0 - abs(error)*0.09;
-                wheelSpeed2 = wheelSpeed4 = 1;
+                wheelSpeed1 = wheelSpeed3 = (1.0 - abs(error)*CORRECTION_SCALE)*MOTOR_SCALE;
+                wheelSpeed2 = wheelSpeed4 = MOTOR_SCALE;
             }
             else
             {
-                wheelSpeed1 = wheelSpeed3 = 1;
-                wheelSpeed2 = wheelSpeed4 = 1.0 - error*0.09;
+                wheelSpeed1 = wheelSpeed3 = MOTOR_SCALE;
+                wheelSpeed2 = wheelSpeed4 = (1.0 - error*CORRECTION_SCALE)*MOTOR_SCALE;
             }
             
-            if(atLine)
-                atLine = false;
+            atLine = false;
         }       
     } while(!intersection() || atLine);
     
     wheelSpeed1 = wheelSpeed2 = wheelSpeed3 = wheelSpeed4 = 0;
+    debugLog << "Move Complete" << endl;
+    debugLog << "=====================================" << endl;
 }
 
 int DriveController::calculateError()
@@ -143,26 +168,45 @@
     {
         wheelDirection1 = wheelDirection3 = 0;
         wheelDirection2 = wheelDirection4 = 1;
+        debugLog << "Rotating Left" << endl;
     }
     else
     {
         wheelDirection1 = wheelDirection3 = 1;
         wheelDirection2 = wheelDirection3 = 0;
+        debugLog << "Rotating Right" << endl;
     }
     
-    wheelSpeed1 = wheelSpeed2 = wheelSpeed3 = wheelSpeed4 = 1;
+    wheelSpeed1 = wheelSpeed2 = wheelSpeed3 = wheelSpeed4 = MOTOR_SCALE;
     
     do
     {
         sensors.read();
         sensors.lineDetect(sensorStates);
         
+        debugLog << "Updating Sensors" << endl;
+        debugLog << "    1   2   3   4   5   6   7   8" << endl;
+        debugLog << "----------------------------------" << endl;
+        for(int i = 0; i < 3; i++)
+        {
+            debugLog << i + 1 << " | ";
+            for(int j = 0; j < 8; j++)
+            {
+                debugLog << int(sensorStates[j][i]) << "   ";
+            }
+            debugLog << endl;
+        }
+        
         if(!intersection())
             atLine = false;
             
     } while(!intersection() || atLine);
     
     wheelSpeed1 = wheelSpeed2 = wheelSpeed3 = wheelSpeed4 = 0;
+    
+    debugLog << "Rotate Complete" << endl;
+    debugLog << "=====================================" << endl;
+
 }
 
 void DriveController::getCommand()