3/20 2:04
Dependencies: mbed move4wheel2 EC CruizCore_R1370P
Diff: movement/movement.cpp
- Revision:
- 9:7667dcfc7ce5
- Parent:
- 8:2ba338b4590e
- Child:
- 10:c741191360de
--- a/movement/movement.cpp Thu Mar 07 02:54:55 2019 +0000 +++ b/movement/movement.cpp Wed Mar 13 03:24:49 2019 +0000 @@ -33,6 +33,8 @@ double xy_type,pm_typeX,pm_typeY,x_base,y_base; +int flag; + ///////////////////機体情報をメンバとする構造体"robo_data"と構造体型変数info(←この変数に各センサーにより求めた機体情報(機体位置/機体角度)を格納する)の宣言///////////////// /*「info.(機体情報の種類).(使用センサーの種類)」に各情報を格納する @@ -172,7 +174,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 +201,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,6 +285,19 @@ }*/ } +void enc_correction(int x_select,int y_select) //エンコーダの座標を超音波センサの座標で上書き +{ +//x_select,y_select → (0:上書きしない/1:上書きする) + + if(x_select == 1) { + info.nowX.enc = info.nowX.usw; + } + if(y_select == 1) { + info.nowY.enc = info.nowY.usw; + } + +} + //ここからそれぞれのプログラム///////////////////////////////////////////////////////////////////////////////////////////////////////////////// //now_x(現在のx座標),now_y(現在のy座標),now_angle(機体角度(ラジアンではない)(0~360や-180~180とは限らない))(反時計回りが正) //ジャイロの出力は角度だが三角関数はラジアンとして計算する @@ -392,6 +426,9 @@ while(1) { + if(id1_value[0] != 1)break; + if(id1_value[6] != flag)break; + calc_xy(target_angle,u,v); XYRmotorout(plotx[t],ploty[t],plotx[t+1],ploty[t+1],&x_out,&y_out,&r_out,speed,speed); @@ -407,7 +444,6 @@ debug_printf("t=%d m1=%d m2=%d m3=%d m4=%d x=%f y=%f angle=%f\n\r",t,m1,m2,m3,m4,now_x,now_y,now_angle); if(t == (90/theta))break; - if(id1_value[0] != 1)break; } } @@ -429,6 +465,9 @@ while (1) { + if(id1_value[0] != 1)break; + if(id1_value[6] != flag)break; + calc_xy(target_angle,u,v); XYRmotorout(x1_point,y1_point,x2_point,y2_point,&x_out,&y_out,&r_out,speed1,speed2); @@ -444,11 +483,10 @@ debug_printf("m1=%d m2=%d m3=%d m4=%d x=%f y=%f angle=%f\n\r",m1,m2,m3,m4,now_x,now_y,now_angle); if(((x2_point - now_x)*(x2_point - x1_point) + (y2_point - now_y)*(y2_point - y1_point)) < 0)break; - if(id1_value[0] != 1)break; } } -void pos_correction(double tgt_x, double tgt_y, double tgt_angle, double u, double v) //位置補正(使用前にMaxonControl(0,0,0,0)を入れる) +/*void pos_correction(double tgt_x, double tgt_y, double tgt_angle, double u, double v) //位置補正(使用前にMaxonControl(0,0,0,0)を入れる) { double r, R=10; // r:一回補正が入るごとの機体の位置と目標位置の距離(ズレ) R:補正終了とみなす目標位置からの機体の位置のズレ @@ -486,4 +524,64 @@ if(id1_value[0] != 1)break; } MaxonControl(0,0,0,0); +}*/ + +void pos_correction(double tgt_x, double tgt_y, double tgt_angle, double u, double v) //改良版 位置補正(使用前にMaxonControl(0,0,0,0)を入れる) +{ +//距離に比例させて補正初速度を増加させる。(最大速度を設定しそれ以上は出ないようにする) + + 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); + + while(1) { //機体の位置を目標領域(目標座標+許容誤差)に収める + + if(id1_value[0] != 1)break; + if(id1_value[6] != flag)break; + + //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); + + r=hypot(now_x - tgt_x, now_y - tgt_y); + + if(r < R) break; + } + + while(1) { + + if(id1_value[0] != 1)break; + if(id1_value[6] != flag)break; + + //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); + } else if(out < -300) { + MaxonControl(300,300,300,300); + } else if(out <= 300 && out > -300) { + MaxonControl(-out,-out,-out,-out); + } + + if(tgt_angle - 1 < now_angle && now_angle < tgt_angle + 1) break; //目標角度からの許容誤差内に機体の角度が収まった時、補正終了 + } + MaxonControl(0,0,0,0); } \ No newline at end of file