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@13:5601fedf0e12, 2019-07-03 (annotated)

Committer:
whutsup
Date:
Wed Jul 03 08:32:41 2019 +0000
Revision:
13:5601fedf0e12
Parent:
11:5f05b14649ee
Child:
14:d059d99e9b5e
demo ver.1

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