Diff: LOCOMOTION.cpp

Revision:

25:f3bf8351bbd4

Parent:

24:fb1f083ebd62

Child:

26:0ea6a550a99d

--- a/LOCOMOTION.cpp	Sun Apr 03 07:25:07 2016 +0000
+++ b/LOCOMOTION.cpp	Tue Apr 05 02:30:40 2016 +0000
@@ -4,7 +4,7 @@
     _en(en), _m1f(motor1F), _m1b(motor1B), _m2f(motor2F), _m2b(motor2B), _m1dir(forward1), _m2dir(forward2), _led1(led1), _led2(led2), _led3(led3), _led4(led4)
 {
     s=0;
-    stopMotors();
+    setMotors(0);
     _m1dir=0;
     _m2dir=0;
     _en=1;
@@ -13,12 +13,12 @@
     _led1=0;
 }
 
-inline void LOCOMOTION::stopMotors(void)
+inline void LOCOMOTION::setMotors(float x)
 {
-    _m1f=0;
-    _m1b=0;
-    _m2f=0;
-    _m2b=0;
+    _m1f=x;
+    _m1b=x;
+    _m2f=x;
+    _m2b=x;
 }
 
 
@@ -30,18 +30,17 @@
 void LOCOMOTION::eStop(void)
 {
     //Stop Motors
-    stopMotors();
+    setMotors(0);
     //Disable Motor Drivers
     _en=0;
 }
 
 bool LOCOMOTION::setXPos(int target, int current, int error, int angle)
 {
-    //s = 0;
     if(abs(current-target)<=error)
-        s=0.07;
+        s=SPEED_X_MIN;
     else
-        s=((0.17-0.07)*abs(current-target)/FRAME_W)+0.07;
+        s=((SPEED_X_MAX-SPEED_X_MIN)*abs(current-target)/FRAME_W)+SPEED_X_MIN;
     if(current>target+error) {
         if(angle==0) {
             _m1dir=1;
@@ -50,10 +49,7 @@
             _m1dir=0;
             _m2dir=0;
         }
-        _m1f=s;
-        _m1b=s;
-        _m2f=s;
-        _m2b=s;
+        setMotors(s);
     } else if(current<target-error) {
         if(angle==0) {
             _m1dir=0;
@@ -62,24 +58,20 @@
             _m1dir=1;
             _m2dir=1;
         }
-        _m1f=s;
-        _m1b=s;
-        _m2f=s;
-        _m2b=s;
+        setMotors(s);
     } else {
-        stopMotors();
+        setMotors(0);
         return true;
     }
     return false;
 }
 
-bool LOCOMOTION::setYPos(int target, int current, int error, int angle)
+bool LOCOMOTION::setYBias(int target, int current, int error, int angle)
 {
-    //float s = 0;
     if(abs(current-target)<=error)
-        s=0.50;
+        s=Y_BIAS_MIN;
     else
-        s=((1-0.50)*abs(current-target)/FRAME_H)+0.50;
+        s=((Y_BIAS_MAX-Y_BIAS_MIN)*abs(current-target)/FRAME_H)+Y_BIAS_MIN;
     if(current>target+error) {
         //_m1dir=1;
         //_m2dir=1;
@@ -102,7 +94,6 @@
         }
     } else {
         s=0;
-
         return true;
     }
     return false;
@@ -118,29 +109,89 @@
     else
         s=((SPEED_TURN_MAX-SPEED_TURN_MIN)*abs(wrap(current+diff)-180)/180)+SPEED_TURN_MIN;
     switch(mode) {
+        default:
         case ANGLE_TURN:
             if(wrap(current+diff)>180+error) {
                 _m1dir=1;
                 _m2dir=0;
-                _m1f=s;
-                _m1b=s;
-                _m2f=s;
-                _m2b=s;
+                setMotors(s);
             } else if(wrap(current+diff)<180-error) {
                 _m1dir=0;
                 _m2dir=1;
-                _m1f=s;
-                _m1b=s;
-                _m2f=s;
-                _m2b=s;
+                setMotors(s);
             } else {
-                stopMotors();
+                setMotors(0);
                 return true;
             }
             break;
-        case ANGLE_BIAS:
+    }
+    return false;
+}
+
+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=X_FAR_GOAL;
+            _m1dir=1;
+            _m2dir=1;
+            break;
+
+        case X_DECREASE:
+            *angleGoal=0;
+            *xTarget=X_NEAR_GOAL;
+            _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;
             break;
     }
-    return false;
+}
+
+void LOCOMOTION::setAngleTol(int *angleTol,bool yGood, bool xGood)
+{
+    if (xGood||yGood) {
+        *angleTol=2;
+    } else {
+        *angleTol=10;
+    }
+}
+
+void LOCOMOTION::setYgoal(bool xGood,bool angleGood,bool yGood,int *yTarget)
+{
+    if (xGood && angleGood && 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);    
 }
\ No newline at end of file