Diff: LOCOMOTION.cpp

Revision:

17:2f89826b5679

Parent:

16:d6f15a13c3aa

Child:

18:f9012e93edb8

--- a/LOCOMOTION.cpp	Sun Mar 27 02:39:07 2016 +0000
+++ b/LOCOMOTION.cpp	Tue Mar 29 01:11:09 2016 +0000
@@ -11,9 +11,66 @@
     _m2dir=0;
 }
 
+bool LOCOMOTION::setXPos(int target, int current, int error)
+{
+    //s = 0;
+    if(abs(current-target)<=error)
+        s=0.07;
+    else
+        s=((0.17-0.07)*abs(current-target)/FRAME_W)+0.07;
+    if(current>target+error) {
+        _m1dir=1;
+        _m2dir=1;
+        _m1f=s;
+        _m1b=s;
+        _m2f=s;
+        _m2b=s;
+    } else if(current<target-error) {
+        _m1dir=0;
+        _m2dir=0;
+        _m1f=s;
+        _m1b=s;
+        _m2f=s;
+        _m2b=s;
+    } else {
+        s=0;
+        _m1f=s;
+        _m1b=s;
+        _m2f=s;
+        _m2b=s;
+        return true;
+    }
+    return false;
+}
+
+bool LOCOMOTION::setYPos(int target, int current, int error)
+{
+    //float s = 0;
+    if(abs(current-target)<=error)
+        s=0.50;
+    else
+        s=((1-0.50)*abs(current-target)/FRAME_H)+0.50;
+    if(current>target+error) {
+        //_m1dir=1;
+        //_m2dir=1;
+        _m1f=_m1f*(1+s);
+        _m1b=_m1b*(1+s);
+    } else if(current<target-error) {
+        //_m1dir=0;
+        //_m2dir=0;
+        _m2f=_m2f*(1+s);
+        _m2b=_m2b*(1+s);
+    } else {
+        s=0;
+        
+        return true;
+    }
+    return false;
+}
+
 bool LOCOMOTION::setAngle(int target, int current, int error, int mode)
 {
-    float s = 0;
+    s = 0;
     int diff = 0;
     diff = 180-wrap(target);
     if(abs(wrap(current+diff)-180)<=error)
@@ -55,30 +112,4 @@
 int LOCOMOTION::wrap(int num)
 {
     return num%360;
-}
-/*
-void setAngle(int angle)
-{
-    float s = 0;
-    int diff = 0;
-    diff = 180-wrap(angle);
-    if(abs(wrap(xya.a+diff)-180)<=5)
-        s=SPEED_TURN_MIN;
-    else
-        s=((SPEED_TURN_MAX-SPEED_TURN_MIN)*abs(wrap(xya.a+diff)-180)/180)+SPEED_TURN_MIN;
-    motor1F=s;
-    motor1B=s;
-    motor2F=s;
-    motor2B=s;
-    if(wrap(xya.a+diff)>180+2) {
-        dir1=1;
-        dir2=0;
-    } else if(wrap(xya.a+diff)<180-2) {
-        dir1=0;
-        dir2=1;
-    } else {
-        motor1F=0;
-        motor1B=0;
-        motor2F=0;
-        motor2B=0;
-    }*/
+}
\ No newline at end of file