DECS @UNIST / Mbed 2 deprecated Anybaro_ver_7

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

MotorControl.cpp@4:bf4ad2079096, 2018-12-21 (annotated)

Committer:
whutsup
Date:
Fri Dec 21 20:39:24 2018 +0000
Revision:
4:bf4ad2079096
Child:
5:6bb52b2a79bf
updated 181222

Who changed what in which revision?

UserRevisionLine numberNew contents of line
whutsup 4:bf4ad2079096 1 #include "mbed.h"
whutsup 4:bf4ad2079096 2
whutsup 4:bf4ad2079096 3 extern Serial bt;
whutsup 4:bf4ad2079096 4 extern Serial pc;
whutsup 4:bf4ad2079096 5
whutsup 4:bf4ad2079096 6 DigitalOut enable(PB_12);
whutsup 4:bf4ad2079096 7 PwmOut p1(PB_13);
whutsup 4:bf4ad2079096 8 PwmOut p2(PB_14);
whutsup 4:bf4ad2079096 9
whutsup 4:bf4ad2079096 10 extern InterruptIn up;
whutsup 4:bf4ad2079096 11 extern InterruptIn down;
whutsup 4:bf4ad2079096 12
whutsup 4:bf4ad2079096 13 extern AnalogIn disSensor;
whutsup 4:bf4ad2079096 14
whutsup 4:bf4ad2079096 15 extern Serial bt;
whutsup 4:bf4ad2079096 16 extern Ticker timer1;
whutsup 4:bf4ad2079096 17 extern Ticker timer3;
whutsup 4:bf4ad2079096 18
whutsup 4:bf4ad2079096 19 extern int targetDis;
whutsup 4:bf4ad2079096 20 extern int milLoop;
whutsup 4:bf4ad2079096 21 extern int onewayNum;
whutsup 4:bf4ad2079096 22 extern bool mkOn;
whutsup 4:bf4ad2079096 23
whutsup 4:bf4ad2079096 24
whutsup 4:bf4ad2079096 25 #define parA 347.44f // parmeter of equation (values of distance sensor)
whutsup 4:bf4ad2079096 26 #define parB -1*481.16f
whutsup 4:bf4ad2079096 27 #define parC 155.42f
whutsup 4:bf4ad2079096 28
whutsup 4:bf4ad2079096 29
whutsup 4:bf4ad2079096 30 void MotorButton()
whutsup 4:bf4ad2079096 31 {
whutsup 4:bf4ad2079096 32
whutsup 4:bf4ad2079096 33 milLoop = 0;
whutsup 4:bf4ad2079096 34 targetDis = 0;
whutsup 4:bf4ad2079096 35 onewayNum = 0;
whutsup 4:bf4ad2079096 36 timer3.detach();
whutsup 4:bf4ad2079096 37
whutsup 4:bf4ad2079096 38 int a = up.read();
whutsup 4:bf4ad2079096 39 int b = down.read();
whutsup 4:bf4ad2079096 40 int c = a*2 + b;
whutsup 4:bf4ad2079096 41
whutsup 4:bf4ad2079096 42 switch(c)
whutsup 4:bf4ad2079096 43 {
whutsup 4:bf4ad2079096 44 case 0 : //stop
whutsup 4:bf4ad2079096 45
whutsup 4:bf4ad2079096 46 enable = 0;
whutsup 4:bf4ad2079096 47 p1 = 0;
whutsup 4:bf4ad2079096 48 p2 = 0;
whutsup 4:bf4ad2079096 49 break;
whutsup 4:bf4ad2079096 50
whutsup 4:bf4ad2079096 51 case 1 : // up
whutsup 4:bf4ad2079096 52
whutsup 4:bf4ad2079096 53 enable = 1;
whutsup 4:bf4ad2079096 54 p1 = 0;
whutsup 4:bf4ad2079096 55 p2 = 1;
whutsup 4:bf4ad2079096 56
whutsup 4:bf4ad2079096 57 break;
whutsup 4:bf4ad2079096 58
whutsup 4:bf4ad2079096 59 case 2 : // down
whutsup 4:bf4ad2079096 60
whutsup 4:bf4ad2079096 61 enable = 1;
whutsup 4:bf4ad2079096 62 p1 = 1;
whutsup 4:bf4ad2079096 63 p2 = 0;
whutsup 4:bf4ad2079096 64
whutsup 4:bf4ad2079096 65 break;
whutsup 4:bf4ad2079096 66
whutsup 4:bf4ad2079096 67 case 3 : // stop
whutsup 4:bf4ad2079096 68
whutsup 4:bf4ad2079096 69 enable = 0;
whutsup 4:bf4ad2079096 70 p1 = 0;
whutsup 4:bf4ad2079096 71 p2 = 0;
whutsup 4:bf4ad2079096 72
whutsup 4:bf4ad2079096 73 break;
whutsup 4:bf4ad2079096 74
whutsup 4:bf4ad2079096 75 }
whutsup 4:bf4ad2079096 76
whutsup 4:bf4ad2079096 77
whutsup 4:bf4ad2079096 78
whutsup 4:bf4ad2079096 79 }
whutsup 4:bf4ad2079096 80
whutsup 4:bf4ad2079096 81 void MotorTest(int mode)
whutsup 4:bf4ad2079096 82 {
whutsup 4:bf4ad2079096 83
whutsup 4:bf4ad2079096 84 timer3.detach();
whutsup 4:bf4ad2079096 85
whutsup 4:bf4ad2079096 86 switch (mode)
whutsup 4:bf4ad2079096 87
whutsup 4:bf4ad2079096 88 {
whutsup 4:bf4ad2079096 89 case 0 : //stop
whutsup 4:bf4ad2079096 90
whutsup 4:bf4ad2079096 91 enable = 0;
whutsup 4:bf4ad2079096 92
whutsup 4:bf4ad2079096 93 break;
whutsup 4:bf4ad2079096 94
whutsup 4:bf4ad2079096 95 case 1 : // up
whutsup 4:bf4ad2079096 96
whutsup 4:bf4ad2079096 97 enable = 1;
whutsup 4:bf4ad2079096 98 p1 = 0;
whutsup 4:bf4ad2079096 99 p2 = 1;
whutsup 4:bf4ad2079096 100
whutsup 4:bf4ad2079096 101 break;
whutsup 4:bf4ad2079096 102
whutsup 4:bf4ad2079096 103 case 2 : // down
whutsup 4:bf4ad2079096 104
whutsup 4:bf4ad2079096 105 enable = 1;
whutsup 4:bf4ad2079096 106 p1 = 1;
whutsup 4:bf4ad2079096 107 p2 = 0;
whutsup 4:bf4ad2079096 108
whutsup 4:bf4ad2079096 109 break;
whutsup 4:bf4ad2079096 110
whutsup 4:bf4ad2079096 111 }
whutsup 4:bf4ad2079096 112
whutsup 4:bf4ad2079096 113
whutsup 4:bf4ad2079096 114 }
whutsup 4:bf4ad2079096 115
whutsup 4:bf4ad2079096 116 void ControlAng()
whutsup 4:bf4ad2079096 117 {
whutsup 4:bf4ad2079096 118
whutsup 4:bf4ad2079096 119 float d1 = disSensor.read();
whutsup 4:bf4ad2079096 120 float d2 = parA*d1*d1+parB*d1+parC;
whutsup 4:bf4ad2079096 121 float ref_d = targetDis-d2;
whutsup 4:bf4ad2079096 122
whutsup 4:bf4ad2079096 123 if(ref_d > 0.9f)
whutsup 4:bf4ad2079096 124 {
whutsup 4:bf4ad2079096 125 enable = 1;
whutsup 4:bf4ad2079096 126 p1 = 0;
whutsup 4:bf4ad2079096 127 p2 = 1;
whutsup 4:bf4ad2079096 128
whutsup 4:bf4ad2079096 129 }
whutsup 4:bf4ad2079096 130
whutsup 4:bf4ad2079096 131 else if(ref_d < -0.9f)
whutsup 4:bf4ad2079096 132 {
whutsup 4:bf4ad2079096 133
whutsup 4:bf4ad2079096 134 enable = 1;
whutsup 4:bf4ad2079096 135 p1 = 1;
whutsup 4:bf4ad2079096 136 p2 = 0;
whutsup 4:bf4ad2079096 137
whutsup 4:bf4ad2079096 138 }
whutsup 4:bf4ad2079096 139
whutsup 4:bf4ad2079096 140 else
whutsup 4:bf4ad2079096 141 {
whutsup 4:bf4ad2079096 142 enable = 0;
whutsup 4:bf4ad2079096 143 timer1.detach();
whutsup 4:bf4ad2079096 144
whutsup 4:bf4ad2079096 145 }
whutsup 4:bf4ad2079096 146 }
whutsup 4:bf4ad2079096 147
whutsup 4:bf4ad2079096 148
whutsup 4:bf4ad2079096 149 void MkAction()
whutsup 4:bf4ad2079096 150 {
whutsup 4:bf4ad2079096 151
whutsup 4:bf4ad2079096 152
whutsup 4:bf4ad2079096 153 float d1 = disSensor.read();
whutsup 4:bf4ad2079096 154 float d2 = parA*d1*d1+parB*d1+parC;
whutsup 4:bf4ad2079096 155
whutsup 4:bf4ad2079096 156 static float curTarget;
whutsup 4:bf4ad2079096 157 float s;
whutsup 4:bf4ad2079096 158 float abs_error;
whutsup 4:bf4ad2079096 159
whutsup 4:bf4ad2079096 160 curTarget = (onewayNum%2==0)? targetDis: 1.2f ; // initial angle
whutsup 4:bf4ad2079096 161
whutsup 4:bf4ad2079096 162 s = (curTarget-d2)/targetDis;
whutsup 4:bf4ad2079096 163 abs_error = abs(curTarget-d2);
whutsup 4:bf4ad2079096 164
whutsup 4:bf4ad2079096 165
whutsup 4:bf4ad2079096 166 if(s>=0)
whutsup 4:bf4ad2079096 167 {
whutsup 4:bf4ad2079096 168 enable = 1;
whutsup 4:bf4ad2079096 169 p1 = 0;
whutsup 4:bf4ad2079096 170 p2 = 1; //(s>0.7f)?s:0.7f;
whutsup 4:bf4ad2079096 171 }
whutsup 4:bf4ad2079096 172
whutsup 4:bf4ad2079096 173 else
whutsup 4:bf4ad2079096 174 {
whutsup 4:bf4ad2079096 175 enable = 1;
whutsup 4:bf4ad2079096 176 p1 = 1; //(s<-0.7f)?(-1*s):0.7f;
whutsup 4:bf4ad2079096 177 p2 = 0;
whutsup 4:bf4ad2079096 178 }
whutsup 4:bf4ad2079096 179
whutsup 4:bf4ad2079096 180 if (abs_error <0.5f)
whutsup 4:bf4ad2079096 181 {
whutsup 4:bf4ad2079096 182
whutsup 4:bf4ad2079096 183 onewayNum = onewayNum +1;
whutsup 4:bf4ad2079096 184
whutsup 4:bf4ad2079096 185 if(onewayNum >=milLoop*2)
whutsup 4:bf4ad2079096 186 {
whutsup 4:bf4ad2079096 187 onewayNum = 0;
whutsup 4:bf4ad2079096 188 milLoop = 0;
whutsup 4:bf4ad2079096 189 enable = 0;
whutsup 4:bf4ad2079096 190 timer3.detach();
whutsup 4:bf4ad2079096 191 }
whutsup 4:bf4ad2079096 192 }
whutsup 4:bf4ad2079096 193
whutsup 4:bf4ad2079096 194
whutsup 4:bf4ad2079096 195 }
whutsup 4:bf4ad2079096 196
whutsup 4:bf4ad2079096 197 void MkActionManual()
whutsup 4:bf4ad2079096 198 {
whutsup 4:bf4ad2079096 199
whutsup 4:bf4ad2079096 200 float d1 = disSensor.read();
whutsup 4:bf4ad2079096 201 float d2 = parA*d1*d1+parB*d1+parC;
whutsup 4:bf4ad2079096 202
whutsup 4:bf4ad2079096 203 enable = 0;
whutsup 4:bf4ad2079096 204 timer3.detach();
whutsup 4:bf4ad2079096 205 onewayNum=0;
whutsup 4:bf4ad2079096 206 targetDis = d2;
whutsup 4:bf4ad2079096 207 milLoop = 99;
whutsup 4:bf4ad2079096 208 timer3.attach(&MkAction,0.1);
whutsup 4:bf4ad2079096 209
whutsup 4:bf4ad2079096 210 }