Revision 20:1da786e205eb, committed 2016-03-29

Comitter:

alaurens

Date:

Tue Mar 29 22:49:05 2016 +0000

Parent:

19:5832e34b7533

Commit message:

should theoretically work

Changed in this revision

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

--- a/LOCOMOTION.h	Tue Mar 29 21:58:26 2016 +0000
+++ b/LOCOMOTION.h	Tue Mar 29 22:49:05 2016 +0000
@@ -6,13 +6,13 @@
 
 #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
+#define X_INCREASE 1
+#define X_DECREASE 2
+#define X_BACKWARDS 3
+#define BACKOFF 20
+#define X_NEAR_GOAL 20
+#define X_FAR_GOAL 80
+#define Y_INCREMENT 20
 
 
 enum {

--- a/main.cpp	Tue Mar 29 21:58:26 2016 +0000
+++ b/main.cpp	Tue Mar 29 22:49:05 2016 +0000
@@ -19,15 +19,15 @@
 Ticker t;
 Ticker tTarget;
 bool flag=false;
-int target=20;
+int xTarget=20;
 int angle_error=2;
 bool xGood=false;
 bool yGood=false;
 bool angleGood=false;
-
+int xState=X_INCREASE;
 int angleTarget=0;
 int yTarget=30;
-void setTarget();
+//void setTarget();
 void send();
 //void setAngle(int angle);
 int wrap(int a);
@@ -42,23 +42,23 @@
     while(1) {
         //loc.get_angle(&xya);
         loc.get_xy(&xya);
-        motion.check_xya(&xGood,&yGood,&angleGood,target,angleTarget,yTarget,angle_error)
+        motion.check_xya(&xGood,&yGood,&angleGood,xTarget,angleTarget,yTarget,xya,2,2,angle_error);
+        
+        motion.setXstate(&xState,&xTarget,xGood,angleGood,&angleTarget);
+        motion.setAngleTol(&angle_error,yGood,xGood);
+        motion.setYgoal(xGood,angleGood,yGood,&yTarget);
         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)) {
-                angle_error=2;
-            } else if(xGood) {
-                target=target==20?80:20;
-                angleTarget=angleTarget==0?180:0;
-                angle_error=2;
-            } else
-                angle_error=10;
+            motion.setXPos(xTarget,xya.x,2,angleTarget);
+            motion.setYPos(yTarget,xya.y,2,angleTarget);
+
         }
+
         //pc.printf("X: %3d\tY: %3d\tP: %3d\n",xya.x,xya.y,xya.a);
         wdt.kick();
     }
 }
 
+
 void send()
 {
     pc.printf("%c%c%c%c\n",(char)xya.x,(char)xya.y,xya.a/10,xya.a%10);
@@ -66,5 +66,5 @@
 
 void setTarget()
 {
-    target=target==20?80:20;
+    xTarget=xTarget==20?80:20;
 }
\ No newline at end of file