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@15:e5e34512a00e, 2019-07-04 (annotated)

Committer:
Bhoney
Date:
Thu Jul 04 07:02:38 2019 +0000
Revision:
15:e5e34512a00e
Parent:
14:d059d99e9b5e
Child:
19:67584cb64b9c
demo ver .2

Who changed what in which revision?

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