Diff: LOCOMOTION.cpp

Revision:

18:f9012e93edb8

Parent:

17:2f89826b5679

Child:

19:2dd81b864e14

--- a/LOCOMOTION.cpp	Tue Mar 29 01:11:09 2016 +0000
+++ b/LOCOMOTION.cpp	Tue Mar 29 02:12:08 2016 +0000
@@ -11,7 +11,7 @@
     _m2dir=0;
 }
 
-bool LOCOMOTION::setXPos(int target, int current, int error)
+bool LOCOMOTION::setXPos(int target, int current, int error, int angle)
 {
     //s = 0;
     if(abs(current-target)<=error)
@@ -19,15 +19,25 @@
     else
         s=((0.17-0.07)*abs(current-target)/FRAME_W)+0.07;
     if(current>target+error) {
-        _m1dir=1;
-        _m2dir=1;
+        if(angle==0) {
+            _m1dir=1;
+            _m2dir=1;
+        } else {
+            _m1dir=0;
+            _m2dir=0;
+        }
         _m1f=s;
         _m1b=s;
         _m2f=s;
         _m2b=s;
     } else if(current<target-error) {
-        _m1dir=0;
-        _m2dir=0;
+        if(angle==0) {
+            _m1dir=0;
+            _m2dir=0;
+        } else {
+            _m1dir=1;
+            _m2dir=1;
+        }
         _m1f=s;
         _m1b=s;
         _m2f=s;
@@ -43,7 +53,7 @@
     return false;
 }
 
-bool LOCOMOTION::setYPos(int target, int current, int error)
+bool LOCOMOTION::setYPos(int target, int current, int error, int angle)
 {
     //float s = 0;
     if(abs(current-target)<=error)
@@ -53,16 +63,26 @@
     if(current>target+error) {
         //_m1dir=1;
         //_m2dir=1;
-        _m1f=_m1f*(1+s);
-        _m1b=_m1b*(1+s);
+        if(angle==0) {
+            _m1f=_m1f*(1+s);
+            _m1b=_m1b*(1+s);
+        } else {
+            _m2f=_m2f*(1+s);
+            _m2b=_m2b*(1+s);
+        }
     } else if(current<target-error) {
         //_m1dir=0;
         //_m2dir=0;
-        _m2f=_m2f*(1+s);
-        _m2b=_m2b*(1+s);
+        if(angle==0) {
+            _m2f=_m2f*(1+s);
+            _m2b=_m2b*(1+s);
+        } else {
+            _m1f=_m1f*(1+s);
+            _m1b=_m1b*(1+s);
+        }
     } else {
         s=0;
-        
+
         return true;
     }
     return false;