改良版位置補正プログラム動作未確認
Dependencies: mbed move4wheel2 EC CruizCore_R1370P
Diff: movement/movement.cpp
- Revision:
- 6:26724c287387
- Parent:
- 4:317c53a674fa
- Child:
- 7:44ce34007499
diff -r 6cebe1c458a9 -r 26724c287387 movement/movement.cpp --- a/movement/movement.cpp Sat Mar 02 07:18:38 2019 +0000 +++ b/movement/movement.cpp Sat Mar 02 07:48:18 2019 +0000 @@ -11,10 +11,12 @@ #define PI 3.141592 -char ashi_data[4]={0}; +char can_ashileddata[1]= {0}; +int can_ashileddata0_0,can_ashileddata0_1,can_ashileddata0_2,can_ashileddata0_3; +Ec EC1(p22,p21,NC,500,0.05); Ec EC2(p26,p8,NC,500,0.05); -Ec EC1(p22,p21,NC,500,0.05); + Ticker ec_ticker; //ec角速度計算用ticker R1370P gyro(p28,p27); @@ -40,25 +42,26 @@ *info.nowY.usw → 超音波センサーにより求めた機体位置のy座標 */ -typedef struct{ //使用センサーの種類 +typedef struct { //使用センサーの種類 double usw; //超音波センサー double enc; //エンコーダ double gyro; //ジャイロ //double line;//ラインセンサー -}robo_sensor; +} robo_sensor; -typedef struct{ //機体情報の種類 +typedef struct { //機体情報の種類 robo_sensor angle; //←機体角度は超音波センサーやラインセンサーからも算出可能なので一応格納先を用意したが、ジャイロの値を完全に信用してもいいかも robo_sensor nowX; robo_sensor nowY; -}robo_data; +} robo_data; -robo_data info={{0,0,0},{0,0,0},{0,0,0}}; //全てのデータを0に初期化 +robo_data info= {{0,0,0},{0,0,0},{0,0,0}}; //全てのデータを0に初期化 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -void UserLoopSetting_sensor(){ - +void UserLoopSetting_sensor() +{ + gyro.initialize(); ec_ticker.attach(&calOmega,0.05); //0.05秒間隔で角速度を計算 EC1.setDiameter_mm(25.5); @@ -91,40 +94,43 @@ else if(fabs(BL)>=fabs(FL)&&fabs(BL)>=fabs(BR)&&fabs(BL)>=fabs(FR))output(Max*FL/fabs(BL),Max*BL/fabs(BL),Max*BR/fabs(BL),Max*FR/fabs(BL)); else if(fabs(BR)>=fabs(FL)&&fabs(BR)>=fabs(BL)&&fabs(BR)>=fabs(FR))output(Max*FL/fabs(BR),Max*BL/fabs(BR),Max*BR/fabs(BR),Max*FR/fabs(BR)); else output(Max*FL/fabs(FR),Max*BL/fabs(FR),Max*BR/fabs(FR),Max*FR/fabs(FR)); - } else { + } else { output(FL,BL,BR,FR); } } -void ashi_led(){ - - if(now_angle > -1 && now_angle < 1){ - ashi_data[0] = 1; - }else{ - ashi_data[0] = 0; +void ashi_led() +{ + + if(now_angle > -1 && now_angle < 1) { + can_ashileddata0_0 = 1; + } else { + can_ashileddata0_0 = 0; } - - if(now_angle > 350){ - ashi_data[1] = 1; - }else{ - ashi_data[1] = 0; + + if(now_angle > 350) { + can_ashileddata0_1 = 1; + } else { + can_ashileddata0_1 = 0; } - - if(now_x > -1 && now_x < 1){ - - ashi_data[2] = 1; - }else{ - ashi_data[2] = 0; + + if(now_x > -1 && now_x < 1) { + can_ashileddata0_2 = 1; + } else { + can_ashileddata0_2 = 0; } - - if(now_y > -1 && now_y < 1){ - ashi_data[3] = 1; - }else{ - ashi_data[3] = 0; + + if(now_y > -1 && now_y < 1) { + can_ashileddata0_3 = 1; + } else { + can_ashileddata0_3 = 0; } + + can_ashileddata[0] = (can_ashileddata0_0<<7 | can_ashileddata0_1<<6 | can_ashileddata0_2<<5 | can_ashileddata0_3<<4); } -void calc_gyro(){ +void calc_gyro() +{ now_angle=gyro.getAngle(); //ジャイロの値読み込み } @@ -145,109 +151,112 @@ info.nowY.enc = info.nowY.enc - d_y; //微小時間毎に座標に加算 } -void set_cond(int t, int px, double bx, int py, double by){ //超音波センサーを使用するときの条件設定関数 +void set_cond(int t, int px, double bx, int py, double by) //超音波センサーを使用するときの条件設定関数 +{ //引数の詳細は関数"calc_xy_usw"参照 - - xy_type = t; - - pm_typeX = px; - x_base = bx; - - pm_typeY = py; - y_base = by; + + xy_type = t; + + pm_typeX = px; + x_base = bx; + + pm_typeY = py; + y_base = by; } -void calc_xy_usw(double tgt_angle){ //超音波センサーによる座標計算(機体が旋回する場合はこの方法による座標計算は出来ない) +void calc_xy_usw(double tgt_angle) //超音波センサーによる座標計算(機体が旋回する場合はこの方法による座標計算は出来ない) +{ //tgt_angle:機体の目標角度(運動初期角度と同じ/今大会では0,90,180のみ) //xy_type:(0:Y軸平行の壁を読む/1:X軸平行の壁を読む/2:X,Y軸平行の壁を共に読む) //pm_typeX,pm_typeY:(0:各軸正方向側の壁を読む/1:各軸負方向側の壁を読む) //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; //各超音波センサーが付いている面の中心から各超音波センサーまでの距離 - + now_angle=gyro.getAngle(); //ジャイロの値読み込み - - if(tgt_angle==0){ - if((xy_type==0 || xy_type==2) && pm_typeX==0){ - - info.nowX.usw = x_base - (usw_data4 + R4*cos(now_angle*PI/180) + D4*sin(now_angle*PI/180)); - - }else if((xy_type==0 || xy_type==2) && pm_typeX==1){ - - info.nowX.usw = x_base + (usw_data3 + R3*cos(now_angle*PI/180) + D3*sin(now_angle*PI/180)); - - } - if((xy_type==1 || xy_type==2) && pm_typeY==0){ - - info.nowY.usw = y_base - (usw_data2 + R2*cos(now_angle*PI/180) + D2*sin(now_angle*PI/180)); - - }else if((xy_type==1 || xy_type==2) && pm_typeY==1){ - - info.nowY.usw = y_base + (usw_data1 + R1*cos(now_angle*PI/180) + D1*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_data1 + R1*cos(now_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)); - - } - 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)); - - }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)); - - } - - }else if(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)); - - }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)); - - } - 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)); - - }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)); - - } + + if(tgt_angle==0) { + if((xy_type==0 || xy_type==2) && pm_typeX==0) { + + info.nowX.usw = x_base - (usw_data4 + R4*cos(now_angle*PI/180) + D4*sin(now_angle*PI/180)); + + } else if((xy_type==0 || xy_type==2) && pm_typeX==1) { + + info.nowX.usw = x_base + (usw_data3 + R3*cos(now_angle*PI/180) + D3*sin(now_angle*PI/180)); + + } + if((xy_type==1 || xy_type==2) && pm_typeY==0) { + + info.nowY.usw = y_base - (usw_data2 + R2*cos(now_angle*PI/180) + D2*sin(now_angle*PI/180)); + + } else if((xy_type==1 || xy_type==2) && pm_typeY==1) { + + info.nowY.usw = y_base + (usw_data1 + R1*cos(now_angle*PI/180) + D1*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_data1 + R1*cos(now_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)); + + } + 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)); + + } 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)); + + } + + } else if(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)); + + } 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)); + + } + 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)); + + } 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)); + + } } } -void calc_xy(double target_angle, double u,double v){ +void calc_xy(double target_angle, double u,double v) +{ //エンコーダにより求めた機体の座標と超音波センサーにより求めた機体の座標を(エンコーダ : 超音波 = u : 1-u / v : 1-v)の割合で混ぜて now_x,now_y に代入する - - calc_xy_enc(); - - if(u != 1 || v != 1){ - calc_xy_usw(target_angle); //エンコーダの値しか使用しない場合は超音波センサーによる座標計算は行わずに計算量を減らす。 - } - + + calc_xy_enc(); + + if(u != 1 || v != 1) { + calc_xy_usw(target_angle); //エンコーダの値しか使用しない場合は超音波センサーによる座標計算は行わずに計算量を減らす。 + } + now_x = u * info.nowX.enc + (1-u) * info.nowX.usw; now_y = v * info.nowY.enc + (1-v) * info.nowY.usw; - + /*if(now_x >-1 && now_x <1 && now_y >-1 && now_y <1){ //スタート時の0合わせ用 ec_led = 1; }else{ ec_led = 0; } - + if(now_angle >-0.5 && now_angle <0.5){ gyro_led = 1; }else{ @@ -391,12 +400,12 @@ base(GetMotorOut(0),GetMotorOut(1),GetMotorOut(2),GetMotorOut(3),4095); //m1~m4に代入 //debug_printf("t=%d (0)=%f (1)=%f (2)=%f (3)=%f\n\r",t,GetMotorOut(0),GetMotorOut(1),GetMotorOut(2),GetMotorOut(3)); - + if(((plotx[t+1] - now_x)*(plotx[t+1] - plotx[t]) + (ploty[t+1] - now_y)*(ploty[t+1] - ploty[t])) < 0)t++; MaxonControl(m1,m2,m3,m4); //出力 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; } @@ -417,7 +426,7 @@ r_setPDparam(r_p,r_d); //機体角度と目標角度の誤差を埋めるPD制御のパラメータ設定関数 set_r_out(r_out_max); //旋回時の最大出力値設定関数 set_target_angle(target_angle); //機体目標角度設定関数 - + while (1) { calc_xy(target_angle,u,v); @@ -430,50 +439,51 @@ base(GetMotorOut(0),GetMotorOut(1),GetMotorOut(2),GetMotorOut(3),4095); //printf("m1=%d, m2=%d, m3=%d, m4=%d\r\n",m_1,m_2,m_3,m_4); - + MaxonControl(m1,m2,m3,m4); 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; + 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:補正終了とみなす目標位置からの機体の位置のズレ double out; - + calc_xy(tgt_angle, u, v); - - while(1){ //機体の位置を目標領域(目標座標+許容誤差)に収める + + while(1) { //機体の位置を目標領域(目標座標+許容誤差)に収める gogo_straight(u,v,now_x,now_y,tgt_x,tgt_y,200,50,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){ - + + while(1) { + calc_gyro(); - + 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) { + } else if(out < -300) { + MaxonControl(-300,-300,-300,-300); + } else if(out <= 300 && out > -300) { MaxonControl(out,out,out,out); } - + if(tgt_angle - 0.5 < now_angle && now_angle < tgt_angle + 0.5) break; //目標角度からの許容誤差内に機体の角度が収まった時、補正終了 - + } MaxonControl(0,0,0,0); } \ No newline at end of file