Diff: LOCOMOTION.cpp

Revision:

26:5ae5ef623b72

Parent:

25:f3bf8351bbd4

--- a/LOCOMOTION.cpp	Tue Apr 05 02:30:40 2016 +0000
+++ b/LOCOMOTION.cpp	Wed Apr 06 03:54:29 2016 +0000
@@ -128,24 +128,36 @@
     return false;
 }
 
-void LOCOMOTION::setXstate(int *xCurrState, int *xTarget,bool xGood,bool angleGood,int *angleGoal)
+void LOCOMOTION::setXstate(int *xCurrState, int *xTarget,bool xGood,bool angleGood,int *angleGoal,int *angleTol)
 {
     if (*xCurrState==X_INCREASE|| *xCurrState==X_DECREASE) {
         if(angleGood && xGood) {
             *xCurrState=X_BACKWARDS;
+            wait(0.5);
 
         }
     }
 
-    if(*xCurrState==X_BACKWARDS) {
-        if(xGood && angleGood) {
-            if(*angleGoal==0) {
-                *xCurrState=X_INCREASE;
-            } else {
-                *xCurrState=X_DECREASE;
-            }
+    else if(xGood && angleGood && *xCurrState==X_BACKWARDS) {
+        if(*angleGoal==0) {
+            *xCurrState=ROTATE_1;
+            wait(0.5);
+        } else {
+            *xCurrState=ROTATE_2;
+            wait(0.5);
         }
     }
+
+    else if (xGood && angleGood) {
+        if (*xCurrState==ROTATE_1) {
+            *xCurrState=X_INCREASE;
+            wait(0.5);
+        } else {
+            *xCurrState=X_DECREASE;
+            wait(0.5);
+        }
+    }
+
     switch(*xCurrState) {
         case X_INCREASE:
             *angleGoal=180;
@@ -157,8 +169,8 @@
         case X_DECREASE:
             *angleGoal=0;
             *xTarget=X_NEAR_GOAL;
-            _m1dir=1;
-            _m2dir=1;
+            //_m1dir=1;
+            //_m2dir=1;
             break;
 
         case X_BACKWARDS:
@@ -167,12 +179,24 @@
             } else {
                 *xTarget=X_FAR_GOAL-BACKOFF;
             }
-            _m1dir=0;
-            _m2dir=0;
+            //_m1dir=0;
+            //_m2dir=0;
             break;
+
+        case ROTATE_1:
+            *xTarget=*xTarget;
+            *angleTol=2;
+            *angleGoal=180;
+            break;
+
+        case ROTATE_2:
+            *xTarget=*xTarget;
+            *angleTol=2;
+            *angleGoal=0;
+            break;
+
     }
 }
-
 void LOCOMOTION::setAngleTol(int *angleTol,bool yGood, bool xGood)
 {
     if (xGood||yGood) {
@@ -184,7 +208,7 @@
 
 void LOCOMOTION::setYgoal(bool xGood,bool angleGood,bool yGood,int *yTarget)
 {
-    if (xGood && angleGood && yGood) {
+    if (yGood) {
         *yTarget+=Y_INCREMENT;
     }
 }
@@ -193,5 +217,10 @@
 {
     *xGood=abs(xya.x-xGoal)<xErr;
     *yGood=abs(xya.y-yGoal)<yErr;
-    *angleGood=setAngle(angleGoal,xya.a,angleErr,ANGLE_TURN);    
+    *angleGood=setAngle(angleGoal,xya.a,angleErr,ANGLE_TURN);
+}
+
+void LOCOMOTION::mStop(void)
+{
+    setMotors(0);
 }
\ No newline at end of file