Diff: LOCOMOTION.cpp

Revision:

36:dc69442c4c47

Parent:

35:68917796c30a

Child:

37:e8a6ea255c09

--- a/LOCOMOTION.cpp	Sun Apr 24 22:37:07 2016 +0000
+++ b/LOCOMOTION.cpp	Mon Apr 25 17:47:51 2016 +0000
@@ -50,7 +50,7 @@
     _en=0;
 }
 
-bool LOCOMOTION::setXPos(int target, int current, int error, int angle, int curr_angle)
+bool LOCOMOTION::setXPos(int target, int current, int error, int angle)
 {
     /*if(abs(current-target)<=error)
         s=SPEED_X_MIN;
@@ -62,11 +62,11 @@
         s=SPEED_X_MAX;
     if(current>target+error) {
         if(angle==0) {
+            _m1dir=1;
+            _m2dir=1;
+        } else {
             _m1dir=0;
             _m2dir=0;
-        } else {
-            _m1dir=1;
-            _m2dir=1;
         }
         setMotors(s);
     } else if(current<target-error) {
@@ -82,7 +82,6 @@
         setMotors(0);
         return true;
     }
-    //setMotors12(compG(s,~_m1dir,curr_angle),compG(s,~_m2dir,curr_angle));
     return false;
 }
 
@@ -147,23 +146,25 @@
             break;
         case ANGLE_BIAS:
             if(wrap(current+diff)>180+error) {
-                //_m1dir=0;
-                //_m2dir=1;
-                setMotors12(s+a,s*0.6);
+                if(_m1dir==0)
+                    setMotors12(s+(a*0.75),s);
+                else
+                    setMotors12(s,s+(a*0.75));
                 //setMotors12(compG(s,_m1dir,current),compG(s,_m2dir,current));
                 //_m2dir=0;
             } else if(wrap(current+diff)<180-error) {
                 //_m1dir=1;
                 //_m2dir=0;
-                setMotors12(s,s+a*0.1);
+                if(_m1dir==0)
+                    setMotors12(s,s+(a*0.75));
+                else
+                    setMotors12(s+(a*0.75),s);
                 //setMotors12(compG(s,_m1dir,current),compG(s,_m2dir,current));
                 //_m2dir=1;
             } else {
                 //setMotors(0);
                 return true;
-            }
-            
-            
+            }         
             break;
     }
     return false;