3/20 2:04
Dependencies: mbed move4wheel2 EC CruizCore_R1370P
Diff: movement/movement.cpp
- Revision:
- 13:f9867c3b97b0
- Parent:
- 11:9db93bce4eef
--- a/movement/movement.cpp Tue Mar 19 14:31:24 2019 +0000 +++ b/movement/movement.cpp Tue Mar 19 17:03:27 2019 +0000 @@ -34,6 +34,7 @@ double xy_type,pm_typeX,pm_typeY,x_base,y_base; int flag; +int RL_mode; ///////////////////機体情報をメンバとする構造体"robo_data"と構造体型変数info(←この変数に各センサーにより求めた機体情報(機体位置/機体角度)を格納する)の宣言///////////////// @@ -73,11 +74,13 @@ { info.nowX.enc = 3112; //エンコーダの初期位置の設定(右側フィールド) info.nowY.enc = 3500; + RL_mode = 0; } void UserLoopSetting_enc_left() { info.nowX.enc = -3112; //エンコーダの初期位置の設定(左側フィールド) info.nowY.enc = 3500; + RL_mode = 1; } void calOmega() //角速度計算関数 @@ -124,16 +127,30 @@ can_ashileddata0_1 = 0; } - if(now_x > -1 && now_x < 1) { - can_ashileddata0_2 = 1; - } else { - can_ashileddata0_2 = 0; - } + if(RL_mode == 0) { + if(now_x > 3110 && now_x < 3114) { + can_ashileddata0_2 = 1; + } else { + can_ashileddata0_2 = 0; + } - if(now_y > -1 && now_y < 1) { - can_ashileddata0_3 = 1; - } else { - can_ashileddata0_3 = 0; + if(now_y > 3498 && now_y < 3502) { + can_ashileddata0_3 = 1; + } else { + can_ashileddata0_3 = 0; + } + } else if(RL_mode == 1) { + if(now_x > -3114 && now_x < -3110) { + can_ashileddata0_2 = 1; + } else { + can_ashileddata0_2 = 0; + } + + if(now_y > 3498 && now_y < 3502) { + 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); @@ -272,10 +289,10 @@ //エンコーダにより求めた機体の座標と超音波センサーにより求めた機体の座標を(エンコーダ : 超音波 = u : 1-u / v : 1-v)の割合で混ぜて now_x,now_y に代入する calc_xy_enc(); - - if(usw_flag == 1){ - u = 1; - v = 1; + + if(usw_flag == 1) { + u = 1; + v = 1; } if(u != 1 || v != 1) { @@ -542,7 +559,7 @@ { //距離に比例的に補正初速度を増加させる。(最大速度を設定しそれ以上は出ないようにする/初期速度が目標速度を下回らないようにする) - double first_speed, first_speed50 = 150,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:一回補正が入るごとの機体の位置と目標位置の距離(ズレ) R:補正終了とみなす目標位置からの機体の位置のズレ double out; @@ -556,20 +573,20 @@ first_speed = (first_speed50 - last_speed) * r / 50 + last_speed; - if(r < 20){ //目標地点までの距離が1センチ以上2センチ未満のとき + 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センチ未満のとき + } 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){ + } 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{ + } 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);