Diff: LOCOMOTION.cpp

Revision:

35:68917796c30a

Parent:

34:083073e54dbd

Child:

36:dc69442c4c47

--- a/LOCOMOTION.cpp	Wed Apr 20 18:25:43 2016 +0000
+++ b/LOCOMOTION.cpp	Sun Apr 24 22:37:07 2016 +0000
@@ -50,14 +50,18 @@
     _en=0;
 }
 
-bool LOCOMOTION::setXPos(int target, int current, int error, int angle)
+bool LOCOMOTION::setXPos(int target, int current, int error, int angle, int curr_angle)
 {
-    if(abs(current-target)<=error)
+    /*if(abs(current-target)<=error)
         s=SPEED_X_MIN;
     else
+        s=((SPEED_X_MAX-SPEED_X_MIN)*abs(current-target)/FRAME_W)+SPEED_X_MIN;*/
+    if(abs(current-target)<25)
         s=((SPEED_X_MAX-SPEED_X_MIN)*abs(current-target)/FRAME_W)+SPEED_X_MIN;
+    else
+        s=SPEED_X_MAX;
     if(current>target+error) {
-        if(angle==0+TILTZZ) {
+        if(angle==0) {
             _m1dir=0;
             _m2dir=0;
         } else {
@@ -66,7 +70,7 @@
         }
         setMotors(s);
     } else if(current<target-error) {
-        if(angle==0+TILTZZ) {
+        if(angle==0) {
             _m1dir=0;
             _m2dir=0;
         } else {
@@ -78,6 +82,7 @@
         setMotors(0);
         return true;
     }
+    //setMotors12(compG(s,~_m1dir,curr_angle),compG(s,~_m2dir,curr_angle));
     return false;
 }
 
@@ -116,30 +121,50 @@
 
 bool LOCOMOTION::setAngle(int target, int current, int error, int mode)
 {
-    s = 0;
+    float a = 0;
     int diff = 0;
     diff = 180-wrap(target);
     if(abs(wrap(current+diff)-180)<=error)
-        s=SPEED_TURN_MIN;
+        a=SPEED_TURN_MIN;
     else
-        s=((SPEED_TURN_MAX-SPEED_TURN_MIN)*abs(wrap(current+diff)-180)/180)+SPEED_TURN_MIN;
+        a=((SPEED_TURN_MAX-SPEED_TURN_MIN)*abs(wrap(current+diff)-180)/180)+SPEED_TURN_MIN;
     switch(mode) {
         case ANGLE_TURN:
             if(wrap(current+diff)>180+error) {
                 _m1dir=0;
                 _m2dir=1;
-                setMotors12(compG(s,_m1dir,current),compG(s,_m2dir,current));
+                setMotors12(compG(a,_m1dir,current),compG(a,_m2dir,current));
                 //_m2dir=0;
             } else if(wrap(current+diff)<180-error) {
                 _m1dir=1;
                 _m2dir=0;
-                setMotors12(compG(s,_m1dir,current),compG(s,_m2dir,current));
+                setMotors12(compG(a,_m1dir,current),compG(a,_m2dir,current));
                 //_m2dir=1;
             } else {
                 setMotors(0);
                 return true;
             }
             break;
+        case ANGLE_BIAS:
+            if(wrap(current+diff)>180+error) {
+                //_m1dir=0;
+                //_m2dir=1;
+                setMotors12(s+a,s*0.6);
+                //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);
+                //setMotors12(compG(s,_m1dir,current),compG(s,_m2dir,current));
+                //_m2dir=1;
+            } else {
+                //setMotors(0);
+                return true;
+            }
+            
+            
+            break;
     }
     return false;
 }