Diff: movement/movement.cpp

Revision:

11:9db93bce4eef

Parent:

10:c741191360de

Child:

13:f9867c3b97b0

--- 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:５センチのズレを補正するときの補正初速度 / 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の中身を一部修正しました。（引数に変更はありません。）