3/19 23:30
Dependencies: mbed move4wheel2 EC CruizCore_R1370P
Diff: movement/movement.cpp
- Revision:
- 11:9db93bce4eef
- Parent:
- 10:c741191360de
diff -r c741191360de -r 9db93bce4eef movement/movement.cpp --- a/movement/movement.cpp Wed Mar 13 04:23:18 2019 +0000 +++ b/movement/movement.cpp Tue Mar 19 02:30:34 2019 +0000 @@ -71,12 +71,12 @@ } void UserLoopSetting_enc_right() { - info.nowX.enc = 2962; //エンコーダの初期位置の設定(右側フィールド) + info.nowX.enc = 3112; //エンコーダの初期位置の設定(右側フィールド) info.nowY.enc = 3500; } void UserLoopSetting_enc_left() { - info.nowX.enc = -2962; //エンコーダの初期位置の設定(左側フィールド) + info.nowX.enc = -3112; //エンコーダの初期位置の設定(左側フィールド) info.nowY.enc = 3500; } @@ -272,6 +272,11 @@ //エンコーダにより求めた機体の座標と超音波センサーにより求めた機体の座標を(エンコーダ : 超音波 = u : 1-u / v : 1-v)の割合で混ぜて now_x,now_y に代入する calc_xy_enc(); + + if(usw_flag == 1){ + u = 1; + v = 1; + } if(u != 1 || v != 1) { calc_xy_usw(target_angle); //エンコーダの値しか使用しない場合は超音波センサーによる座標計算は行わずに計算量を減らす。 @@ -533,14 +538,13 @@ } 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)を入れる) +void pos_correction(double tgt_x, double tgt_y, double tgt_angle, double u, double v, double R) //改良版 位置補正(使用前にMaxonControl(0,0,0,0)を入れる) { //距離に比例的に補正初速度を増加させる。(最大速度を設定しそれ以上は出ないようにする/初期速度が目標速度を下回らないようにする) - double first_speed, first_speed50 = 100,last_speed = 50, Max_speed = 300; + double first_speed, first_speed50 = 150,last_speed = 50, Max_speed = 300; //first_speed50:5センチのズレを補正するときの補正初速度 / last_speed:目標速度 / first_speed:first_speed50とlast_speedを元に計算した実際の補正初速度 / Max_speed:補正速度の上限 - double r, R=10; // r:一回補正が入るごとの機体の位置と目標位置の距離(ズレ) R:補正終了とみなす目標位置からの機体の位置のズレ + double r; // r:一回補正が入るごとの機体の位置と目標位置の距離(ズレ) R:補正終了とみなす目標位置からの機体の位置のズレ double out; double tgt_xx, tgt_yy; @@ -581,7 +585,7 @@ //calc_gyro(); now_angle=gyro.getAngle(); - printf("angle = %f\n\r",now_angle); + //printf("angle = %f\n\r",now_angle); out = 10 * (tgt_angle - now_angle); @@ -599,6 +603,7 @@ MaxonControl(0,0,0,0); } + /*----------------------コメント------------------------*/ /* 3/10(日):pos_correctionの中身を一部修正しました。(引数に変更はありません。)