Diff: LOCOMOTION.cpp

Revision:

20:1da786e205eb

Parent:

19:5832e34b7533

diff -r 5832e34b7533 -r 1da786e205eb LOCOMOTION.cpp
--- a/LOCOMOTION.cpp	Tue Mar 29 21:58:26 2016 +0000
+++ b/LOCOMOTION.cpp	Tue Mar 29 22:49:05 2016 +0000
@@ -61,8 +61,6 @@
     else
         s=((1-0.50)*abs(current-target)/FRAME_H)+0.50;
     if(current>target+error) {
-        //_m1dir=1;
-        //_m2dir=1;
         if(angle==0) {
             _m1f=_m1f*(1+s);
             _m1b=_m1b*(1+s);
@@ -71,8 +69,6 @@
             _m2b=_m2b*(1+s);
         }
     } else if(current<target-error) {
-        //_m1dir=0;
-        //_m2dir=0;
         if(angle==0) {
             _m2f=_m2f*(1+s);
             _m2b=_m2b*(1+s);
@@ -135,42 +131,42 @@
 }
 void LOCOMOTION::setXstate(int *xCurrState, int *xTarget,bool xGood,bool angleGood,int *angleGoal)
 {
-    if (*xCurrState==x_increase|| *xCurrState==x_decrease) {
+    if (*xCurrState==X_INCREASE|| *xCurrState==X_DECREASE) {
         if(angleGood && xGood) {
-            *xCurrState=x_backwards;
+            *xCurrState=X_BACKWARDS;
 
         }
     }
 
-    if(*xCurrState==x_backwards) {
+    if(*xCurrState==X_BACKWARDS) {
         if(xGood && angleGood) {
             if(*angleGoal==0) {
-                *xCurrState=x_increase;
+                *xCurrState=X_INCREASE;
             } else {
-                *xCurrState=x_decrease;
+                *xCurrState=X_DECREASE;
             }
         }
     }
     switch(*xCurrState) {
-        case x_increase:
+        case X_INCREASE:
             *angleGoal=180;
-            *xTarget=xFarGoal;
+            *xTarget=X_FAR_GOAL;
             _m1dir=1;
             _m2dir=1;
             break;
 
-        case x_decrease:
+        case X_DECREASE:
             *angleGoal=0;
-            *xTarget=xNearGoal;
+            *xTarget=X_NEAR_GOAL;
             _m1dir=1;
             _m2dir=1;
             break;
 
-        case x_backwards:
+        case X_BACKWARDS:
             if (*angleGoal==0) {
-                *xTarget=xNearGoal+backoff;
+                *xTarget=X_NEAR_GOAL+BACKOFF;
             } else {
-                *xTarget=xNearGoal-backoff;
+                *xTarget=X_FAR_GOAL-BACKOFF;
             }
             _m1dir=0;
             _m2dir=0;
@@ -180,9 +176,7 @@
 
 void LOCOMOTION::setAngleTol(int *angleTol,bool yGood, bool xGood)
 {
-    if (xGood) {
-        *angleTol=2;
-    } else if(yGood) {
+    if (xGood||yGood) {
         *angleTol=2;
     } else {
         *angleTol=10;
@@ -192,7 +186,7 @@
 void LOCOMOTION::setYgoal(bool xGood,bool angleGood,bool yGood,int *yTarget)
 {
     if (xGood && angleGood && yGood) {
-        *yTarget+=yIncrement;
+        *yTarget+=Y_INCREMENT;
     }
 }