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@9:7d6fa62f9022, 2019-06-25 (annotated)

Committer:
whutsup
Date:
Tue Jun 25 02:10:31 2019 +0000
Revision:
9:7d6fa62f9022
Parent:
8:a435e7aa7a02
Child:
10:3fcaf50f528f
190625 updated ver

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 5:6bb52b2a79bf 26 #define parA 343.75f // parmeter of equation (values of distance sensor)
whutsup 5:6bb52b2a79bf 27 #define parB -1*508.08f
whutsup 5:6bb52b2a79bf 28 #define parC 166.67f
whutsup 4:bf4ad2079096 29
whutsup 4:bf4ad2079096 30
whutsup 4:bf4ad2079096 31 void MotorButton()
whutsup 4:bf4ad2079096 32 {
whutsup 8:a435e7aa7a02 33 mkOn=0;
whutsup 4:bf4ad2079096 34 milLoop = 0;
whutsup 8:a435e7aa7a02 35 // targetDis = 0;
whutsup 4:bf4ad2079096 36 onewayNum = 0;
whutsup 8:a435e7aa7a02 37 timer3.detach();
whutsup 8:a435e7aa7a02 38
whutsup 8:a435e7aa7a02 39 float d1 = disSensor.read();
whutsup 8:a435e7aa7a02 40 float d2 = parA*d1*d1+parB*d1+parC;
whutsup 8:a435e7aa7a02 41 targetDis = d2; // setup to target distance in MkActionManual
whutsup 4:bf4ad2079096 42
whutsup 4:bf4ad2079096 43 int a = up.read();
whutsup 4:bf4ad2079096 44 int b = down.read();
whutsup 4:bf4ad2079096 45 int c = a*2 + b;
whutsup 4:bf4ad2079096 46
whutsup 4:bf4ad2079096 47 switch(c)
whutsup 4:bf4ad2079096 48 {
whutsup 8:a435e7aa7a02 49
whutsup 8:a435e7aa7a02 50
whutsup 4:bf4ad2079096 51 case 0 : //stop
whutsup 4:bf4ad2079096 52
whutsup 4:bf4ad2079096 53 enable = 0;
whutsup 9:7d6fa62f9022 54 nreset = 0;
whutsup 4:bf4ad2079096 55 p1 = 0;
whutsup 4:bf4ad2079096 56 p2 = 0;
whutsup 4:bf4ad2079096 57 break;
whutsup 4:bf4ad2079096 58
whutsup 4:bf4ad2079096 59 case 1 : // up
whutsup 8:a435e7aa7a02 60 {
whutsup 4:bf4ad2079096 61
whutsup 4:bf4ad2079096 62 enable = 1;
whutsup 9:7d6fa62f9022 63 nreset = 1;
whutsup 4:bf4ad2079096 64 p1 = 0;
whutsup 4:bf4ad2079096 65 p2 = 1;
whutsup 8:a435e7aa7a02 66
whutsup 4:bf4ad2079096 67
whutsup 4:bf4ad2079096 68 break;
whutsup 8:a435e7aa7a02 69 }
whutsup 4:bf4ad2079096 70
whutsup 4:bf4ad2079096 71 case 2 : // down
whutsup 8:a435e7aa7a02 72 {
whutsup 8:a435e7aa7a02 73
whutsup 8:a435e7aa7a02 74
whutsup 8:a435e7aa7a02 75
whutsup 4:bf4ad2079096 76 enable = 1;
whutsup 9:7d6fa62f9022 77 nreset = 1;
whutsup 4:bf4ad2079096 78 p1 = 1;
whutsup 4:bf4ad2079096 79 p2 = 0;
whutsup 8:a435e7aa7a02 80
whutsup 4:bf4ad2079096 81
whutsup 4:bf4ad2079096 82 break;
whutsup 8:a435e7aa7a02 83 }
whutsup 4:bf4ad2079096 84
whutsup 4:bf4ad2079096 85 case 3 : // stop
whutsup 4:bf4ad2079096 86
whutsup 4:bf4ad2079096 87 enable = 0;
whutsup 9:7d6fa62f9022 88 nreset = 0;
whutsup 4:bf4ad2079096 89 p1 = 0;
whutsup 4:bf4ad2079096 90 p2 = 0;
whutsup 4:bf4ad2079096 91
whutsup 4:bf4ad2079096 92 break;
whutsup 4:bf4ad2079096 93
whutsup 4:bf4ad2079096 94 }
whutsup 4:bf4ad2079096 95
whutsup 4:bf4ad2079096 96
whutsup 4:bf4ad2079096 97
whutsup 4:bf4ad2079096 98 }
whutsup 4:bf4ad2079096 99
whutsup 4:bf4ad2079096 100 void MotorTest(int mode)
whutsup 4:bf4ad2079096 101 {
whutsup 4:bf4ad2079096 102
whutsup 4:bf4ad2079096 103 timer3.detach();
whutsup 4:bf4ad2079096 104
whutsup 4:bf4ad2079096 105 switch (mode)
whutsup 4:bf4ad2079096 106
whutsup 4:bf4ad2079096 107 {
whutsup 4:bf4ad2079096 108 case 0 : //stop
whutsup 4:bf4ad2079096 109
whutsup 4:bf4ad2079096 110 enable = 0;
whutsup 4:bf4ad2079096 111
whutsup 4:bf4ad2079096 112 break;
whutsup 4:bf4ad2079096 113
whutsup 4:bf4ad2079096 114 case 1 : // up
whutsup 4:bf4ad2079096 115
whutsup 4:bf4ad2079096 116 enable = 1;
whutsup 9:7d6fa62f9022 117 nreset = 1;
whutsup 4:bf4ad2079096 118 p1 = 0;
whutsup 4:bf4ad2079096 119 p2 = 1;
whutsup 4:bf4ad2079096 120
whutsup 4:bf4ad2079096 121 break;
whutsup 4:bf4ad2079096 122
whutsup 4:bf4ad2079096 123 case 2 : // down
whutsup 4:bf4ad2079096 124
whutsup 4:bf4ad2079096 125 enable = 1;
whutsup 9:7d6fa62f9022 126 nreset = 1;
whutsup 4:bf4ad2079096 127 p1 = 1;
whutsup 4:bf4ad2079096 128 p2 = 0;
whutsup 4:bf4ad2079096 129
whutsup 4:bf4ad2079096 130 break;
whutsup 4:bf4ad2079096 131
whutsup 4:bf4ad2079096 132 }
whutsup 4:bf4ad2079096 133
whutsup 4:bf4ad2079096 134
whutsup 4:bf4ad2079096 135 }
whutsup 4:bf4ad2079096 136
whutsup 4:bf4ad2079096 137 void ControlAng()
whutsup 4:bf4ad2079096 138 {
whutsup 4:bf4ad2079096 139
whutsup 4:bf4ad2079096 140 float d1 = disSensor.read();
whutsup 4:bf4ad2079096 141 float d2 = parA*d1*d1+parB*d1+parC;
whutsup 4:bf4ad2079096 142 float ref_d = targetDis-d2;
whutsup 8:a435e7aa7a02 143 // float kp = ref_d/(errorPrevious);
whutsup 4:bf4ad2079096 144
whutsup 8:a435e7aa7a02 145 if(ref_d > 0.3f)
whutsup 4:bf4ad2079096 146 {
whutsup 4:bf4ad2079096 147 enable = 1;
whutsup 4:bf4ad2079096 148 p1 = 0;
whutsup 4:bf4ad2079096 149 p2 = 1;
whutsup 8:a435e7aa7a02 150 // p2 = (kp<0.4?0.4:kp);
whutsup 8:a435e7aa7a02 151 // p2 = (ref_d>1?1:ref_d);
whutsup 4:bf4ad2079096 152
whutsup 4:bf4ad2079096 153 }
whutsup 4:bf4ad2079096 154
whutsup 8:a435e7aa7a02 155 else if(ref_d < -0.3f)
whutsup 4:bf4ad2079096 156 {
whutsup 4:bf4ad2079096 157
whutsup 4:bf4ad2079096 158 enable = 1;
whutsup 8:a435e7aa7a02 159 // p1 = -1*(ref_d<-1?-1:ref_d);
whutsup 4:bf4ad2079096 160 p1 = 1;
whutsup 8:a435e7aa7a02 161 // p1 = -1*(kp>0.4?0.4:kp);
whutsup 4:bf4ad2079096 162 p2 = 0;
whutsup 4:bf4ad2079096 163 }
whutsup 4:bf4ad2079096 164
whutsup 4:bf4ad2079096 165 else
whutsup 4:bf4ad2079096 166 {
whutsup 4:bf4ad2079096 167 enable = 0;
whutsup 4:bf4ad2079096 168 timer1.detach();
whutsup 4:bf4ad2079096 169
whutsup 4:bf4ad2079096 170 }
whutsup 4:bf4ad2079096 171 }
whutsup 4:bf4ad2079096 172
whutsup 4:bf4ad2079096 173
whutsup 4:bf4ad2079096 174 void MkAction()
whutsup 4:bf4ad2079096 175 {
whutsup 4:bf4ad2079096 176
whutsup 4:bf4ad2079096 177
whutsup 4:bf4ad2079096 178 float d1 = disSensor.read();
whutsup 4:bf4ad2079096 179 float d2 = parA*d1*d1+parB*d1+parC;
whutsup 4:bf4ad2079096 180
whutsup 4:bf4ad2079096 181 static float curTarget;
whutsup 4:bf4ad2079096 182 float s;
whutsup 4:bf4ad2079096 183 float abs_error;
whutsup 4:bf4ad2079096 184
whutsup 5:6bb52b2a79bf 185 curTarget = (onewayNum%2==0)? targetDis: 0.0f ; // initial angle
whutsup 4:bf4ad2079096 186
whutsup 4:bf4ad2079096 187 s = (curTarget-d2)/targetDis;
whutsup 4:bf4ad2079096 188 abs_error = abs(curTarget-d2);
whutsup 4:bf4ad2079096 189
whutsup 4:bf4ad2079096 190
whutsup 4:bf4ad2079096 191 if(s>=0)
whutsup 4:bf4ad2079096 192 {
whutsup 4:bf4ad2079096 193 enable = 1;
whutsup 4:bf4ad2079096 194 p1 = 0;
whutsup 4:bf4ad2079096 195 p2 = 1; //(s>0.7f)?s:0.7f;
whutsup 4:bf4ad2079096 196 }
whutsup 4:bf4ad2079096 197
whutsup 4:bf4ad2079096 198 else
whutsup 4:bf4ad2079096 199 {
whutsup 4:bf4ad2079096 200 enable = 1;
whutsup 4:bf4ad2079096 201 p1 = 1; //(s<-0.7f)?(-1*s):0.7f;
whutsup 4:bf4ad2079096 202 p2 = 0;
whutsup 4:bf4ad2079096 203 }
whutsup 4:bf4ad2079096 204
whutsup 5:6bb52b2a79bf 205 if (abs_error <0.7f)
whutsup 4:bf4ad2079096 206 {
whutsup 4:bf4ad2079096 207
whutsup 4:bf4ad2079096 208 onewayNum = onewayNum +1;
whutsup 4:bf4ad2079096 209
whutsup 4:bf4ad2079096 210 if(onewayNum >=milLoop*2)
whutsup 4:bf4ad2079096 211 {
whutsup 4:bf4ad2079096 212 onewayNum = 0;
whutsup 4:bf4ad2079096 213 milLoop = 0;
whutsup 4:bf4ad2079096 214 enable = 0;
whutsup 8:a435e7aa7a02 215 timer3.detach();
whutsup 8:a435e7aa7a02 216 bt.putc('<');
whutsup 8:a435e7aa7a02 217 bt.putc('M');
whutsup 8:a435e7aa7a02 218 bt.putc('E');
whutsup 8:a435e7aa7a02 219 bt.putc('>');
whutsup 8:a435e7aa7a02 220 bt.putc('\r');
whutsup 4:bf4ad2079096 221 }
whutsup 4:bf4ad2079096 222 }
whutsup 4:bf4ad2079096 223
whutsup 4:bf4ad2079096 224
whutsup 4:bf4ad2079096 225 }
whutsup 4:bf4ad2079096 226
whutsup 4:bf4ad2079096 227 void MkActionManual()
whutsup 4:bf4ad2079096 228 {
whutsup 4:bf4ad2079096 229
whutsup 8:a435e7aa7a02 230
whutsup 8:a435e7aa7a02 231 if( 1 == mkOn)
whutsup 8:a435e7aa7a02 232 {
whutsup 8:a435e7aa7a02 233 mkOn = 0;
whutsup 8:a435e7aa7a02 234 enable = 0;
whutsup 8:a435e7aa7a02 235 timer3.detach();
whutsup 8:a435e7aa7a02 236 }
whutsup 8:a435e7aa7a02 237
whutsup 8:a435e7aa7a02 238 else
whutsup 8:a435e7aa7a02 239 {
whutsup 8:a435e7aa7a02 240 mkOn = 1;
whutsup 8:a435e7aa7a02 241 onewayNum=0;
whutsup 8:a435e7aa7a02 242 milLoop = 99;
whutsup 8:a435e7aa7a02 243 timer3.attach(&MkAction,0.1);
whutsup 8:a435e7aa7a02 244 }
whutsup 4:bf4ad2079096 245
whutsup 4:bf4ad2079096 246
whutsup 4:bf4ad2079096 247 }