Revision 19:5832e34b7533, committed 2016-03-29

Comitter:

alaurens

Date:

Tue Mar 29 21:58:26 2016 +0000

Parent:

18:f9012e93edb8

Commit message:

everything is beautiful

Changed in this revision

--- 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

--- a/LOCOMOTION.h	Tue Mar 29 02:12:08 2016 +0000
+++ b/LOCOMOTION.h	Tue Mar 29 21:58:26 2016 +0000
@@ -6,6 +6,14 @@
 
 #define SPEED_TURN_MIN  0.20
 #define SPEED_TURN_MAX  0.65
+#define x_increase 1
+#define x_decrease 2
+#define x_backwards 3
+#define backoff 20
+#define xNearGoal 20
+#define xFarGoal 80
+#define yIncrement 20
+
 
 enum {
     ANGLE_TURN  = 0,
@@ -22,9 +30,14 @@
     PwmOut _m2b;
     DigitalOut _m1dir;
     DigitalOut _m2dir;
+    void setXstate(int *xCurrState, int *xTarget,bool xGood,bool angleGood,int *angleGoal);
+    void setAngleTol(int *angleTol,bool yGood, bool xGood);
+    void setYgoal(bool xGood,bool angleGood,bool yGood,int *yTarget);
+    void adjustY(int XcurrState,int Ytarget);
     bool setXPos(int target, int current, int error, int angle);
     bool setYPos(int target, int current, int error, int angle);
     bool setAngle(int target, int current, int error, int mode);
+    void check_xya(bool *xGood,bool *yGood,bool *angleGood, int xGoal,int angleGoal,int yGoal,LOCALIZE_xya xya,int xErr, int yErr,int angleErr);
     int wrap(int num);
 
 protected:

--- a/main.cpp	Tue Mar 29 02:12:08 2016 +0000
+++ b/main.cpp	Tue Mar 29 21:58:26 2016 +0000
@@ -22,8 +22,11 @@
 int target=20;
 int angle_error=2;
 bool xGood=false;
+bool yGood=false;
+bool angleGood=false;
+
 int angleTarget=0;
-
+int yTarget=30;
 void setTarget();
 void send();
 //void setAngle(int angle);
@@ -39,6 +42,7 @@
     while(1) {
         //loc.get_angle(&xya);
         loc.get_xy(&xya);
+        motion.check_xya(&xGood,&yGood,&angleGood,target,angleTarget,yTarget,angle_error)
         if(motion.setAngle(angleTarget,xya.a,angle_error,ANGLE_TURN)) {
             xGood = motion.setXPos(target,xya.x,2,angleTarget);
             if(motion.setYPos(130,xya.y,2,angleTarget)) {