Diff: LOCOMOTION.cpp

Revision:

26:0ea6a550a99d

Parent:

25:f3bf8351bbd4

Child:

27:fb1298d35bd1

--- a/LOCOMOTION.cpp	Tue Apr 05 02:30:40 2016 +0000
+++ b/LOCOMOTION.cpp	Wed Apr 06 04:04:00 2016 +0000
@@ -1,5 +1,5 @@
 #include "LOCOMOTION.h"
-
+ 
 LOCOMOTION::LOCOMOTION (PinName en, PinName motor1F, PinName motor1B, PinName motor2F, PinName motor2B, PinName forward1, PinName forward2, DigitalOut& led1, DigitalOut& led2, DigitalOut& led3, DigitalOut& led4):
     _en(en), _m1f(motor1F), _m1b(motor1B), _m2f(motor2F), _m2b(motor2B), _m1dir(forward1), _m2dir(forward2), _led1(led1), _led2(led2), _led3(led3), _led4(led4)
 {
@@ -12,7 +12,7 @@
     wait(0.01);
     _led1=0;
 }
-
+ 
 inline void LOCOMOTION::setMotors(float x)
 {
     _m1f=x;
@@ -20,13 +20,13 @@
     _m2f=x;
     _m2b=x;
 }
-
-
+ 
+ 
 inline int LOCOMOTION::wrap(int num)
 {
     return num%360;
 }
-
+ 
 void LOCOMOTION::eStop(void)
 {
     //Stop Motors
@@ -34,7 +34,7 @@
     //Disable Motor Drivers
     _en=0;
 }
-
+ 
 bool LOCOMOTION::setXPos(int target, int current, int error, int angle)
 {
     if(abs(current-target)<=error)
@@ -65,7 +65,7 @@
     }
     return false;
 }
-
+ 
 bool LOCOMOTION::setYBias(int target, int current, int error, int angle)
 {
     if(abs(current-target)<=error)
@@ -98,7 +98,7 @@
     }
     return false;
 }
-
+ 
 bool LOCOMOTION::setAngle(int target, int current, int error, int mode)
 {
     s = 0;
@@ -127,25 +127,37 @@
     }
     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;
@@ -153,26 +165,38 @@
             _m1dir=1;
             _m2dir=1;
             break;
-
+ 
         case X_DECREASE:
             *angleGoal=0;
             *xTarget=X_NEAR_GOAL;
-            _m1dir=1;
-            _m2dir=1;
+            //_m1dir=1;
+            //_m2dir=1;
             break;
-
+ 
         case X_BACKWARDS:
             if (*angleGoal==0) {
                 *xTarget=X_NEAR_GOAL+BACKOFF;
             } 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) {
@@ -181,17 +205,22 @@
         *angleTol=10;
     }
 }
-
+ 
 void LOCOMOTION::setYgoal(bool xGood,bool angleGood,bool yGood,int *yTarget)
 {
-    if (xGood && angleGood && yGood) {
+    if (yGood) {
         *yTarget+=Y_INCREMENT;
     }
 }
-
+ 
 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=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