ROBOSTEP_LIBRARY
F3RC4班 自動機プログラム by巨泉 速度制御ユニット使用の2輪(活かせなかった)、測定輪エンコーダ、MicroInfinityを用いている XY座標を読むことには成功したが、まともに制御できなかったのでノーカン()
Dependencies: CruizCore_R1370P EC delta enc_1multi mbed
Fork of
F3RC915
by 
春ロボ1班(元F3RC4班+)
Diff: main.cpp
- Revision:
- 8:7b368096fed8
- Parent:
- 7:3a7e49ed1162
--- 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