F3RC1班 / Mbed 2 deprecated f3rc_auto

自動機

Dependencies:   EC mbed

main.cpp@2:c8c264a9a7f6, 2017-09-06 (annotated)

Committer:
eri
Date:
Wed Sep 06 02:47:52 2017 +0000
Revision:
2:c8c264a9a7f6
Parent:
1:651bd0514eb9
Child:
3:f5e875d8df30
?????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
eri 0:1f542f8756d6 1 #include "mbed.h"
eri 0:1f542f8756d6 2 #include "EC.h"
eri 0:1f542f8756d6 3
eri 0:1f542f8756d6 4 PwmOut servo(PB_7);
eri 0:1f542f8756d6 5 DigitalIn sw(PA_15);
eri 0:1f542f8756d6 6 SPISlave spi(PB_15,PB_14,PB_13,PB_12);
eri 0:1f542f8756d6 7
eri 0:1f542f8756d6 8 Ec Ec1(PB_6,PC_7,NC,1048,0.05);
eri 1:651bd0514eb9 9 Ticker ticker;
eri 1:651bd0514eb9 10 Ticker ticker2;
eri 1:651bd0514eb9 11 DigitalIn button(USER_BUTTON);
eri 1:651bd0514eb9 12 Serial pc(USBTX,USBRX);
eri 1:651bd0514eb9 13
eri 1:651bd0514eb9 14 SpeedControl motor1(PA_10,PB_3,NC,1048,0.05,PA_1,PA_0); //左
eri 1:651bd0514eb9 15 SpeedControl motor2(PA_9,PA_8,NC,1048,0.05,PB_4,PB_5); //右
eri 1:651bd0514eb9 16
eri 1:651bd0514eb9 17 Timer timer;
eri 0:1f542f8756d6 18
eri 1:651bd0514eb9 19 int kai=0;
eri 0:1f542f8756d6 20 int n=1;
eri 1:651bd0514eb9 21 int ang=0;
eri 1:651bd0514eb9 22 int a=0;
eri 1:651bd0514eb9 23 int c=0;
eri 1:651bd0514eb9 24 int d=0;
eri 1:651bd0514eb9 25 int dis=0;
eri 1:651bd0514eb9 26 double V=0;
eri 1:651bd0514eb9 27 double v();
eri 1:651bd0514eb9 28 int e=0;
eri 1:651bd0514eb9 29 int f=0;
eri 1:651bd0514eb9 30
eri 0:1f542f8756d6 31
eri 0:1f542f8756d6 32
eri 1:651bd0514eb9 33 void calOmega() //角速度計算関数
eri 1:651bd0514eb9 34 {
eri 1:651bd0514eb9 35 motor1.CalOmega();
eri 1:651bd0514eb9 36 motor2.CalOmega();
eri 1:651bd0514eb9 37 Ec1.CalOmega();
eri 1:651bd0514eb9 38 }
eri 1:651bd0514eb9 39
eri 1:651bd0514eb9 40 void Ang() //Nucleoリセット時からの機体角度(0~3600)
eri 0:1f542f8756d6 41 {
eri 1:651bd0514eb9 42 if(spi.receive())ang=spi.read() - d;
eri 1:651bd0514eb9 43
eri 1:651bd0514eb9 44 if(ang < 0)ang += 3600;
eri 1:651bd0514eb9 45 if(ang > 3600)ang-=3600;
eri 1:651bd0514eb9 46
eri 0:1f542f8756d6 47 }
eri 0:1f542f8756d6 48
eri 1:651bd0514eb9 49
eri 1:651bd0514eb9 50 void print()
eri 1:651bd0514eb9 51 {
eri 1:651bd0514eb9 52 if(kai>=500) {
eri 1:651bd0514eb9 53 //pc.printf("count1=%d\r\n",dis);
eri 1:651bd0514eb9 54 // pc.printf("count=%f ",motor1.getOmega());
eri 1:651bd0514eb9 55 // pc.printf("count2=%f\r\n ",motor2.getOmega());
eri 1:651bd0514eb9 56 //pc.printf("duty1=%f ",motor1.duty);
eri 1:651bd0514eb9 57 //pc.printf("duty2=%f\r\n",motor2.duty);
eri 1:651bd0514eb9 58 //pc.printf("d=%d",d);
eri 1:651bd0514eb9 59 //pc.printf("v=%f\r\n",v());
eri 2:c8c264a9a7f6 60 //pc.printf("ang=%d\r\n",ang);
eri 1:651bd0514eb9 61 kai=0;
eri 1:651bd0514eb9 62 }
eri 1:651bd0514eb9 63 kai++;
eri 1:651bd0514eb9 64 }
eri 1:651bd0514eb9 65
eri 1:651bd0514eb9 66
eri 0:1f542f8756d6 67 void st()
eri 0:1f542f8756d6 68 {
eri 0:1f542f8756d6 69 motor1.stop();
eri 0:1f542f8756d6 70 motor2.stop();
eri 1:651bd0514eb9 71 wait(0.5);
eri 0:1f542f8756d6 72 }
eri 0:1f542f8756d6 73
eri 1:651bd0514eb9 74
eri 1:651bd0514eb9 75
eri 1:651bd0514eb9 76 void str(int a)
eri 0:1f542f8756d6 77 {
eri 1:651bd0514eb9 78
eri 1:651bd0514eb9 79 while(n==1) {
eri 1:651bd0514eb9 80 dis=0.301*Ec1.getCount();
eri 1:651bd0514eb9 81 if(dis < a) {
eri 1:651bd0514eb9 82 V=15;
eri 1:651bd0514eb9 83 e=400;
eri 0:1f542f8756d6 84
eri 1:651bd0514eb9 85 if(dis > a-e) {
eri 1:651bd0514eb9 86 V = (a - dis)*V/e;
eri 1:651bd0514eb9 87 if(V<2)V=2;
eri 1:651bd0514eb9 88 }
eri 1:651bd0514eb9 89
eri 1:651bd0514eb9 90 motor1.Sc(V);
eri 1:651bd0514eb9 91 motor2.Sc(V);
eri 1:651bd0514eb9 92
eri 1:651bd0514eb9 93 } else {
eri 1:651bd0514eb9 94 n=2;
eri 1:651bd0514eb9 95 }
eri 1:651bd0514eb9 96 }
eri 1:651bd0514eb9 97 st();
eri 0:1f542f8756d6 98 }
eri 0:1f542f8756d6 99
eri 1:651bd0514eb9 100
eri 1:651bd0514eb9 101 void back(int a)
eri 0:1f542f8756d6 102 {
eri 1:651bd0514eb9 103 e=400;
eri 1:651bd0514eb9 104
eri 1:651bd0514eb9 105 motor1.setDOconstant(20.7);
eri 1:651bd0514eb9 106 motor2.setDOconstant(20.3);
eri 1:651bd0514eb9 107
eri 0:1f542f8756d6 108
eri 1:651bd0514eb9 109 while(n==2) {
eri 1:651bd0514eb9 110 V=15;
eri 1:651bd0514eb9 111 dis=0.301*Ec1.getCount();
eri 1:651bd0514eb9 112 if(dis > a) {
eri 1:651bd0514eb9 113 if(dis < a+e ) {
eri 1:651bd0514eb9 114 V = (a - dis)*V/e;
eri 1:651bd0514eb9 115 if(V<2)V=2;
eri 1:651bd0514eb9 116 }
eri 1:651bd0514eb9 117 motor1.Sc(-V);
eri 1:651bd0514eb9 118 motor2.Sc(-V);
eri 1:651bd0514eb9 119 } else {
eri 1:651bd0514eb9 120 motor1.Sc(0);
eri 1:651bd0514eb9 121 motor2.Sc(0);
eri 1:651bd0514eb9 122 n=1;
eri 1:651bd0514eb9 123 }
eri 1:651bd0514eb9 124 }
eri 1:651bd0514eb9 125 st();
eri 1:651bd0514eb9 126 motor1.setDOconstant(19.7);
eri 1:651bd0514eb9 127 motor2.setDOconstant(21.3);
eri 0:1f542f8756d6 128 }
eri 0:1f542f8756d6 129
eri 0:1f542f8756d6 130
eri 1:651bd0514eb9 131 void turn(int b)
eri 1:651bd0514eb9 132 {
eri 1:651bd0514eb9 133 c=0;
eri 1:651bd0514eb9 134 f=700;
eri 1:651bd0514eb9 135
eri 1:651bd0514eb9 136 while(n==2) {
eri 1:651bd0514eb9 137 V=5;
eri 2:c8c264a9a7f6 138
eri 1:651bd0514eb9 139 if(b >= 0) {
eri 1:651bd0514eb9 140
eri 1:651bd0514eb9 141
eri 1:651bd0514eb9 142 a = ang - b; //角度(目標値から±1800)
eri 1:651bd0514eb9 143
eri 1:651bd0514eb9 144 if(a < -1800) {
eri 1:651bd0514eb9 145 a += 3600;
eri 1:651bd0514eb9 146 } else if(a > 1800) {
eri 1:651bd0514eb9 147 a -= 3600;
eri 1:651bd0514eb9 148 }
eri 1:651bd0514eb9 149 print();
eri 1:651bd0514eb9 150 if(c==1 && a > 20) {
eri 1:651bd0514eb9 151 motor1.Sc(-v());
eri 1:651bd0514eb9 152 motor2.stop();
eri 1:651bd0514eb9 153 } else if(c==2 && a < -20) {
eri 1:651bd0514eb9 154 motor1.stop();
eri 1:651bd0514eb9 155 motor2.Sc(-v());
eri 1:651bd0514eb9 156 } else if(a < -f) {
eri 1:651bd0514eb9 157 motor1.Sc(V);
eri 1:651bd0514eb9 158 motor2.stop();
eri 1:651bd0514eb9 159 } else if(a > f) {
eri 1:651bd0514eb9 160 motor1.stop();
eri 1:651bd0514eb9 161 motor2.Sc(V);
eri 1:651bd0514eb9 162 } else if(a < -10) {
eri 1:651bd0514eb9 163 motor1.Sc(v());
eri 1:651bd0514eb9 164 motor2.stop();
eri 1:651bd0514eb9 165 c=1;
eri 1:651bd0514eb9 166 } else if(a > 10) {
eri 1:651bd0514eb9 167 motor1.stop();
eri 1:651bd0514eb9 168 motor2.Sc(v());
eri 1:651bd0514eb9 169 c=2;
eri 1:651bd0514eb9 170 } else {
eri 1:651bd0514eb9 171 Ec1.reset();
eri 1:651bd0514eb9 172 motor1.stop();
eri 1:651bd0514eb9 173 motor2.stop();
eri 1:651bd0514eb9 174 n=1;
eri 1:651bd0514eb9 175 }
eri 1:651bd0514eb9 176
eri 1:651bd0514eb9 177 } else {
eri 1:651bd0514eb9 178 a = ang + b;
eri 1:651bd0514eb9 179
eri 1:651bd0514eb9 180 if(a < -1800) {
eri 1:651bd0514eb9 181 a += 3600;
eri 1:651bd0514eb9 182 } else if(a > 1800) {
eri 1:651bd0514eb9 183 a -= 3600;
eri 1:651bd0514eb9 184 }
eri 1:651bd0514eb9 185
eri 1:651bd0514eb9 186 if(c==1 && a > 20) {
eri 1:651bd0514eb9 187 motor1.stop();
eri 1:651bd0514eb9 188 motor2.Sc(v());
eri 1:651bd0514eb9 189 } else if(c==2 && a < -20) {
eri 1:651bd0514eb9 190 motor1.Sc(v());
eri 1:651bd0514eb9 191 motor2.stop();
eri 1:651bd0514eb9 192 } else if(a < -f) {
eri 1:651bd0514eb9 193 motor1.stop();
eri 1:651bd0514eb9 194 motor2.Sc(-V);
eri 1:651bd0514eb9 195 } else if(a > f) {
eri 1:651bd0514eb9 196 motor1.Sc(-V);
eri 1:651bd0514eb9 197 motor2.stop();
eri 1:651bd0514eb9 198 } else if(a < -10) {
eri 1:651bd0514eb9 199 motor1.stop();
eri 1:651bd0514eb9 200 motor2.Sc(-v());
eri 1:651bd0514eb9 201 c=1;
eri 1:651bd0514eb9 202 } else if(a > 10) {
eri 1:651bd0514eb9 203 motor1.Sc(-v());
eri 1:651bd0514eb9 204 motor2.stop();
eri 1:651bd0514eb9 205 c=2;
eri 1:651bd0514eb9 206 } else {
eri 1:651bd0514eb9 207 Ec1.reset();
eri 1:651bd0514eb9 208 motor1.stop();
eri 1:651bd0514eb9 209 motor2.stop();
eri 1:651bd0514eb9 210 n=1;
eri 1:651bd0514eb9 211 }
eri 1:651bd0514eb9 212 }
eri 1:651bd0514eb9 213 }
eri 1:651bd0514eb9 214 while(1){
eri 1:651bd0514eb9 215 print();
eri 1:651bd0514eb9 216 }
eri 1:651bd0514eb9 217 }
eri 1:651bd0514eb9 218
eri 1:651bd0514eb9 219 double v(){
eri 1:651bd0514eb9 220 if(a < 0)a=-a;
eri 1:651bd0514eb9 221 V=(a-10)*V/f;
eri 1:651bd0514eb9 222 if(V < 0.3)V=0.3;
eri 1:651bd0514eb9 223 return V;
eri 1:651bd0514eb9 224 }
eri 1:651bd0514eb9 225
eri 1:651bd0514eb9 226
eri 1:651bd0514eb9 227 int main()
eri 1:651bd0514eb9 228 {
eri 0:1f542f8756d6 229
eri 0:1f542f8756d6 230 servo.period_ms(20);
eri 0:1f542f8756d6 231 spi.format(16,3);
eri 0:1f542f8756d6 232 spi.frequency(1000000);
eri 0:1f542f8756d6 233
eri 1:651bd0514eb9 234
eri 0:1f542f8756d6 235 ticker.attach(&calOmega,0.05);
eri 1:651bd0514eb9 236 ticker2.attach(&Ang,0.05);
eri 1:651bd0514eb9 237
eri 1:651bd0514eb9 238 motor1.setPDparam(0.3,0.4); //PDパラメータを設定
eri 1:651bd0514eb9 239 motor2.setPDparam(0.3,0.2); //PDパラメータを設定
eri 1:651bd0514eb9 240
eri 1:651bd0514eb9 241 motor1.setDOconstant(19.6);
eri 1:651bd0514eb9 242 motor2.setDOconstant(22.3);
eri 1:651bd0514eb9 243
eri 1:651bd0514eb9 244
eri 1:651bd0514eb9 245 sw.mode(PullUp);
eri 1:651bd0514eb9 246
eri 1:651bd0514eb9 247
eri 1:651bd0514eb9 248
eri 1:651bd0514eb9 249
eri 0:1f542f8756d6 250
eri 1:651bd0514eb9 251 if(sw==1) { //スタートゾーン１
eri 1:651bd0514eb9 252 servo.pulsewidth_us(900);
eri 1:651bd0514eb9 253 wait(0.5);
eri 1:651bd0514eb9 254 d=ang; //初期角度
eri 1:651bd0514eb9 255 n=2;
eri 2:c8c264a9a7f6 256 turn(3100);
eri 1:651bd0514eb9 257 str(950);
eri 1:651bd0514eb9 258 turn(2410);
eri 1:651bd0514eb9 259 str(1070);
eri 1:651bd0514eb9 260 turn(2700);
eri 1:651bd0514eb9 261 servo.pulsewidth_us(1800);
eri 1:651bd0514eb9 262 str(870);
eri 1:651bd0514eb9 263 servo.pulsewidth_us(900);
eri 1:651bd0514eb9 264 wait(1);
eri 1:651bd0514eb9 265 back(0);
eri 1:651bd0514eb9 266 n=2;
eri 1:651bd0514eb9 267 turn(-2430);
eri 1:651bd0514eb9 268 n=2;
eri 1:651bd0514eb9 269 back(-1080);
eri 1:651bd0514eb9 270 n=2;
eri 1:651bd0514eb9 271 turn(-2700);
eri 1:651bd0514eb9 272 n=2;
eri 1:651bd0514eb9 273 back(-970);
eri 1:651bd0514eb9 274 n=2;
eri 1:651bd0514eb9 275 turn(1800);
eri 1:651bd0514eb9 276 str(500);
eri 1:651bd0514eb9 277 servo.pulsewidth_us(1800);
eri 1:651bd0514eb9 278 wait(0.3);
eri 0:1f542f8756d6 279
eri 1:651bd0514eb9 280 motor1.setDOconstant(20.7);
eri 1:651bd0514eb9 281 motor2.setDOconstant(20.3);
eri 1:651bd0514eb9 282 while(n==2) {
eri 1:651bd0514eb9 283 dis=0.301*Ec1.getCount();
eri 1:651bd0514eb9 284 if(dis > 370) {
eri 1:651bd0514eb9 285 motor1.Sc(-15);
eri 1:651bd0514eb9 286 motor2.Sc(-15);
eri 1:651bd0514eb9 287 } else {
eri 1:651bd0514eb9 288 motor1.Sc(0);
eri 1:651bd0514eb9 289 motor2.Sc(0);
eri 1:651bd0514eb9 290 n=1;
eri 1:651bd0514eb9 291 }
eri 1:651bd0514eb9 292 }
eri 1:651bd0514eb9 293 st();
eri 1:651bd0514eb9 294 motor1.setDOconstant(19.7);
eri 1:651bd0514eb9 295 motor2.setDOconstant(21.3);
eri 1:651bd0514eb9 296
eri 1:651bd0514eb9 297 servo.pulsewidth_us(900);
eri 1:651bd0514eb9 298 str(500);
eri 1:651bd0514eb9 299
eri 1:651bd0514eb9 300 while(n==2) {
eri 1:651bd0514eb9 301 dis=0.301*Ec1.getCount();
eri 1:651bd0514eb9 302 if(dis > 370) {
eri 1:651bd0514eb9 303 motor1.Sc(-15);
eri 1:651bd0514eb9 304 motor2.Sc(-15);
eri 1:651bd0514eb9 305 } else {
eri 1:651bd0514eb9 306 motor1.stop();
eri 1:651bd0514eb9 307 motor2.stop();
eri 1:651bd0514eb9 308 n=1;
eri 1:651bd0514eb9 309 }
eri 2:c8c264a9a7f6 310 }
eri 1:651bd0514eb9 311
eri 1:651bd0514eb9 312 } else { //スタートゾーン２
eri 1:651bd0514eb9 313 servo.pulsewidth_us(1800);
eri 1:651bd0514eb9 314 wait(0.5);
eri 1:651bd0514eb9 315 d=ang - 1800;
eri 1:651bd0514eb9 316 str(450);
eri 1:651bd0514eb9 317 servo.pulsewidth_us(900);
eri 1:651bd0514eb9 318 wait(1);
eri 1:651bd0514eb9 319 turn(880);
eri 1:651bd0514eb9 320 str(820);
eri 1:651bd0514eb9 321 turn(610);
eri 1:651bd0514eb9 322 str(1080);
eri 1:651bd0514eb9 323 turn(1320);
eri 1:651bd0514eb9 324 str(930);
eri 1:651bd0514eb9 325 turn(1800);
eri 1:651bd0514eb9 326 servo.pulsewidth_us(1800);
eri 1:651bd0514eb9 327 n=2;
eri 1:651bd0514eb9 328 wait(0.3);
eri 0:1f542f8756d6 329
eri 1:651bd0514eb9 330 motor1.setDOconstant(20.7);
eri 1:651bd0514eb9 331 motor2.setDOconstant(20.3);
eri 1:651bd0514eb9 332 while(n==2) {
eri 1:651bd0514eb9 333 dis=0.301*Ec1.getCount();
eri 1:651bd0514eb9 334 if(dis > -130) {
eri 1:651bd0514eb9 335 motor1.Sc(-15);
eri 1:651bd0514eb9 336 motor2.Sc(-15);
eri 1:651bd0514eb9 337 } else {
eri 1:651bd0514eb9 338 motor1.stop();
eri 1:651bd0514eb9 339 motor2.stop();
eri 1:651bd0514eb9 340 n=1;
eri 1:651bd0514eb9 341 }
eri 1:651bd0514eb9 342 }
eri 0:1f542f8756d6 343
eri 1:651bd0514eb9 344 }
eri 1:651bd0514eb9 345
eri 1:651bd0514eb9 346 }
eri 1:651bd0514eb9 347