Localization
Dependencies: BNO055_fusion mbed
Diff: LOCOMOTION.cpp
- Revision:
- 15:7729da55873a
- Parent:
- 6:0602a9e8118b
- Child:
- 16:d6f15a13c3aa
--- a/LOCOMOTION.cpp Thu Mar 24 03:54:29 2016 +0000 +++ b/LOCOMOTION.cpp Thu Mar 24 07:20:51 2016 +0000 @@ -1,5 +1,69 @@ #include "LOCOMOTION.h" + +LOCOMOTION::LOCOMOTION (PinName motor1F, PinName motor1B, PinName motor2F, PinName motor2B, PinName forward1, PinName forward2): + _m1f(motor1F), _m1b(motor2F), _m2f(motor2F), _m2b(motor2B), _m1dir(forward1), _m2dir(forward2) +{ + _m1f=0; + _m1b=0; + _m2f=0; + _m2b=0; + _m1dir=0; + _m2dir=0; +} + +bool LOCOMOTION::setAngle(int target, int current, int error, int mode) +{ + float s = 0; + int diff = 0; + diff = 180-wrap(target); + if(abs(wrap(current+diff)-180)<=error) + s=SPEED_TURN_MIN; + else + s=((SPEED_TURN_MAX-SPEED_TURN_MIN)*abs(wrap(current+diff)-180)/180)+SPEED_TURN_MIN; + if(wrap(current+diff)>180+error) { + + } else if(wrap(current+diff)<180-error) { + + } else { + s = 0; + } + switch(mode) { + case ANGLE_TURN: + + break; + case ANGLE_BIAS: + + break; + } +} + +int LOCOMOTION::wrap(int num) +{ + return num%360; +} /* -LOCOMOTION::LOCOMOTION (PwmOut& motor1F, PwmOut& motor1B,PwmOut& motor2F, PwmOut& motor2B, DigitalOut& forward1, DigitalOut& forward2): - _speed.m1f(motor1F), _speed.m1b(motor1B), _speed.m2f(motor2F), _speed.m2b(motor2B){} - */ \ No newline at end of file +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; + }*/