Diff: DriveController.cpp

Revision:

13:aa6f64c73271

Parent:

12:edcae0f36e9c

Child:

14:7a4cf2ed2499

--- a/DriveController.cpp	Fri Nov 14 18:51:04 2014 +0000
+++ b/DriveController.cpp	Fri Apr 03 23:02:38 2015 +0000
@@ -3,8 +3,7 @@
 #define MOTOR_SCALE 0.4
 #define CORRECTION_SCALE 0.05
 
-DriveController::DriveController() : i2c(PTC2, PTC1), wheelSpeed1(PTB0), wheelSpeed2(PTB1), wheelSpeed3(PTB2),
-    wheelSpeed4(PTB3), wheelDirection1(PTE20), wheelDirection2(PTE21), wheelDirection3(PTE22), wheelDirection4(PTE23),
+DriveController::DriveController() : i2c(PTC2, PTC1), treadSpeed1(PTB0), treadSpeed2(PTB1), treadDirection1(PTE20), treadDirection2(PTE21),
     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)
 {
@@ -17,10 +16,8 @@
     
     wait(10);
     
-    wheelSpeed1.period_us(50);  //setting pwm period for all wheels to 20khz
-    wheelSpeed2.period_us(50);
-    wheelSpeed3.period_us(50);
-    wheelSpeed4.period_us(50);
+    treadSpeed1.period_us(50);  //setting pwm period for all wheels to 20khz
+    treadSpeed2.period_us(50);
     
     //spinTest();
     
@@ -42,13 +39,13 @@
         
         switch(command)
         {
-            case 0: move(); //move forward
+            case 'F' : move(); //move forward
                 break;
-            case 1: move('B'); //move backward
+            case 'B' : move('B'); //move backward
                 break;
-            case 2: rotate('R'); //rotate right
+            case 'R' : rotate('R'); //rotate right
                 break;
-            case 3: rotate('L'); //rotate left
+            case 'L' : rotate('L'); //rotate left
         }
             
         sendComplete();
@@ -62,11 +59,11 @@
     
     if(direction == 'B')
     {  
-        wheelDirection1 = wheelDirection2 = wheelDirection3 = wheelDirection4 = 0;
+        treadDirection1 = treadDirection2 = 0;
     }
     else
     {
-        wheelDirection1 = wheelDirection2 = wheelDirection3 = wheelDirection4 = 1;
+        treadDirection1 = treadDirection2 = 1;
     }
         
     do
@@ -75,7 +72,7 @@
         
         if(intersection())
         {
-            wheelSpeed1 = wheelSpeed2 = wheelSpeed3 = wheelSpeed4 = MOTOR_SCALE;
+            treadSpeed1 = treadSpeed2 = MOTOR_SCALE;
         }
         else
         {
@@ -83,20 +80,20 @@
             
             if(error < 0)
             {
-                wheelSpeed1 = wheelSpeed3 = (1.0 - fabs(error))*MOTOR_SCALE;
-                wheelSpeed2 = wheelSpeed4 = MOTOR_SCALE;
+                treadSpeed1 = (1.0 - fabs(error))*MOTOR_SCALE;
+                treadSpeed2 = MOTOR_SCALE;
             }
             else
             {
-                wheelSpeed1 = wheelSpeed3 = MOTOR_SCALE;
-                wheelSpeed2 = wheelSpeed4 = (1.0 - error)*MOTOR_SCALE;
+                treadSpeed1 = MOTOR_SCALE;
+                treadSpeed2 = (1.0 - error)*MOTOR_SCALE;
             }
             
             atLine = false;
         }       
     } while(!intersection() || atLine);
     
-    wheelSpeed1 = wheelSpeed2 = wheelSpeed3 = wheelSpeed4 = 0;
+    treadSpeed1 = treadSpeed2 = 0;
 }
 
 double DriveController::calculateError()
@@ -106,7 +103,7 @@
     
     for(int i = 7; i >= 0; i--)
     {
-            bin += sensorStates[i][1]<<(7-i);
+            bin += sensorStates[i][0]<<(7-i);
     }
     
     switch(bin)
@@ -152,8 +149,8 @@
     
     // 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]);
+    return (sensorStates[0][0]&&sensorStates[1][0]&&sensorStates[2][0]&&sensorStates[3][0]) 
+            || (sensorStates[4][0]&&sensorStates[5][0]&&sensorStates[6][0]&&sensorStates[7][0]);
 }
 
 void DriveController::rotate(char direction)
@@ -162,16 +159,16 @@
     
     if(direction == 'L')
     {
-        wheelDirection1 = wheelDirection3 = 0;
-        wheelDirection2 = wheelDirection4 = 1;
+        treadDirection1 = 0;
+        treadDirection2 = 1;
     }
     else
     {
-        wheelDirection1 = wheelDirection3 = 0;
-        wheelDirection2 = wheelDirection4 = 1;
+        treadDirection1 = 0;
+        treadDirection2 = 1;
     }
     
-    wheelSpeed1 = wheelSpeed2 = wheelSpeed3 = wheelSpeed4 = MOTOR_SCALE;
+    treadSpeed1 = treadSpeed2 = MOTOR_SCALE;
     
     /*do
     {
@@ -182,16 +179,15 @@
             
     } while(!intersection() || atLine);*/
     
-    wait(0.6);
+    wait(0.85);
     
-    wheelSpeed1 = wheelSpeed2 = wheelSpeed3 = wheelSpeed4 = 0;
+    treadSpeed1 = treadSpeed2 = 0;
 }
 
 void DriveController::getCommand()
 {
     bool received = false;
     int status;
-    int msg;
     
     while(!received)
     {
@@ -199,14 +195,10 @@
         
         if(status == 2) //if status is WriteGeneral
         {
-            msg = i2c.read();
+            command = i2c.read();
             received = true;
         }
     }
-    
-    command = msg & 7;
-    edge = msg & 24;
-        
 }
 
 void DriveController::sendComplete()
@@ -216,10 +208,10 @@
 
 void DriveController::spinTest()
 {
-    wheelDirection1 = wheelDirection3 = 1;
-    wheelDirection2 = wheelDirection4 = 0;
+    treadDirection1 = 1;
+    treadDirection2 = 0;
     
-    wheelSpeed1 = wheelSpeed2 = wheelSpeed3 = wheelSpeed4 = MOTOR_SCALE;
+    treadSpeed1 = treadSpeed2 = MOTOR_SCALE;
     
     while(true)
         wait(1);