F3RC4班 自動機プログラム by巨泉 速度制御ユニット使用の2輪(活かせなかった)、測定輪エンコーダ、MicroInfinityを用いている XY座標を読むことには成功したが、まともに制御できなかったのでノーカン()
Dependencies: CruizCore_R1370P EC delta enc_1multi mbed
Fork of F3RC915 by
Revision 8:7b368096fed8, committed 2018-09-18
- Comitter:
- aoikoizumi
- Date:
- Tue Sep 18 04:49:40 2018 +0000
- Parent:
- 7:3a7e49ed1162
- Commit message:
- F3RC?4?????
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r 3a7e49ed1162 -r 7b368096fed8 main.cpp --- a/main.cpp Sat Sep 15 07:54:13 2018 +0000 +++ b/main.cpp Tue Sep 18 04:49:40 2018 +0000 @@ -66,7 +66,7 @@ void susumu_y(double ay,double by,double goaly)//y zahyou wo motiita susumikata subete { //pc.printf("ay=%f by=%f y=%f goaly=%f\r\n",ay,by,y,goaly); - if(y<goaly-100&&ay>=0) { + if(y<goaly-120&&ay>=0) { t.start(); //pc.printf("R=%f L=%f\r\n",target_R,target_L); if(t.read_ms()<150) { @@ -80,11 +80,11 @@ t.stop(); //pc.printf("R=%f L=%f",target_R,target_L); } - } else if(y<goaly&&y>=goaly-100&&ay>=0) { + } else if(y<goaly-20&&y>=goaly-120&&ay>=0) { tgt(ay/2,by/2); - } else if(y>goaly+100&&ay<0) { + } else if(y>goaly+120&&ay<0) { t.start(); //pc.printf("R=%f L=%f\r\n",target_R,target_L); if(t.read_ms()<150) { @@ -98,7 +98,7 @@ t.stop(); //pc.printf("R=%f L=%f",target_R,target_L); } - } else if(y>goaly&&y<=goaly+100&&ay<0) { + } else if(y>goaly+20&&y<=goaly+120&&ay<0) { tgt(ay/2,by/2); } else { i++; @@ -112,7 +112,7 @@ void susumu_xl(double axl,double bxl,double goalxl)//hidarikara mokuhyoutenn ni toutatsu surutameno dousa { - if(x<goalxl-100) { + if(x<goalxl-120) { t.start(); //pc.printf("R=%f L=%f\r\n",target_R,target_L); if(t.read_ms()<150) { @@ -126,7 +126,7 @@ t.stop(); ////pc.printf("R=%f L=%f",target_R,target_L); } - } else if(x<goalxl&&x>=goalxl-100) { + } else if(x<goalxl-20&&x>=goalxl-120) { tgt(axl/2,bxl/2); } else { @@ -139,7 +139,7 @@ void susumu_xr(double axr,double bxr,double goalxr)//migikara mokuhyoutenn ni toutatsu surutameno dousa { - if(x>goalxr+100) { + if(x>goalxr+120) { t.start(); //pc.printf("R=%f L=%f\r\n",target_R,target_L); if(t.read_ms()<150) { @@ -153,7 +153,7 @@ t.stop(); ////pc.printf("R=%f L=%f",target_R,target_L); } - } else if(x>goalxr&&x<=goalxr+100) { + } else if(x>goalxr+20&&x<=goalxr+120) { tgt(axr/2,bxr/2); } else { @@ -179,7 +179,7 @@ t.stop(); ////pc.printf("R=%f L=%f",target_R,target_L); } - } else if(angle<goal&&angle>=goal-30&&a<b) { + } else if(angle<goal-5&&angle>=goal-30&&a<b) { tgt(a/2,b/2); } else if(angle>goal+30&&a>b) { //sasetsu @@ -196,7 +196,7 @@ t.stop(); ////pc.printf("R=%f L=%f",target_R,target_L); } - } else if(angle>goal&&angle<=goal+30&&a>b) { + } else if(angle>goal+5&&angle<=goal+30&&a>b) { tgt(a/2,b/2); } else { @@ -218,7 +218,7 @@ printf("start\r\n"); motor_tick.attach(&calOmega,0.05); - motorR.setDOconstant(34.1); + motorR.setDOconstant(31.2); motorL.setDOconstant(30);//c motorR.setPDparam(0,0); motorL.setPDparam(0,0);//pd//akirameta @@ -237,7 +237,7 @@ motor_b.period_ms(30);//arm x=start_x; y=start_y;//start point//keisann wo sinaosu hitsuyouga arimasu - //servo.pulsewidth_us(950); + while(1) { //target_R=(-1)*BASIC_SPEED; //target_L=(-1)*BASIC_SPEED; @@ -274,9 +274,9 @@ //y=y+d_y; pc.printf("R=%f L=%f",target_R,target_L); - pc.printf("omg_R=%f omg_L=%f \r\n",motorR.getOmega(),motorL.getOmega()); + //pc.printf("omg_R=%f omg_L=%f \r\n",motorR.getOmega(),motorL.getOmega()); pc.printf("i=%d\r\n",i); - //pc.printf("EC1=%f",EC1.getDistance_mm(),EC1.getCount()); + pc.printf("EC1=%f",EC1.getDistance_mm(),EC1.getCount()); pc.printf("x=%f y=%f",x,y); pc.printf("angle=%f\r\n",angle); //機体の進む方向、右モーターの角速度、左モーターの角速度を表示 @@ -284,6 +284,7 @@ } kai++; if(i==0) { + servo.pulsewidth_us(950); if(reset_f.read()==0&&button.read()==0) { wait(0.05); if(reset_f.read()==0&&button.read()==0) { @@ -299,24 +300,25 @@ x=180; y=2674; - i=6; + i=12; } }//x,y } if(i==1) { - susumu_y(1,1,857);//x,x+178 + servo.pulsewidth_us(950); + susumu_y(0.8,0.8,857);//x,x+178 } if(i==2) { - susumu_ang(0,0.7,90);//x+155,x+471.5 + susumu_ang(0,0.7,90);//x+155,x+471.5//219.2,65.8//534.2,730.8//180,576.6 } if(i==3) { - susumu_xl(0.7,0.7,745);//728.5,1078.5 + susumu_xl(0.8,0.8,745);//728.5,1078.5 } if(i==4) { susumu_ang(0.7,0,0);//850,1372 } if(i==5) { - susumu_y(0.7,0.7,2016);//850,1700 + susumu_y(0.8,0.8,2016);//850,1700 } if(i==6) { // motorR.stop(); @@ -343,7 +345,7 @@ if(i==7) { wait(2); t.start(); - if(t<1) { + if(t<2) { motor_f=0; motor_b=0.82; @@ -388,9 +390,9 @@ if(i==12) { motorR.stop(); motorL.stop(); - //servo.pulsewidth_us(2100); + servo.pulsewidth_us(2100); wait(1.5); - //servo.pulsewidth_us(2400); + servo.pulsewidth_us(2400); wait(2); i++; } @@ -413,7 +415,7 @@ } } if(i==14) { - susumu_xl(0.6,0.6,asari_x);//x,y + susumu_xl(0.8,0.8,asari_x);//x,y } if(i==15) { motorR.stop(); @@ -423,7 +425,7 @@ wait(0.5); denjiben=0; wait(0.5); - //servo.pulsewidth_us(900); + servo.pulsewidth_us(900); wait(2); i++; } @@ -431,13 +433,13 @@ susumu_xr(-1,-1,555);//700,y } if(i==17) { - susumu_ang(0,-0.6,0);//390,y-310 + susumu_ang(0,-0.7,0);//390,y-310 } if(i==18) { susumu_y(-1,-1,goal_y+155);//390,y+310 } if(i==19) { - susumu_ang(0,-0.6,-90);//700,y + susumu_ang(0,-0.7,-90);//700,y } if(i==20) { susumu_xl(-1,-1,goal_x);//x,y