![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
added
Dependencies: BNO055_fusion PowerControl mbed
Fork of TEAM_G_FLOW_RIDA by
LOCOMOTION.cpp
- Committer:
- 12104404
- Date:
- 2016-03-27
- Revision:
- 16:d6f15a13c3aa
- Parent:
- 15:7729da55873a
- Child:
- 17:2f89826b5679
File content as of revision 16:d6f15a13c3aa:
#include "LOCOMOTION.h" LOCOMOTION::LOCOMOTION (PinName motor1F, PinName motor1B, PinName motor2F, PinName motor2B, PinName forward1, PinName forward2): _m1f(motor1F), _m1b(motor1B), _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; switch(mode) { case ANGLE_TURN: if(wrap(current+diff)>180+error) { _m1dir=1; _m2dir=0; _m1f=s; _m1b=s; _m2f=s; _m2b=s; } else if(wrap(current+diff)<180-error) { _m1dir=0; _m2dir=1; _m1f=s; _m1b=s; _m2f=s; _m2b=s; } else { s=0; _m1f=s; _m1b=s; _m2f=s; _m2b=s; return true; } break; case ANGLE_BIAS: break; } return false; } int LOCOMOTION::wrap(int num) { return num%360; } /* 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; }*/