春ロボ1班(元F3RC4班+)
改良版位置補正プログラム動作未確認
Dependencies: mbed move4wheel2 EC CruizCore_R1370P
Diff: movement/movement.cpp
- Revision:
- 10:cf5c159e729a
- Parent:
- 9:63924280272d
--- a/movement/movement.cpp Thu Mar 07 05:47:18 2019 +0000
+++ b/movement/movement.cpp Fri Mar 08 07:09:33 2019 +0000
@@ -172,7 +172,7 @@
//x_base,y_base:超音波センサーで読む壁の座標(y軸並行の壁のx座標/x軸平行の壁のy座標)
double R1=240,R2=240,R3=240,R4=240; //機体の中心から各超音波センサーが付いている面までの距離
- double D1=30,D2=30,D3=30,D4=30; //各超音波センサーが付いている面の中心から各超音波センサーまでの距離
+ double D1=30,D2=0,D3=0,D4=0; //各超音波センサーが付いている面の中心から各超音波センサーまでの距離（時計回りを正とする）
now_angle=gyro.getAngle(); //ジャイロの値読み込み
@@ -199,40 +199,59 @@
} else if(tgt_angle==90) {
if((xy_type==0 || xy_type==2) && pm_typeX==0) {
- info.nowX.usw = x_base - (usw_data1 + R1*cos(now_angle*PI/180) + D1*sin(now_angle*PI/180));
+ info.nowX.usw = x_base - (usw_data1 + R1*cos((now_angle-tgt_angle)*PI/180) + D1*sin(now_angle*PI/180));
} else if((xy_type==0 || xy_type==2) && pm_typeX==1) {
- info.nowX.usw = x_base + (usw_data2 + R2*cos(now_angle*PI/180) + D2*sin(now_angle*PI/180));
+ info.nowX.usw = x_base + (usw_data2 + R2*cos((now_angle-tgt_angle)*PI/180) + D2*sin(now_angle*PI/180));
}
if((xy_type==1 || xy_type==2) && pm_typeY==0) {
- info.nowY.usw = y_base - (usw_data4 + R4*cos(now_angle*PI/180) + D4*sin(now_angle*PI/180));
+ info.nowY.usw = y_base - (usw_data4 + R4*cos((now_angle-tgt_angle)*PI/180) + D4*sin(now_angle*PI/180));
} else if((xy_type==1 || xy_type==2) && pm_typeY==1) {
- info.nowY.usw = y_base + (usw_data3 + R3*cos(now_angle*PI/180) + D3*sin(now_angle*PI/180));
+ info.nowY.usw = y_base + (usw_data3 + R3*cos((now_angle-tgt_angle)*PI/180) + D3*sin(now_angle*PI/180));
}
- } else if(tgt_angle==180) {
+ } else if(tgt_angle==180 || tgt_angle==-180) {
if((xy_type==0 || xy_type==2) && pm_typeX==0) {
- info.nowX.usw = x_base - (usw_data3 + R3*cos(now_angle*PI/180) + D3*sin(now_angle*PI/180));
+ info.nowX.usw = x_base - (usw_data3 + R3*cos((now_angle-tgt_angle)*PI/180) + D3*sin(now_angle*PI/180));
} else if((xy_type==0 || xy_type==2) && pm_typeX==1) {
- info.nowX.usw = x_base + (usw_data4 + R4*cos(now_angle*PI/180) + D4*sin(now_angle*PI/180));
+ info.nowX.usw = x_base + (usw_data4 + R4*cos((now_angle-tgt_angle)*PI/180) + D4*sin(now_angle*PI/180));
}
if((xy_type==1 || xy_type==2) && pm_typeY==0) {
- info.nowY.usw = y_base - (usw_data1+ R1*cos(now_angle*PI/180) + D1*sin(now_angle*PI/180));
+ info.nowY.usw = y_base - (usw_data1+ R1*cos((now_angle-tgt_angle)*PI/180) + D1*sin(now_angle*PI/180));
} else if((xy_type==1 || xy_type==2) && pm_typeY==1) {
- info.nowY.usw = y_base + (usw_data2 + R2*cos(now_angle*PI/180) + D2*sin(now_angle*PI/180));
+ info.nowY.usw = y_base + (usw_data2 + R2*cos((now_angle-tgt_angle)*PI/180) + D2*sin(now_angle*PI/180));
+
+ }
+ } else if(tgt_angle==-90) {
+ if((xy_type==0 || xy_type==2) && pm_typeX==0) {
+
+ info.nowX.usw = x_base - (usw_data2 + R2*cos((now_angle-tgt_angle)*PI/180) + D2*sin(now_angle*PI/180));
+
+ } else if((xy_type==0 || xy_type==2) && pm_typeX==1) {
+
+ info.nowX.usw = x_base + (usw_data1 + R1*cos((now_angle-tgt_angle)*PI/180) + D1*sin(now_angle*PI/180));
+
+ }
+ if((xy_type==1 || xy_type==2) && pm_typeY==0) {
+
+ info.nowY.usw = y_base - (usw_data3 + R3*cos((now_angle-tgt_angle)*PI/180) + D3*sin(now_angle*PI/180));
+
+ } else if((xy_type==1 || xy_type==2) && pm_typeY==1) {
+
+ info.nowY.usw = y_base + (usw_data4 + R4*cos((now_angle-tgt_angle)*PI/180) + D4*sin(now_angle*PI/180));
}
}
@@ -264,16 +283,17 @@
}*/
}
-void enc_correction(int x_select,int y_select){ //エンコーダの座標を超音波センサの座標で上書き
+void enc_correction(int x_select,int y_select) //エンコーダの座標を超音波センサの座標で上書き
+{
//x_select,y_select → (0:上書きしない/1:上書きする)
- if(x_select == 1){
+ if(x_select == 1) {
info.nowX.enc = info.nowX.usw;
}
- if(y_select == 1){
+ if(y_select == 1) {
info.nowY.enc = info.nowY.usw;
}
-
+
}
//ここからそれぞれのプログラム/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -504,24 +524,26 @@
{
//距離に比例させて補正初速度を増加させる。（最大速度を設定しそれ以上は出ないようにする）
- double first_speed, first_speed50 = 200, last_speed = 50, Max_speed = 1000;
+ double first_speed, first_speed50 = 150, last_speed = 25, Max_speed = 500;
double r, R=10; // r:一回補正が入るごとの機体の位置と目標位置の距離(ズレ) R:補正終了とみなす目標位置からの機体の位置のズレ
double out;
calc_xy(tgt_angle, u, v);
-
- r = hypot(now_x - tgt_x, now_y - tgt_y);
+
+ //r = hypot(now_x - tgt_x, now_y - tgt_y);
while(1) { //機体の位置を目標領域（目標座標＋許容誤差）に収める
-
- first_speed = first_speed50 * r / 50;
-
- if(first_speed > Max_speed){
+
+ //first_speed = first_speed50 * r / 50;
+
+ /*if(first_speed > Max_speed){
gogo_straight(u,v,now_x,now_y,tgt_x,tgt_y,Max_speed,Max_speed,5,0.1,10,0.1,500,tgt_angle);
}else{
gogo_straight(u,v,now_x,now_y,tgt_x,tgt_y,first_speed,last_speed,5,0.1,10,0.1,500,tgt_angle);
- }
-
+ }*/
+
+ gogo_straight(u,v,now_x,now_y,tgt_x,tgt_y,first_speed50,last_speed,5,0.1,10,0.1,500,tgt_angle);
+
MaxonControl(0,0,0,0);
calc_xy(tgt_angle, u, v);
@@ -534,19 +556,21 @@
while(1) {
- calc_gyro();
+ //calc_gyro();
+ now_angle=gyro.getAngle();
+ printf("angle = %f\n\r",now_angle);
out = 10 * (tgt_angle - now_angle);
if(out > 300) { //0～179°のときは時計回りに回転
- MaxonControl(300,300,300,300);
+ MaxonControl(-300,-300,-300,-300);
} else if(out < -300) {
- MaxonControl(-300,-300,-300,-300);
+ MaxonControl(300,300,300,300);
} else if(out <= 300 && out > -300) {
- MaxonControl(out,out,out,out);
+ MaxonControl(-out,-out,-out,-out);
}
- if(tgt_angle - 0.5 < now_angle && now_angle < tgt_angle + 0.5) break; //目標角度からの許容誤差内に機体の角度が収まった時、補正終了
+ if(tgt_angle - 1 < now_angle && now_angle < tgt_angle + 1) break; //目標角度からの許容誤差内に機体の角度が収まった時、補正終了
if(id1_value[0] != 1)break;
}
MaxonControl(0,0,0,0);