![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
F3RC9/15
Dependencies: CruizCore_R1370P EC delta enc_1multi mbed
Fork of F3RCrere by
Diff: main.cpp
- Revision:
- 6:7ec6c3d3a30a
- Parent:
- 5:0160b73f3d9e
- Child:
- 7:3a7e49ed1162
--- a/main.cpp Wed Sep 12 05:35:59 2018 +0000 +++ b/main.cpp Thu Sep 13 04:17:15 2018 +0000 @@ -4,7 +4,7 @@ #include "EC.h" #include "R1370P.h" #include "enc_1multi.h" -#define BASIC_SPEED 30 //モーターはこの角速度で駆動させる +#define BASIC_SPEED 15 //モーターはこの角速度で駆動させる SpeedControl motorR(PB_13,PA_10,NC,500,0.05,PB_10,PB_1); //right enc migi ue SpeedControl motorL(PB_5,PB_3,NC,500,0.05,PA_5,PA_7); //left enc hidari sita //ok @@ -164,7 +164,7 @@ void susumu_ang(double a,double b,double goal)//kakudo { - if(x<angle-5&&a>b) {//usetsu + if(goal-5>angle&&a<b) {//usetsu t.start(); pc.printf("R=%f L=%f\r\n",target_R,target_L); if(t.read_ms()<100) { @@ -178,10 +178,10 @@ t.stop(); pc.printf("R=%f L=%f",target_R,target_L); } - } else if(angle<goal&&angle>=goal-5&&a>b) { + } else if(angle<goal&&angle>=goal-5&&a<b) { tgt(a/3,b/3); - } else if(angle>goal+5&&a<b) { //sasetsu + } else if(angle>goal+5&&a>b) { //sasetsu t.start(); pc.printf("R=%f L=%f\r\n",target_R,target_L); if(t.read_ms()<100) { @@ -195,7 +195,7 @@ t.stop(); pc.printf("R=%f L=%f",target_R,target_L); } - } else if(angle>goal&&angle<=goal+5&&a<b) { + } else if(angle>goal&&angle<=goal+5&&a>b) { tgt(a/3,b/3); } else { @@ -252,6 +252,10 @@ else motorR.Sc(target_R); if(target_L==0) motorL.stop(); else motorL.Sc(target_L); + + new_dist=EC1.getDistance_mm(); + d_dist=new_dist-old_dist; + old_dist=new_dist; double d_x=d_dist*sin(angle); double d_y=d_dist*cos(angle); @@ -261,9 +265,7 @@ if(kai>=3) { - new_dist=EC1.getDistance_mm(); - d_dist=new_dist-old_dist; - old_dist=new_dist; + //double d_x=d_dist*sin(angle);; //double d_y=d_dist*cos(angle);; @@ -302,13 +304,13 @@ susumu_y(1,1,start_x+178);//x,x+178 } if(i==2) { - susumu_ang(1/2,1,45);//x+121.5,x+471.5 + susumu_ang(0.5,1,45);//x+121.5,x+471.5 } if(i==3) { susumu_xl(1,1,728.5);//728.5,1078.5 } if(i==4) { - susumu_ang(1,1/2,0);//850,1372 + susumu_ang(1,0.5,0);//850,1372 } if(i==5) { susumu_y(1,1,1700);//850,1700 @@ -373,7 +375,9 @@ if(i==12) { motorR.stop(); motorL.stop(); - servo.pulsewidth_us(3000); + servo.pulsewidth_us(2100); + wait(1.5); + servo.pulsewidth_us(2400); wait(1); i++; } @@ -405,7 +409,7 @@ denjiben=1; wait(0.5); servo.pulsewidth_us(900); - wait(1); + wait(2); i++; } if(i==16) {