![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
F3RC9/13 1317
Dependencies: CruizCore_R1370P EC delta enc_1multi mbed
Diff: main.cpp
- Revision:
- 5:0160b73f3d9e
- Parent:
- 4:e3d739bf09b4
- Child:
- 6:7ec6c3d3a30a
--- a/main.cpp Tue Sep 11 06:42:51 2018 +0000 +++ b/main.cpp Wed Sep 12 05:35:59 2018 +0000 @@ -36,8 +36,14 @@ double new_dist=0; double old_dist=0; double d_dist=0; -double x=185; -double y=150;//start point//keisann wo sinaosu hitsuyouga arimasu +double x; +double y; +double asari_x=900; +double asari_y=2500;//asariwo toru tekisetsuna zahyouwo kaeraremasu +double goal_x=1120; +double goal_y=1700; +double start_x=185; +double start_y=150; Timer t; int i=1; @@ -205,6 +211,10 @@ int main(void) { + + gyro.initialize(); //main関数の最初に一度だけ実行 + gyro.acc_offset(); //やってもやらなくてもいい + printf("start\r\n"); motor_tick.attach(&calOmega,0.05); motorR.setDOconstant(34.1); @@ -213,25 +223,26 @@ motorR.setPDparam(0,0);//pd//akirameta - gyro.initialize(); //main関数の最初に一度だけ実行 EC1.setDiameter_mm(50);//sokuteirinnhannkei double getDistance_mm(); //int EC1.getCount()=0; + void reset (); EC1.reset(); - void reset (); servo.period_ms(20); motor_f.period_ms(30); motor_b.period_ms(30);//arm + x=start_x; + y=start_y;//start point//keisann wo sinaosu hitsuyouga arimasu while(1) { // motorR.turnF(0.3); //motorL.turnF(0.3);//for debug motorR.Sc(target_R); motorL.Sc(target_L);//target_R,target_Lの値を変えることで直進・後退・右回転・左回転を行う - angle=gyro.getAngle(); //角度の値を受け取る + angle=gyro.getAngle()*(-1); //角度の値を受け取る EC1.getDistance_mm(); // EC1.CalOmega(); @@ -285,22 +296,22 @@ y=2500; i=14; } - } + }//x,y } if(i==1) { - susumu_y(1,1,407); + susumu_y(1,1,start_x+178);//x,x+178 } if(i==2) { - susumu_ang(1/3,1,45); + susumu_ang(1/2,1,45);//x+121.5,x+471.5 } if(i==3) { - susumu_xl(1,1,709); + susumu_xl(1,1,728.5);//728.5,1078.5 } if(i==4) { - susumu_ang(1,1/3,0); + susumu_ang(1,1/2,0);//850,1372 } if(i==5) { - susumu_y(1,1,1700); + susumu_y(1,1,1700);//850,1700 } if(i==6) { motorR.stop(); @@ -348,22 +359,22 @@ } }//kakudo tyousei if(i==8) { - susumu_xr(1,1,700); + susumu_xr(1,1,700);//700,1700 } if(i==9) { - susumu_ang(1/3,1,0); + susumu_ang(1/3,1,0);//390,2010 } if(i==10) { - susumu_y(1,1,2200); + susumu_y(1,1,asari_y-310);//390,y-310 } if(i==11) { - susumu_ang(1/3,1,90); + susumu_ang(1/3,1,90);//700,y } if(i==12) { motorR.stop(); motorL.stop(); - servo.pulsewidth_us(1000); - wait(0.5); + servo.pulsewidth_us(3000); + wait(1); i++; } if(i==13) { @@ -385,7 +396,7 @@ } } if(i==14) { - susumu_xl(1,1,1000); + susumu_xl(1,1,asari_x);//x,y } if(i==15) { motorR.stop(); @@ -393,29 +404,28 @@ wait(0.5); denjiben=1; wait(0.5); - servo.pulsewidth_us(1500); - wait(0.5); + servo.pulsewidth_us(900); + wait(1); i++; } if(i==16) { - susumu_xr(-1,-1,700); + susumu_xr(-1,-1,700);//700,y } if(i==17) { - susumu_ang(-1/3,-1,0); + susumu_ang(-1/3,-1,0);//390,y-310 } if(i==18) { - susumu_y(-1,-1,2000); + susumu_y(-1,-1,goal_y+310);//390,y+310 } if(i==19) { - susumu_ang(-1/3,-1,-90); + susumu_ang(-1/3,-1,-90);//700,y } if(i==20) { - susumu_xl(-1,-1,1100); + susumu_xl(-1,-1,goal_x);//x,y } if(i==21) { motorR.stop(); motorL.stop(); - servo.pulsewidth_us(1500); wait(0.5); denjiben=0; wait(0.5);