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@19:67584cb64b9c, 2019-08-22 (annotated)

Committer:
Bhoney
Date:
Thu Aug 22 08:36:15 2019 +0000
Revision:
19:67584cb64b9c
Parent:
15:e5e34512a00e 
QUALIFYING_MODE is added (Qtimer, Qtime, Qflag, Qfunc for btn)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Bhoney 19:67584cb64b9c 1 #include "mbed.h"
whutsup 4:bf4ad2079096 2
Bhoney 19:67584cb64b9c 3 extern Serial bt;
Bhoney 19:67584cb64b9c 4 //extern Serial pc;
Bhoney 19:67584cb64b9c 5
whutsup 4:bf4ad2079096 6
Bhoney 19:67584cb64b9c 7 extern DigitalOut enable;
Bhoney 19:67584cb64b9c 8 extern DigitalOut nreset;
Bhoney 19:67584cb64b9c 9 extern PwmOut p1;
Bhoney 19:67584cb64b9c 10 extern PwmOut p2;
whutsup 4:bf4ad2079096 11
Bhoney 19:67584cb64b9c 12 extern InterruptIn up;
Bhoney 19:67584cb64b9c 13 extern InterruptIn down;
whutsup 4:bf4ad2079096 14
Bhoney 19:67584cb64b9c 15 extern AnalogIn disSensor;
whutsup 4:bf4ad2079096 16
Bhoney 19:67584cb64b9c 17 extern Serial bt;
Bhoney 19:67584cb64b9c 18 extern Ticker timer1;
Bhoney 19:67584cb64b9c 19 extern Ticker timer3;
whutsup 4:bf4ad2079096 20
whutsup 4:bf4ad2079096 21 extern int targetDis;
whutsup 4:bf4ad2079096 22 extern int milLoop;
whutsup 4:bf4ad2079096 23 extern int onewayNum;
whutsup 4:bf4ad2079096 24 extern bool mkOn;
whutsup 8:a435e7aa7a02 25 extern float errorPrevious;
whutsup 4:bf4ad2079096 26
Bhoney 19:67584cb64b9c 27 extern float parA;
Bhoney 19:67584cb64b9c 28 extern float parB;
Bhoney 19:67584cb64b9c 29 extern float parC;
Bhoney 19:67584cb64b9c 30
Bhoney 19:67584cb64b9c 31 extern bool nFaultFlag;
Bhoney 19:67584cb64b9c 32
Bhoney 19:67584cb64b9c 33 #ifdef QUALIFYING_MODE
whutsup 4:bf4ad2079096 34
Bhoney 19:67584cb64b9c 35 extern Timer Qtimer; // timer for qualifying test
Bhoney 19:67584cb64b9c 36 extern int Qtime; // Qtime stores result from Qtimer
Bhoney 19:67584cb64b9c 37 extern bool Qflag; // Qflag rises when result is stored
whutsup 13:5601fedf0e12 38
Bhoney 19:67584cb64b9c 39 void Qfunc(){
Bhoney 19:67584cb64b9c 40 Qtimer.stop();
Bhoney 19:67584cb64b9c 41 Qtime = Qtimer.read_us();
Bhoney 19:67584cb64b9c 42 Qflag = true;
Bhoney 19:67584cb64b9c 43 }
Bhoney 19:67584cb64b9c 44 #endif
whutsup 13:5601fedf0e12 45
whutsup 4:bf4ad2079096 46 void MotorButton()
whutsup 4:bf4ad2079096 47 {
whutsup 8:a435e7aa7a02 48 mkOn=0;
whutsup 4:bf4ad2079096 49 milLoop = 0;
whutsup 4:bf4ad2079096 50 onewayNum = 0;
whutsup 8:a435e7aa7a02 51 timer3.detach();
whutsup 8:a435e7aa7a02 52
whutsup 8:a435e7aa7a02 53 float d1 = disSensor.read();
whutsup 8:a435e7aa7a02 54 float d2 = parA*d1*d1+parB*d1+parC;
Bhoney 19:67584cb64b9c 55 targetDis = d2; // setup to target distance in MkActionManual
whutsup 4:bf4ad2079096 56
whutsup 4:bf4ad2079096 57 int a = up.read();
whutsup 4:bf4ad2079096 58 int b = down.read();
whutsup 4:bf4ad2079096 59 int c = a*2 + b;
whutsup 4:bf4ad2079096 60
whutsup 4:bf4ad2079096 61 switch(c)
whutsup 4:bf4ad2079096 62 {
whutsup 4:bf4ad2079096 63 case 0 : //stop
whutsup 4:bf4ad2079096 64
whutsup 4:bf4ad2079096 65 enable = 0;
whutsup 9:7d6fa62f9022 66 nreset = 0;
whutsup 4:bf4ad2079096 67 p1 = 0;
whutsup 4:bf4ad2079096 68 p2 = 0;
Bhoney 19:67584cb64b9c 69 #ifdef QUALIFYING_MODE
Bhoney 19:67584cb64b9c 70 Qfunc();
Bhoney 19:67584cb64b9c 71 #endif
whutsup 4:bf4ad2079096 72 break;
whutsup 4:bf4ad2079096 73
whutsup 4:bf4ad2079096 74 case 1 : // up
Bhoney 19:67584cb64b9c 75
whutsup 4:bf4ad2079096 76 enable = 1;
whutsup 9:7d6fa62f9022 77 nreset = 1;
whutsup 4:bf4ad2079096 78 p1 = 0;
whutsup 4:bf4ad2079096 79 p2 = 1;
Bhoney 19:67584cb64b9c 80 #ifdef QUALIFYING_MODE
Bhoney 19:67584cb64b9c 81 Qfunc();
Bhoney 19:67584cb64b9c 82 #endif
whutsup 4:bf4ad2079096 83 break;
whutsup 4:bf4ad2079096 84
whutsup 4:bf4ad2079096 85 case 2 : // down
Bhoney 19:67584cb64b9c 86
whutsup 4:bf4ad2079096 87 enable = 1;
whutsup 9:7d6fa62f9022 88 nreset = 1;
whutsup 4:bf4ad2079096 89 p1 = 1;
whutsup 4:bf4ad2079096 90 p2 = 0;
Bhoney 19:67584cb64b9c 91 #ifdef QUALIFYING_MODE
Bhoney 19:67584cb64b9c 92 Qfunc();
Bhoney 19:67584cb64b9c 93 #endif
whutsup 4:bf4ad2079096 94 break;
Bhoney 19:67584cb64b9c 95
whutsup 4:bf4ad2079096 96
whutsup 4:bf4ad2079096 97 case 3 : // stop
whutsup 4:bf4ad2079096 98
whutsup 4:bf4ad2079096 99 enable = 0;
whutsup 9:7d6fa62f9022 100 nreset = 0;
whutsup 4:bf4ad2079096 101 p1 = 0;
whutsup 4:bf4ad2079096 102 p2 = 0;
Bhoney 19:67584cb64b9c 103 #ifdef QUALIFYING_MODE
Bhoney 19:67584cb64b9c 104 Qfunc();
Bhoney 19:67584cb64b9c 105 #endif
whutsup 4:bf4ad2079096 106 break;
whutsup 4:bf4ad2079096 107
whutsup 4:bf4ad2079096 108 }
whutsup 4:bf4ad2079096 109
whutsup 4:bf4ad2079096 110
whutsup 4:bf4ad2079096 111
whutsup 4:bf4ad2079096 112 }
whutsup 4:bf4ad2079096 113
whutsup 4:bf4ad2079096 114 void MotorTest(int mode)
whutsup 4:bf4ad2079096 115 {
whutsup 4:bf4ad2079096 116
whutsup 4:bf4ad2079096 117 timer3.detach();
whutsup 4:bf4ad2079096 118
whutsup 4:bf4ad2079096 119 switch (mode)
whutsup 4:bf4ad2079096 120
whutsup 4:bf4ad2079096 121 {
whutsup 4:bf4ad2079096 122 case 0 : //stop
whutsup 4:bf4ad2079096 123
whutsup 4:bf4ad2079096 124 enable = 0;
whutsup 10:3fcaf50f528f 125 nreset = 0;
whutsup 10:3fcaf50f528f 126 p1 = 0;
whutsup 10:3fcaf50f528f 127 p2 = 0;
whutsup 4:bf4ad2079096 128
whutsup 4:bf4ad2079096 129 break;
whutsup 4:bf4ad2079096 130
whutsup 4:bf4ad2079096 131 case 1 : // up
whutsup 4:bf4ad2079096 132
whutsup 4:bf4ad2079096 133 enable = 1;
whutsup 9:7d6fa62f9022 134 nreset = 1;
whutsup 4:bf4ad2079096 135 p1 = 0;
whutsup 4:bf4ad2079096 136 p2 = 1;
whutsup 4:bf4ad2079096 137
whutsup 4:bf4ad2079096 138 break;
whutsup 4:bf4ad2079096 139
whutsup 4:bf4ad2079096 140 case 2 : // down
whutsup 4:bf4ad2079096 141
whutsup 4:bf4ad2079096 142 enable = 1;
whutsup 9:7d6fa62f9022 143 nreset = 1;
whutsup 4:bf4ad2079096 144 p1 = 1;
whutsup 4:bf4ad2079096 145 p2 = 0;
whutsup 4:bf4ad2079096 146
whutsup 4:bf4ad2079096 147 break;
whutsup 4:bf4ad2079096 148
whutsup 4:bf4ad2079096 149 }
whutsup 4:bf4ad2079096 150
whutsup 4:bf4ad2079096 151
whutsup 4:bf4ad2079096 152 }
whutsup 4:bf4ad2079096 153
whutsup 4:bf4ad2079096 154 void ControlAng()
whutsup 4:bf4ad2079096 155 {
whutsup 11:5f05b14649ee 156 bt.putc('1');
whutsup 4:bf4ad2079096 157 float d1 = disSensor.read();
whutsup 4:bf4ad2079096 158 float d2 = parA*d1*d1+parB*d1+parC;
whutsup 4:bf4ad2079096 159 float ref_d = targetDis-d2;
whutsup 4:bf4ad2079096 160
whutsup 8:a435e7aa7a02 161 if(ref_d > 0.3f)
whutsup 4:bf4ad2079096 162 {
whutsup 4:bf4ad2079096 163 enable = 1;
whutsup 10:3fcaf50f528f 164 nreset = 1;
whutsup 4:bf4ad2079096 165 p1 = 0;
whutsup 4:bf4ad2079096 166 p2 = 1;
whutsup 4:bf4ad2079096 167 }
whutsup 4:bf4ad2079096 168
whutsup 8:a435e7aa7a02 169 else if(ref_d < -0.3f)
whutsup 4:bf4ad2079096 170 {
whutsup 4:bf4ad2079096 171 enable = 1;
whutsup 10:3fcaf50f528f 172 nreset = 1;
whutsup 4:bf4ad2079096 173 p1 = 1;
whutsup 4:bf4ad2079096 174 p2 = 0;
whutsup 4:bf4ad2079096 175 }
whutsup 4:bf4ad2079096 176
whutsup 4:bf4ad2079096 177 else
whutsup 4:bf4ad2079096 178 {
whutsup 4:bf4ad2079096 179 enable = 0;
whutsup 10:3fcaf50f528f 180 nreset = 0;
whutsup 4:bf4ad2079096 181 timer1.detach();
whutsup 4:bf4ad2079096 182 }
whutsup 4:bf4ad2079096 183 }
whutsup 4:bf4ad2079096 184
whutsup 4:bf4ad2079096 185
whutsup 4:bf4ad2079096 186 void MkAction()
whutsup 4:bf4ad2079096 187 {
whutsup 4:bf4ad2079096 188 float d1 = disSensor.read();
whutsup 4:bf4ad2079096 189 float d2 = parA*d1*d1+parB*d1+parC;
whutsup 4:bf4ad2079096 190
whutsup 4:bf4ad2079096 191 static float curTarget;
whutsup 4:bf4ad2079096 192 float s;
whutsup 4:bf4ad2079096 193 float abs_error;
whutsup 4:bf4ad2079096 194
whutsup 10:3fcaf50f528f 195 curTarget = (onewayNum%2==0)? targetDis: 0.2f ; // initial angle
whutsup 4:bf4ad2079096 196
whutsup 4:bf4ad2079096 197 s = (curTarget-d2)/targetDis;
whutsup 4:bf4ad2079096 198 abs_error = abs(curTarget-d2);
whutsup 4:bf4ad2079096 199
Bhoney 19:67584cb64b9c 200 if(!nFaultFlag)
Bhoney 15:e5e34512a00e 201 {
Bhoney 19:67584cb64b9c 202 p1 = 0;
Bhoney 19:67584cb64b9c 203 p2 = 0;
Bhoney 15:e5e34512a00e 204 return;
Bhoney 15:e5e34512a00e 205 }
whutsup 4:bf4ad2079096 206 if(s>=0)
whutsup 4:bf4ad2079096 207 {
whutsup 4:bf4ad2079096 208 enable = 1;
whutsup 10:3fcaf50f528f 209 nreset = 1;
whutsup 4:bf4ad2079096 210 p1 = 0;
whutsup 10:3fcaf50f528f 211 p2 = 1;
whutsup 4:bf4ad2079096 212 }
whutsup 4:bf4ad2079096 213
whutsup 4:bf4ad2079096 214 else
whutsup 4:bf4ad2079096 215 {
whutsup 4:bf4ad2079096 216 enable = 1;
whutsup 10:3fcaf50f528f 217 nreset = 1;
whutsup 4:bf4ad2079096 218 p2 = 0;
whutsup 10:3fcaf50f528f 219 p1 = 1;
whutsup 4:bf4ad2079096 220 }
whutsup 10:3fcaf50f528f 221
whutsup 10:3fcaf50f528f 222 if (abs_error <1.5f)
whutsup 4:bf4ad2079096 223 {
whutsup 4:bf4ad2079096 224 onewayNum = onewayNum +1;
whutsup 10:3fcaf50f528f 225 enable = 0;
Bhoney 19:67584cb64b9c 226 nreset = 0;
whutsup 4:bf4ad2079096 227
whutsup 4:bf4ad2079096 228 if(onewayNum >=milLoop*2)
whutsup 4:bf4ad2079096 229 {
whutsup 4:bf4ad2079096 230 onewayNum = 0;
whutsup 4:bf4ad2079096 231 milLoop = 0;
whutsup 4:bf4ad2079096 232 enable = 0;
whutsup 10:3fcaf50f528f 233 nreset = 0;
whutsup 8:a435e7aa7a02 234 timer3.detach();
Bhoney 15:e5e34512a00e 235
Bhoney 15:e5e34512a00e 236 bt.puts("<ME>\r\n");
whutsup 4:bf4ad2079096 237 }
whutsup 4:bf4ad2079096 238 }
whutsup 4:bf4ad2079096 239 }
whutsup 4:bf4ad2079096 240
whutsup 4:bf4ad2079096 241 void MkActionManual()
whutsup 4:bf4ad2079096 242 {
Bhoney 19:67584cb64b9c 243 if( 1 == mkOn)
whutsup 8:a435e7aa7a02 244 {
whutsup 8:a435e7aa7a02 245 mkOn = 0;
whutsup 8:a435e7aa7a02 246 enable = 0;
whutsup 10:3fcaf50f528f 247 nreset = 0;
whutsup 8:a435e7aa7a02 248 timer3.detach();
whutsup 8:a435e7aa7a02 249 }
whutsup 8:a435e7aa7a02 250
whutsup 8:a435e7aa7a02 251 else
whutsup 8:a435e7aa7a02 252 {
whutsup 8:a435e7aa7a02 253 mkOn = 1;
Bhoney 19:67584cb64b9c 254 onewayNum=0;
whutsup 8:a435e7aa7a02 255 milLoop = 99;
whutsup 8:a435e7aa7a02 256 timer3.attach(&MkAction,0.1);
whutsup 8:a435e7aa7a02 257 }
whutsup 4:bf4ad2079096 258 }