Mekatronics Team G
Dependencies: BNO055_fusion PowerControl mbed BMP280
Fork of DEMO3 by
LOCOMOTION.cpp
- Committer:
- 12104404
- Date:
- 2016-04-03
- Revision:
- 24:fb1f083ebd62
- Parent:
- 23:455f7da3dd7a
- Child:
- 25:f3bf8351bbd4
File content as of revision 24:fb1f083ebd62:
#include "LOCOMOTION.h" LOCOMOTION::LOCOMOTION (PinName en, PinName motor1F, PinName motor1B, PinName motor2F, PinName motor2B, PinName forward1, PinName forward2, DigitalOut& led1, DigitalOut& led2, DigitalOut& led3, DigitalOut& led4): _en(en), _m1f(motor1F), _m1b(motor1B), _m2f(motor2F), _m2b(motor2B), _m1dir(forward1), _m2dir(forward2), _led1(led1), _led2(led2), _led3(led3), _led4(led4) { s=0; stopMotors(); _m1dir=0; _m2dir=0; _en=1; _led1=1; wait(0.01); _led1=0; } inline void LOCOMOTION::stopMotors(void) { _m1f=0; _m1b=0; _m2f=0; _m2b=0; } inline int LOCOMOTION::wrap(int num) { return num%360; } void LOCOMOTION::eStop(void) { //Stop Motors stopMotors(); //Disable Motor Drivers _en=0; } bool LOCOMOTION::setXPos(int target, int current, int error, int angle) { //s = 0; if(abs(current-target)<=error) s=0.07; else s=((0.17-0.07)*abs(current-target)/FRAME_W)+0.07; if(current>target+error) { if(angle==0) { _m1dir=1; _m2dir=1; } else { _m1dir=0; _m2dir=0; } _m1f=s; _m1b=s; _m2f=s; _m2b=s; } else if(current<target-error) { if(angle==0) { _m1dir=0; _m2dir=0; } else { _m1dir=1; _m2dir=1; } _m1f=s; _m1b=s; _m2f=s; _m2b=s; } else { stopMotors(); return true; } return false; } bool LOCOMOTION::setYPos(int target, int current, int error, int angle) { //float s = 0; if(abs(current-target)<=error) s=0.50; else s=((1-0.50)*abs(current-target)/FRAME_H)+0.50; if(current>target+error) { //_m1dir=1; //_m2dir=1; if(angle==0) { _m1f=_m1f*(1+s); _m1b=_m1b*(1+s); } else { _m2f=_m2f*(1+s); _m2b=_m2b*(1+s); } } else if(current<target-error) { //_m1dir=0; //_m2dir=0; if(angle==0) { _m2f=_m2f*(1+s); _m2b=_m2b*(1+s); } else { _m1f=_m1f*(1+s); _m1b=_m1b*(1+s); } } else { s=0; return true; } return false; } bool LOCOMOTION::setAngle(int target, int current, int error, int mode) { 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 { stopMotors(); return true; } break; case ANGLE_BIAS: break; } return false; }