Diff: LOCOMOTION.cpp

Revision:

19:5832e34b7533

Parent:

18:f9012e93edb8

Child:

20:1da786e205eb

--- a/LOCOMOTION.cpp	Tue Mar 29 02:12:08 2016 +0000
+++ b/LOCOMOTION.cpp	Tue Mar 29 21:58:26 2016 +0000
@@ -132,4 +132,73 @@
 int LOCOMOTION::wrap(int num)
 {
     return num%360;
+}
+void LOCOMOTION::setXstate(int *xCurrState, int *xTarget,bool xGood,bool angleGood,int *angleGoal)
+{
+    if (*xCurrState==x_increase|| *xCurrState==x_decrease) {
+        if(angleGood && xGood) {
+            *xCurrState=x_backwards;
+
+        }
+    }
+
+    if(*xCurrState==x_backwards) {
+        if(xGood && angleGood) {
+            if(*angleGoal==0) {
+                *xCurrState=x_increase;
+            } else {
+                *xCurrState=x_decrease;
+            }
+        }
+    }
+    switch(*xCurrState) {
+        case x_increase:
+            *angleGoal=180;
+            *xTarget=xFarGoal;
+            _m1dir=1;
+            _m2dir=1;
+            break;
+
+        case x_decrease:
+            *angleGoal=0;
+            *xTarget=xNearGoal;
+            _m1dir=1;
+            _m2dir=1;
+            break;
+
+        case x_backwards:
+            if (*angleGoal==0) {
+                *xTarget=xNearGoal+backoff;
+            } else {
+                *xTarget=xNearGoal-backoff;
+            }
+            _m1dir=0;
+            _m2dir=0;
+            break;
+    }
+}
+
+void LOCOMOTION::setAngleTol(int *angleTol,bool yGood, bool xGood)
+{
+    if (xGood) {
+        *angleTol=2;
+    } else if(yGood) {
+        *angleTol=2;
+    } else {
+        *angleTol=10;
+    }
+}
+
+void LOCOMOTION::setYgoal(bool xGood,bool angleGood,bool yGood,int *yTarget)
+{
+    if (xGood && angleGood && yGood) {
+        *yTarget+=yIncrement;
+    }
+}
+
+void LOCOMOTION::check_xya(bool *xGood,bool *yGood,bool *angleGood, int xGoal,int angleGoal,int yGoal,LOCALIZE_xya xya,int xErr, int yErr,int angleErr)
+{
+    *xGood=abs(xya.x-xGoal)<xErr;
+    *yGood=abs(xya.y-yGoal)<yErr;
+    *angleGood=abs(xya.a-angleGoal)<yErr;    
 }
\ No newline at end of file