version2

Dependencies:   BNO055_fusion mbed

Fork of DEMO2 by Antoine Laurens

Files at this revision

API Documentation at this revision

Comitter:
alaurens
Date:
Tue Mar 29 22:49:05 2016 +0000
Parent:
19:5832e34b7533
Commit message:
should theoretically work

Changed in this revision

LOCOMOTION.cpp Show annotated file Show diff for this revision Revisions of this file
LOCOMOTION.h Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
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;
     }
 }
 
diff -r 5832e34b7533 -r 1da786e205eb LOCOMOTION.h
--- 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 {
diff -r 5832e34b7533 -r 1da786e205eb main.cpp
--- 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