F3RC1班 / Mbed 2 deprecated f3rc_auto

自動機

Dependencies:   EC mbed

main.cpp@3:f5e875d8df30, 2017-09-06 (annotated)

Committer:
eri
Date:
Wed Sep 06 05:52:29 2017 +0000
Revision:
3:f5e875d8df30
Parent:
2:c8c264a9a7f6 
???????

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