Diff: LOCOMOTION.cpp

Revision:

31:69caabc10879

Parent:

30:116cd143fdf7

Child:

33:baf24c59affc

--- a/LOCOMOTION.cpp	Tue Apr 19 02:04:10 2016 +0000
+++ b/LOCOMOTION.cpp	Wed Apr 20 08:21:21 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;
@@ -36,12 +36,12 @@
     else
         return -1*(sin((double)angle*M_PI/180.0)*GAIN_GRAVITY)*speed+speed;
 }
- 
+
 inline int LOCOMOTION::wrap(int num)
 {
     return num%360;
 }
- 
+
 void LOCOMOTION::eStop(void)
 {
     //Stop Motors
@@ -49,7 +49,7 @@
     //Disable Motor Drivers
     _en=0;
 }
- 
+
 bool LOCOMOTION::setXPos(int target, int current, int error, int angle)
 {
     if(abs(current-target)<=error)
@@ -80,13 +80,13 @@
     }
     return false;
 }
- 
+
 bool LOCOMOTION::setYBias(int target, int current, int error, int angle)
 {
     if(abs(current-target)<=error)
         s=Y_BIAS_MIN;
     else
-        s=((Y_BIAS_MAX-Y_BIAS_MIN)*abs(current-target)/FRAME_H)+Y_BIAS_MIN;
+        s=((Y_BIAS_MAX-Y_BIAS_MIN)*abs(current-target)/FRAME_HE)+Y_BIAS_MIN;
     if(current>target+error) {
         //_m1dir=1;
         //_m2dir=1;
@@ -113,7 +113,7 @@
     }
     return false;
 }
- 
+
 bool LOCOMOTION::setAngle(int target, int current, int error, int mode)
 {
     s = 0;
@@ -144,74 +144,42 @@
     }
     return false;
 }
- 
-void LOCOMOTION::setXstate(int *xCurrState, int *xTarget,bool xGood,bool angleGood,int *angleGoal,int *angleTol)
+
+void LOCOMOTION::setXstate(int *xCurrState, int *xTarget,bool xGood,bool angleGood,int *angleGoal,int *angleTol,int yTarget)
 {
-    if (*xCurrState==X_INCREASE|| *xCurrState==X_DECREASE) {
-        if(angleGood && xGood) {
-            *xCurrState=X_BACKWARDS;
-            wait(0.5);
- 
-        }
+    if (yTarget>=FRAME_HE-ROB_SIZE/2 && xGood) {
+        *xCurrState=X_STOP;
     }
- 
-    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 {
+    if (*xCurrState==X_INCREASE) {
+        if(angleGood && xGood) {
             *xCurrState=X_DECREASE;
             wait(0.5);
         }
     }
- 
+    if (*xCurrState==X_DECREASE) {
+        if(angleGood && xGood) {
+            *xCurrState=X_INCREASE;
+            wait(0.5);
+        }
+
+    }
+
     switch(*xCurrState) {
+
         case X_INCREASE:
             *angleGoal=180;
-            *xTarget=X_FAR_GOAL;
             _m1dir=1;
             _m2dir=1;
-            break;
- 
-        case X_DECREASE:
-            *angleGoal=0;
-            *xTarget=X_NEAR_GOAL;
-            //_m1dir=1;
-            //_m2dir=1;
+            *xTarget=X_FAR_GOAL;
             break;
- 
-        case X_BACKWARDS:
-            if (*angleGoal==0) {
-                *xTarget=X_NEAR_GOAL+BACKOFF;
-            } else {
-                *xTarget=X_FAR_GOAL-BACKOFF;
-            }
-            //_m1dir=0;
-            //_m2dir=0;
+
+        case X_DECREASE:
+            *angleGoal=180;
+            _m1dir=0;
+            _m2dir=0;
+            *xTarget=X_NEAR_GOAL;
             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)
@@ -222,21 +190,24 @@
         *angleTol=10;
     }
 }
- 
+
 void LOCOMOTION::setYgoal(bool xGood,bool angleGood,bool yGood,int *yTarget)
 {
-    if (yGood) {
+    if (yGood && xGood) {
         *yTarget+=Y_INCREMENT;
     }
+    if (*yTarget>(FRAME_HE-ROB_SIZE/2)) {
+        *yTarget=FRAME_HE-ROB_SIZE/2;
+    }
 }
- 
+
 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);
 }
- 
+
 void LOCOMOTION::mStop(void)
 {
     setMotors(0);
@@ -245,7 +216,7 @@
 {
     _m1dir=1;
     _m2dir=1;
-    setMotors12(compG(0.2,_m1dir,angle),compG(0.2,_m2dir,angle)); 
+    setMotors12(compG(0.2,_m1dir,angle),compG(0.2,_m2dir,angle));
 }
 
 void LOCOMOTION::moveB(void)