RGB LEDs Control
Dependencies: mbed QEI PololuLedStrip
MotorControl.cpp@8:2a4298cbae3f, 2019-05-04 (annotated)
- Committer:
- whutsup
- Date:
- Sat May 04 18:51:43 2019 +0000
- Revision:
- 8:2a4298cbae3f
Lune RGB LEDs control;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
whutsup | 8:2a4298cbae3f | 1 | #include "mbed.h" |
whutsup | 8:2a4298cbae3f | 2 | #include "QEI.h" |
whutsup | 8:2a4298cbae3f | 3 | |
whutsup | 8:2a4298cbae3f | 4 | extern Serial bt; |
whutsup | 8:2a4298cbae3f | 5 | DigitalOut Enable(A0); |
whutsup | 8:2a4298cbae3f | 6 | DigitalOut M1(PA_1); |
whutsup | 8:2a4298cbae3f | 7 | DigitalOut M2(PF_1); |
whutsup | 8:2a4298cbae3f | 8 | |
whutsup | 8:2a4298cbae3f | 9 | |
whutsup | 8:2a4298cbae3f | 10 | extern QEI re; |
whutsup | 8:2a4298cbae3f | 11 | extern Ticker timer1; |
whutsup | 8:2a4298cbae3f | 12 | extern int targetDis; |
whutsup | 8:2a4298cbae3f | 13 | |
whutsup | 8:2a4298cbae3f | 14 | void MotorTest(int mode) |
whutsup | 8:2a4298cbae3f | 15 | { |
whutsup | 8:2a4298cbae3f | 16 | |
whutsup | 8:2a4298cbae3f | 17 | switch (mode) |
whutsup | 8:2a4298cbae3f | 18 | |
whutsup | 8:2a4298cbae3f | 19 | { |
whutsup | 8:2a4298cbae3f | 20 | case 0 : //stop |
whutsup | 8:2a4298cbae3f | 21 | |
whutsup | 8:2a4298cbae3f | 22 | Enable = 0; |
whutsup | 8:2a4298cbae3f | 23 | timer1.detach(); |
whutsup | 8:2a4298cbae3f | 24 | |
whutsup | 8:2a4298cbae3f | 25 | break; |
whutsup | 8:2a4298cbae3f | 26 | |
whutsup | 8:2a4298cbae3f | 27 | case 1 : // right |
whutsup | 8:2a4298cbae3f | 28 | |
whutsup | 8:2a4298cbae3f | 29 | Enable = 1; |
whutsup | 8:2a4298cbae3f | 30 | M1 = 1; |
whutsup | 8:2a4298cbae3f | 31 | M2 = 0; |
whutsup | 8:2a4298cbae3f | 32 | |
whutsup | 8:2a4298cbae3f | 33 | break; |
whutsup | 8:2a4298cbae3f | 34 | |
whutsup | 8:2a4298cbae3f | 35 | case 2 : // left |
whutsup | 8:2a4298cbae3f | 36 | |
whutsup | 8:2a4298cbae3f | 37 | Enable = 1; |
whutsup | 8:2a4298cbae3f | 38 | M1 = 0; |
whutsup | 8:2a4298cbae3f | 39 | M2 = 1; |
whutsup | 8:2a4298cbae3f | 40 | |
whutsup | 8:2a4298cbae3f | 41 | break; |
whutsup | 8:2a4298cbae3f | 42 | |
whutsup | 8:2a4298cbae3f | 43 | } |
whutsup | 8:2a4298cbae3f | 44 | |
whutsup | 8:2a4298cbae3f | 45 | |
whutsup | 8:2a4298cbae3f | 46 | } |
whutsup | 8:2a4298cbae3f | 47 | |
whutsup | 8:2a4298cbae3f | 48 | void ControlPos() |
whutsup | 8:2a4298cbae3f | 49 | { |
whutsup | 8:2a4298cbae3f | 50 | |
whutsup | 8:2a4298cbae3f | 51 | int pulse = re.getPulses(); |
whutsup | 8:2a4298cbae3f | 52 | int d = pulse/200; |
whutsup | 8:2a4298cbae3f | 53 | |
whutsup | 8:2a4298cbae3f | 54 | float ref_d = (targetDis-d); |
whutsup | 8:2a4298cbae3f | 55 | float abs_ref_d = abs(ref_d); |
whutsup | 8:2a4298cbae3f | 56 | // float s; |
whutsup | 8:2a4298cbae3f | 57 | // s = abs_ref_d; |
whutsup | 8:2a4298cbae3f | 58 | |
whutsup | 8:2a4298cbae3f | 59 | if(-0.8f > ref_d) |
whutsup | 8:2a4298cbae3f | 60 | { |
whutsup | 8:2a4298cbae3f | 61 | Enable = 1; |
whutsup | 8:2a4298cbae3f | 62 | M1 = 0; |
whutsup | 8:2a4298cbae3f | 63 | M2 = 1; //(s<3)?s/5:1; |
whutsup | 8:2a4298cbae3f | 64 | |
whutsup | 8:2a4298cbae3f | 65 | } |
whutsup | 8:2a4298cbae3f | 66 | |
whutsup | 8:2a4298cbae3f | 67 | else if ( 0.8f <ref_d) |
whutsup | 8:2a4298cbae3f | 68 | { |
whutsup | 8:2a4298cbae3f | 69 | |
whutsup | 8:2a4298cbae3f | 70 | Enable = 1; |
whutsup | 8:2a4298cbae3f | 71 | M1 = 1; //(s<3)?s/5:1; |
whutsup | 8:2a4298cbae3f | 72 | M2 = 0; |
whutsup | 8:2a4298cbae3f | 73 | |
whutsup | 8:2a4298cbae3f | 74 | } |
whutsup | 8:2a4298cbae3f | 75 | |
whutsup | 8:2a4298cbae3f | 76 | if (abs_ref_d < 0.8f) |
whutsup | 8:2a4298cbae3f | 77 | { |
whutsup | 8:2a4298cbae3f | 78 | |
whutsup | 8:2a4298cbae3f | 79 | Enable = 0; |
whutsup | 8:2a4298cbae3f | 80 | timer1.detach(); |
whutsup | 8:2a4298cbae3f | 81 | |
whutsup | 8:2a4298cbae3f | 82 | } |
whutsup | 8:2a4298cbae3f | 83 | } |