F3RC9/13 1317
Dependencies: CruizCore_R1370P EC delta enc_1multi mbed
Diff: main.cpp
- Revision:
- 4:e3d739bf09b4
- Parent:
- 3:7bd1afb46094
- Child:
- 5:0160b73f3d9e
--- a/main.cpp Tue Sep 11 04:48:43 2018 +0000 +++ b/main.cpp Tue Sep 11 06:42:51 2018 +0000 @@ -7,7 +7,7 @@ #define BASIC_SPEED 30 //モーターはこの角速度で駆動させる SpeedControl motorR(PB_13,PA_10,NC,500,0.05,PB_10,PB_1); //right enc migi ue -SpeedControl motorL(PB_3,PB_5,NC,500,0.05,PA_5,PA_7); //left enc hidari sita //ok +SpeedControl motorL(PB_5,PB_3,NC,500,0.05,PA_5,PA_7); //left enc hidari sita //ok Ec EC1(PB_4,PA_8,NC,300,0.05); //center enc Ticker motor_tick; //角速度計算用ticker Ticker ticker;//for enc @@ -39,7 +39,7 @@ double x=185; double y=150;//start point//keisann wo sinaosu hitsuyouga arimasu Timer t; -int i=0; +int i=1; int kai=0;//printf kansuu double target_R=0,target_L=0; @@ -264,169 +264,169 @@ pc.printf("i=%d\r\n",i); //pc.printf("EC1=%f",EC1.getDistance_mm(),EC1.getCount()); pc.printf("x=%f y=%f",x,y); - //pc.printf("angle=%f\r\n",angle); + pc.printf("angle=%f\r\n",angle); //機体の進む方向、右モーターの角速度、左モーターの角速度を表示 kai=0; } kai++; if(i==0) { - if(reset_f.read()==1) { + /*if(reset_f.read()==1) { wait(0.05); if(reset_f.read()==1) { - denjiben=0; - i++; - } - } + */ denjiben=0; + i++; + // } + //} + if(reset_a.read()==1) { + wait(0.05); if(reset_a.read()==1) { - wait(0.05); - if(reset_a.read()==1) { - denjiben=0; - x=70; - y=2500; - i=14; - } + denjiben=0; + x=70; + y=2500; + i=14; } } - if(i==1) { - susumu_y(1,1,407); - } - if(i==2) { - susumu_ang(1/3,1,45); - } - if(i==3) { - susumu_xl(1,1,709); - } - if(i==4) { - susumu_ang(1,1/3,0); - } - if(i==5) { - susumu_y(1,1,1700); + } + if(i==1) { + susumu_y(1,1,407); + } + if(i==2) { + susumu_ang(1/3,1,45); + } + if(i==3) { + susumu_xl(1,1,709); + } + if(i==4) { + susumu_ang(1,1/3,0); + } + if(i==5) { + susumu_y(1,1,1700); + } + if(i==6) { + motorR.stop(); + motorL.stop(); + t.start(); + if(t<1) { + motor_f=0.82; + motor_b=0; + } else { + motor_f=0; + motor_b=0; + printf("finish"); + t.reset(); + i++; } - if(i==6) { - motorR.stop(); - motorL.stop(); - t.start(); - if(t<1) { - motor_f=0.82; - motor_b=0; - } else { - motor_f=0; - motor_b=0; - printf("finish"); - t.reset(); - i++; - } - }//gatiasari + }//gatiasari - if(i==7) { - t.start(); - if(t<1) { - motor_f=0; - motor_b=0.82; - } else { - motor_f=0; - motor_b=0; - printf("finish"); - t.reset(); - } - if(angle>=-89) { - target_R=BASIC_SPEED/5; - target_L=BASIC_SPEED/(-5); - } - if(angle<=-91) { - target_R=BASIC_SPEED/(-5); - target_L=BASIC_SPEED/-5; - } - if(angle>-91&angle<-89) { - motorR.stop(); - motorL.stop(); - wait(0.5); - if(angle>-91&angle<-89) { - i++; - } - } - }//kakudo tyousei - if(i==8) { - susumu_xr(1,1,700); - } - if(i==9) { - susumu_ang(1/3,1,0); + if(i==7) { + t.start(); + if(t<1) { + motor_f=0; + motor_b=0.82; + } else { + motor_f=0; + motor_b=0; + printf("finish"); + t.reset(); } - if(i==10) { - susumu_y(1,1,2200); - } - if(i==11) { - susumu_ang(1/3,1,90); - } - if(i==12) { - motorR.stop(); - motorL.stop(); - servo.pulsewidth_us(1000); - wait(0.5); - i++; + if(angle>=-89) { + target_R=BASIC_SPEED/5; + target_L=BASIC_SPEED/(-5); } - if(i==13) { - if(angle>=91) { - target_R=BASIC_SPEED/5; - target_L=BASIC_SPEED/(-5); - } - if(angle<=89) { - target_R=BASIC_SPEED/(-5); - target_L=BASIC_SPEED/5; - } - if(angle>89&angle<91) { - motorR.stop(); - motorL.stop(); - wait(0.1); - if(angle>89&angle<91) { - i++; - } - } + if(angle<=-91) { + target_R=BASIC_SPEED/(-5); + target_L=BASIC_SPEED/-5; } - if(i==14) { - susumu_xl(1,1,1000); - } - if(i==15) { + if(angle>-91&angle<-89) { motorR.stop(); motorL.stop(); wait(0.5); - denjiben=1; - wait(0.5); - servo.pulsewidth_us(1500); - wait(0.5); - i++; - } - if(i==16) { - susumu_xr(-1,-1,700); + if(angle>-91&angle<-89) { + i++; + } } - if(i==17) { - susumu_ang(-1/3,-1,0); - } - if(i==18) { - susumu_y(-1,-1,2000); + }//kakudo tyousei + if(i==8) { + susumu_xr(1,1,700); + } + if(i==9) { + susumu_ang(1/3,1,0); + } + if(i==10) { + susumu_y(1,1,2200); + } + if(i==11) { + susumu_ang(1/3,1,90); + } + if(i==12) { + motorR.stop(); + motorL.stop(); + servo.pulsewidth_us(1000); + wait(0.5); + i++; + } + if(i==13) { + if(angle>=91) { + target_R=BASIC_SPEED/5; + target_L=BASIC_SPEED/(-5); } - if(i==19) { - susumu_ang(-1/3,-1,-90); + if(angle<=89) { + target_R=BASIC_SPEED/(-5); + target_L=BASIC_SPEED/5; } - if(i==20) { - susumu_xl(-1,-1,1100); - } - if(i==21) { + if(angle>89&angle<91) { motorR.stop(); motorL.stop(); - servo.pulsewidth_us(1500); - wait(0.5); - denjiben=0; - wait(0.5); + wait(0.1); + if(angle>89&angle<91) { + i++; + } + } + } + if(i==14) { + susumu_xl(1,1,1000); + } + if(i==15) { + motorR.stop(); + motorL.stop(); + wait(0.5); + denjiben=1; + wait(0.5); + servo.pulsewidth_us(1500); + wait(0.5); + i++; + } + if(i==16) { + susumu_xr(-1,-1,700); + } + if(i==17) { + susumu_ang(-1/3,-1,0); + } + if(i==18) { + susumu_y(-1,-1,2000); + } + if(i==19) { + susumu_ang(-1/3,-1,-90); + } + if(i==20) { + susumu_xl(-1,-1,1100); + } + if(i==21) { + motorR.stop(); + motorL.stop(); + servo.pulsewidth_us(1500); + wait(0.5); + denjiben=0; + wait(0.5); - break; - } + break; + } - }//while - tgt(0,0); - return 0; +}//while +tgt(0,0); +return 0; }//intmain