Diff: DriveController.cpp

Revision:

16:b49db5c8e16c

Parent:

14:7a4cf2ed2499

Child:

17:5046b27f5441

diff -r 7a4cf2ed2499 -r b49db5c8e16c DriveController.cpp
--- a/DriveController.cpp	Wed Apr 15 14:40:08 2015 +0000
+++ b/DriveController.cpp	Sat Apr 18 02:50:24 2015 +0000
@@ -8,63 +8,27 @@
 
 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)
+    PTC16, PTC17, PTD1, PTD0, PTD5, PTD13), orient(NORTH)
 {
     
 }
 
-void DriveController::go()
+void DriveController::move(typename ::abs_direction direction)
 {
-    sensors.setThreshold();
-    
-    wait(10);
-    
-    treadSpeed1.period_us(50);  //setting pwm period for all wheels to 20khz
-    treadSpeed2.period_us(50);
-    
-    //spinTest();
-    
-    while(true) //test loop
-    {
-        move();
-        wait(0.5);
-        move();
-        wait(0.5);
-        rotate('R');
-        wait(0.5);
-        rotate('R');
-        wait(0.5);
-    }
-    
-    /*while(true)
-    {
-        getCommand();
-        
-        switch(command)
-        {
-            case 'F' : move(); //move forward
-                break;
-            case 'B' : move('B'); //move backward
-                break;
-            case 'R' : rotate('R'); //rotate right
-                break;
-            case 'L' : rotate('L'); //rotate left
-        }
-            
-        sendComplete();
-    }*/    
-}
-
-void DriveController::move(char direction)
-{
+    int diff;
     bool atLine = true; //tracks whether the bot is still at the starting intersection
     double error = 0;
     
-    if(direction == 'B')
+    diff=direction-orient;
+    
+    if(abs(diff)==3){
+        diff/=-3;
+    
+    if(diff == 2)//back
     {  
         treadDirection1 = treadDirection2 = 0;
     }
-    else
+    else if(direction == 0)//forward
     {
         treadDirection1 = treadDirection2 = 1;
     }
@@ -156,22 +120,32 @@
             || (sensorStates[4][0]&&sensorStates[5][0]&&sensorStates[6][0]&&sensorStates[7][0]);
 }
 
-void DriveController::rotate(char direction)
+void DriveController::rotate(typename ::abs_direction direction)
 {
+    int diff;
     bool atLine = true;
     
-    if(direction == 'L')
-    {
-        treadDirection1 = 0;
-        treadDirection2 = 1;
+    diff=direction-orient
+    
+    if(abs(diff)==3){
+        diff/=-3;
     }
-    else
-    {
-        treadDirection1 = 0;
-        treadDirection2 = 1;
-    }
-    
-    treadSpeed1 = treadSpeed2 = MOTOR_SCALE;
+    do{
+        if(diff < 0)
+        {
+            treadDirection1 = 0;
+            treadDirection2 = 1;
+            diff++;
+        }
+        else if(diff > 0)
+        {
+            treadDirection1 = 0;
+            treadDirection2 = 1;
+            diff--;
+        }
+        
+        treadSpeed1 = treadSpeed2 = MOTOR_SCALE;
+    }while(diff!=0);
     
     /*do
     {
@@ -188,28 +162,6 @@
     treadSpeed1 = treadSpeed2 = 0;
 }
 
-void DriveController::getCommand()
-{
-    bool received = false;
-    int status;
-    
-    while(!received)
-    {
-        status = i2c.receive();
-        
-        if(status == 2) //if status is WriteGeneral
-        {
-            command = i2c.read();
-            received = true;
-        }
-    }
-}
-
-void DriveController::sendComplete()
-{
-    i2c.write(1);
-}
-
 void DriveController::spinTest()
 {
     treadDirection1 = 1;
@@ -220,3 +172,8 @@
     while(true)
         wait(1);
 }
+
+typename ::abs_direction getOrient()
+{
+    return orient;
+}
\ No newline at end of file