Diff: LOCOMOTION.cpp

Revision:

34:083073e54dbd

Parent:

33:baf24c59affc

--- a/LOCOMOTION.cpp	Wed Apr 20 16:11:08 2016 +0000
+++ b/LOCOMOTION.cpp	Wed Apr 20 18:25:43 2016 +0000
@@ -57,16 +57,16 @@
     else
         s=((SPEED_X_MAX-SPEED_X_MIN)*abs(current-target)/FRAME_W)+SPEED_X_MIN;
     if(current>target+error) {
-        if(angle==0) {
+        if(angle==0+TILTZZ) {
+            _m1dir=0;
+            _m2dir=0;
+        } else {
             _m1dir=1;
             _m2dir=1;
-        } else {
-            _m1dir=0;
-            _m2dir=0;
         }
         setMotors(s);
     } else if(current<target-error) {
-        if(angle==0) {
+        if(angle==0+TILTZZ) {
             _m1dir=0;
             _m2dir=0;
         } else {
@@ -124,7 +124,6 @@
     else
         s=((SPEED_TURN_MAX-SPEED_TURN_MIN)*abs(wrap(current+diff)-180)/180)+SPEED_TURN_MIN;
     switch(mode) {
-        default:
         case ANGLE_TURN:
             if(wrap(current+diff)>180+error) {
                 _m1dir=0;
@@ -167,14 +166,14 @@
     switch(*xCurrState) {
 
         case X_INCREASE:
-            *angleGoal=0;
+            *angleGoal=0+TILTZZ;
             _m1dir=0;
             _m2dir=0;
             *xTarget=X_FAR_GOAL;
             break;
 
         case X_DECREASE:
-            *angleGoal=0;
+            *angleGoal=0-TILTZZ;
             _m1dir=1;
             _m2dir=1;
             *xTarget=X_NEAR_GOAL;