DECS @UNIST / Mbed 2 deprecated 1_LUNE_ver_2_white_copy

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?

UserRevisionLine numberNew 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 }