Diff: movement/movement.cpp

Revision:

10:c741191360de

Parent:

9:7667dcfc7ce5

--- 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:５センチのズレを補正するときの補正初速度 / 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){ //目標地点までの距離が１センチ以上２センチ未満のとき
+            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){ ////目標地点までの距離が２センチ以上３センチ未満のとき
+            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