RGB LEDs Control
Dependencies: mbed QEI PololuLedStrip
Diff: MotorControl.cpp
- Revision:
- 8:2a4298cbae3f
diff -r 050f086f6f5c -r 2a4298cbae3f MotorControl.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MotorControl.cpp Sat May 04 18:51:43 2019 +0000 @@ -0,0 +1,83 @@ +#include "mbed.h" +#include "QEI.h" + +extern Serial bt; +DigitalOut Enable(A0); +DigitalOut M1(PA_1); +DigitalOut M2(PF_1); + + +extern QEI re; +extern Ticker timer1; +extern int targetDis; + +void MotorTest(int mode) +{ + + switch (mode) + + { + case 0 : //stop + + Enable = 0; + timer1.detach(); + + break; + + case 1 : // right + + Enable = 1; + M1 = 1; + M2 = 0; + + break; + + case 2 : // left + + Enable = 1; + M1 = 0; + M2 = 1; + + break; + + } + + +} + +void ControlPos() +{ + + int pulse = re.getPulses(); + int d = pulse/200; + + float ref_d = (targetDis-d); + float abs_ref_d = abs(ref_d); +// float s; +// s = abs_ref_d; + + if(-0.8f > ref_d) + { + Enable = 1; + M1 = 0; + M2 = 1; //(s<3)?s/5:1; + + } + + else if ( 0.8f <ref_d) + { + + Enable = 1; + M1 = 1; //(s<3)?s/5:1; + M2 = 0; + + } + + if (abs_ref_d < 0.8f) + { + + Enable = 0; + timer1.detach(); + + } +} \ No newline at end of file