DERPS
Dependencies: BMP280 BNO055_fusion PowerControl mbed
Fork of STRAIGHT_DRIVE_NO_SEP by
Diff: LOCOMOTION.cpp
- Revision:
- 35:68917796c30a
- Parent:
- 34:083073e54dbd
- Child:
- 36:dc69442c4c47
diff -r 083073e54dbd -r 68917796c30a LOCOMOTION.cpp --- 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; }