3/20 2:04
Dependencies: mbed move4wheel2 EC CruizCore_R1370P
Diff: movement/movement.cpp
- Revision:
- 10:c741191360de
- Parent:
- 9:7667dcfc7ce5
- Child:
- 11:9db93bce4eef
--- a/movement/movement.cpp Wed Mar 13 03:24:49 2019 +0000 +++ b/movement/movement.cpp Wed Mar 13 04:23:18 2019 +0000 @@ -68,7 +68,15 @@ ec_ticker.attach(&calOmega,0.05); //0.05秒間隔で角速度を計算 EC1.setDiameter_mm(25.5); EC2.setDiameter_mm(25.5); //測定輪半径//後で測定 - info.nowX.enc = -2962; //初期位置の設定 +} +void UserLoopSetting_enc_right() +{ + info.nowX.enc = 2962; //エンコーダの初期位置の設定(右側フィールド) + info.nowY.enc = 3500; +} +void UserLoopSetting_enc_left() +{ + info.nowX.enc = -2962; //エンコーダの初期位置の設定(左側フィールド) info.nowY.enc = 3500; } @@ -528,30 +536,36 @@ 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 first_speed, first_speed50 = 100,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 out; + double tgt_xx, tgt_yy; 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) { //機体の位置を目標領域(目標座標+許容誤差)に収める - if(id1_value[0] != 1)break; - if(id1_value[6] != flag)break; + first_speed = (first_speed50 - last_speed) * r / 50 + last_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); + if(r < 20){ //目標地点までの距離が1センチ以上2センチ未満のとき + gogo_straight(u,v,now_x,now_y,tgt_x,tgt_y,30,30,5,0.1,10,0.1,500,tgt_angle); + }else if(r < 30){ ////目標地点までの距離が2センチ以上3センチ未満のとき + gogo_straight(u,v,now_x,now_y,tgt_x,tgt_y,40,40,5,0.1,10,0.1,500,tgt_angle); + }else if(first_speed > Max_speed){ + tgt_xx = ((Max_speed - last_speed)*now_x + (first_speed - Max_speed)*tgt_x)/(first_speed-last_speed); + tgt_yy = ((Max_speed - last_speed)*now_y + (first_speed - Max_speed)*tgt_y)/(first_speed-last_speed); + gogo_straight(u,v,now_x,now_y,tgt_xx,tgt_yy,Max_speed,Max_speed,5,0.1,10,0.1,500,tgt_angle); + gogo_straight(u,v,tgt_xx,tgt_yy,tgt_x,tgt_y,Max_speed,last_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); + //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); @@ -560,13 +574,11 @@ r=hypot(now_x - tgt_x, now_y - tgt_y); if(r < R) break; + if(id1_value[0] != 1)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); @@ -582,6 +594,12 @@ } if(tgt_angle - 1 < now_angle && now_angle < tgt_angle + 1) break; //目標角度からの許容誤差内に機体の角度が収まった時、補正終了 + if(id1_value[0] != 1)break; } MaxonControl(0,0,0,0); -} \ No newline at end of file +} + +/*----------------------コメント------------------------*/ +/* 3/10(日):pos_correctionの中身を一部修正しました。(引数に変更はありません。) + +/*---------------------------------------------------*/ \ No newline at end of file