Diff: LOCOMOTION.cpp

Revision:

29:e8ef4a2e628d

Parent:

28:65daccc10f31

Child:

30:116cd143fdf7

--- a/LOCOMOTION.cpp	Wed Apr 13 07:19:18 2016 +0000
+++ b/LOCOMOTION.cpp	Wed Apr 13 07:48:49 2016 +0000
@@ -32,9 +32,9 @@
 inline float LOCOMOTION::compG(float speed, bool dir, int angle)
 {
     if(dir)
-        return (cos((double)angle*M_PI/180.0)*GAIN_GRAVITY)*speed+speed;
+        return (sin((double)angle*M_PI/180.0)*GAIN_GRAVITY)*speed+speed;
     else
-        return -1*(cos((double)angle*M_PI/180.0)*GAIN_GRAVITY)*speed+speed;
+        return -1*(sin((double)angle*M_PI/180.0)*GAIN_GRAVITY)*speed+speed;
 }
  
 inline int LOCOMOTION::wrap(int num)
@@ -127,13 +127,13 @@
         default:
         case ANGLE_TURN:
             if(wrap(current+diff)>180+error) {
+                _m1dir=0;
+                _m2dir=1;
+                setMotors12(compG(s,_m1dir,current),compG(s,_m2dir,current));
+            } else if(wrap(current+diff)<180-error) {
                 _m1dir=1;
                 _m2dir=0;
                 setMotors12(compG(s,_m1dir,current),compG(s,_m2dir,current));
-            } else if(wrap(current+diff)<180-error) {
-                _m1dir=0;
-                _m2dir=1;
-                setMotors12(compG(s,_m1dir,current),compG(s,_m2dir,current));
             } else {
                 setMotors(0);
                 return true;