Diff: DriveController.cpp

Revision:

12:edcae0f36e9c

Parent:

11:5d20ce95e137

Child:

13:aa6f64c73271

--- a/DriveController.cpp	Mon Nov 03 16:53:44 2014 +0000
+++ b/DriveController.cpp	Fri Nov 14 18:51:04 2014 +0000
@@ -1,7 +1,7 @@
 #include "DriveController.h"
 
-#define MOTOR_SCALE 0.25
-#define CORRECTION_SCALE 0.01
+#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),
@@ -22,6 +22,8 @@
     wheelSpeed3.period_us(50);
     wheelSpeed4.period_us(50);
     
+    //spinTest();
+    
     while(true) //test loop
     {
         move();
@@ -56,7 +58,7 @@
 void DriveController::move(char direction)
 {
     bool atLine = true; //tracks whether the bot is still at the starting intersection
-    int error = 0;
+    double error = 0;
     
     if(direction == 'B')
     {  
@@ -81,13 +83,13 @@
             
             if(error < 0)
             {
-                //wheelSpeed1 = wheelSpeed3 = (1.0 - abs(error)*CORRECTION_SCALE)*MOTOR_SCALE;
-                wheelSpeed1 = wheelSpeed2 = wheelSpeed3 = wheelSpeed4 = MOTOR_SCALE;
+                wheelSpeed1 = wheelSpeed3 = (1.0 - fabs(error))*MOTOR_SCALE;
+                wheelSpeed2 = wheelSpeed4 = MOTOR_SCALE;
             }
             else
             {
-                wheelSpeed1 = wheelSpeed2 = wheelSpeed3 = wheelSpeed4 = MOTOR_SCALE;
-                //wheelSpeed2 = wheelSpeed4 = (1.0 - error*CORRECTION_SCALE)*MOTOR_SCALE;
+                wheelSpeed1 = wheelSpeed3 = MOTOR_SCALE;
+                wheelSpeed2 = wheelSpeed4 = (1.0 - error)*MOTOR_SCALE;
             }
             
             atLine = false;
@@ -97,9 +99,9 @@
     wheelSpeed1 = wheelSpeed2 = wheelSpeed3 = wheelSpeed4 = 0;
 }
 
-int DriveController::calculateError()
+double DriveController::calculateError()
 {
-    int error;
+    double error;
     int bin = 0;
     
     for(int i = 7; i >= 0; i--)
@@ -109,40 +111,40 @@
     
     switch(bin)
     {
-        case 1: error = 7;
+        case 1: error = 6;
             break;
-        case 3: error = 6;
+        case 3: error = 5;
             break;
-        case 2: error = 5;
+        case 2: error = 4;
             break;
-        case 6: error = 4;
+        case 6: error = 3;
             break;
-        case 4: error = 3;
+        case 4: error = 2;
             break;
-        case 12: error = 2;
+        case 12: error = 1;
             break;
-        case 8: error = 1;
+        case 8: error = 0;
             break;
         case 24: error = 0;
             break;
-        case 16: error = -1;
+        case 16: error = 0;
             break;
-        case 48: error = -2;
+        case 48: error = -1;
             break;  
-        case 32: error = -3;
+        case 32: error = -2;
             break;
-        case 96: error = -4;
+        case 96: error = -3;
             break;
-        case 64: error = -5;
+        case 64: error = -4;
             break;
-        case 192: error = -6;
+        case 192: error = -5;
             break;
-        case 128: error = -7;
+        case 128: error = -6;
             break;
         default: error = 0;
     }
                 
-    return error;
+    return error*CORRECTION_SCALE;
 }
 
 bool DriveController::intersection()
@@ -171,14 +173,16 @@
     
     wheelSpeed1 = wheelSpeed2 = wheelSpeed3 = wheelSpeed4 = MOTOR_SCALE;
     
-    do
+    /*do
     {
         sensors.lineDetect(sensorStates);
         
         if(!intersection())
             atLine = false;
             
-    } while(!intersection() || atLine);
+    } while(!intersection() || atLine);*/
+    
+    wait(0.6);
     
     wheelSpeed1 = wheelSpeed2 = wheelSpeed3 = wheelSpeed4 = 0;
 }
@@ -209,3 +213,14 @@
 {
     i2c.write(1);
 }
+
+void DriveController::spinTest()
+{
+    wheelDirection1 = wheelDirection3 = 1;
+    wheelDirection2 = wheelDirection4 = 0;
+    
+    wheelSpeed1 = wheelSpeed2 = wheelSpeed3 = wheelSpeed4 = MOTOR_SCALE;
+    
+    while(true)
+        wait(1);
+}