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@8:a435e7aa7a02, 2019-03-29 (annotated)

Committer:
whutsup
Date:
Fri Mar 29 01:29:39 2019 +0000
Revision:
8:a435e7aa7a02
Parent:
5:6bb52b2a79bf
Child:
9:7d6fa62f9022
anybaro_ver_7;

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