Diff: LOCOMOTION.cpp

Revision:

16:d6f15a13c3aa

Parent:

15:7729da55873a

Child:

17:2f89826b5679

--- a/LOCOMOTION.cpp	Thu Mar 24 07:20:51 2016 +0000
+++ b/LOCOMOTION.cpp	Sun Mar 27 02:39:07 2016 +0000
@@ -1,7 +1,7 @@
 #include "LOCOMOTION.h"
 
 LOCOMOTION::LOCOMOTION (PinName motor1F, PinName motor1B, PinName motor2F, PinName motor2B, PinName forward1, PinName forward2):
-    _m1f(motor1F), _m1b(motor2F), _m2f(motor2F), _m2b(motor2B), _m1dir(forward1), _m2dir(forward2)
+    _m1f(motor1F), _m1b(motor1B), _m2f(motor2F), _m2b(motor2B), _m1dir(forward1), _m2dir(forward2)
 {
     _m1f=0;
     _m1b=0;
@@ -20,21 +20,36 @@
         s=SPEED_TURN_MIN;
     else
         s=((SPEED_TURN_MAX-SPEED_TURN_MIN)*abs(wrap(current+diff)-180)/180)+SPEED_TURN_MIN;
-    if(wrap(current+diff)>180+error) {
-        
-    } else if(wrap(current+diff)<180-error) {
-        
-    } else {
-        s = 0;
-    }
     switch(mode) {
         case ANGLE_TURN:
-
+            if(wrap(current+diff)>180+error) {
+                _m1dir=1;
+                _m2dir=0;
+                _m1f=s;
+                _m1b=s;
+                _m2f=s;
+                _m2b=s;
+            } else if(wrap(current+diff)<180-error) {
+                _m1dir=0;
+                _m2dir=1;
+                _m1f=s;
+                _m1b=s;
+                _m2f=s;
+                _m2b=s;
+            } else {
+                s=0;
+                _m1f=s;
+                _m1b=s;
+                _m2f=s;
+                _m2b=s;
+                return true;
+            }
             break;
         case ANGLE_BIAS:
 
             break;
     }
+    return false;
 }
 
 int LOCOMOTION::wrap(int num)