Diff: movement/movement.cpp

Revision:

11:8eda88c96bb0

Parent:

10:cf5c159e729a

diff -r cf5c159e729a -r 8eda88c96bb0 movement/movement.cpp
--- a/movement/movement.cpp	Fri Mar 08 07:09:33 2019 +0000
+++ b/movement/movement.cpp	Sat Dec 26 09:00:06 2020 +0000
@@ -66,7 +66,7 @@
     ec_ticker.attach(&calOmega,0.05);  //0.05秒間隔で角速度を計算
     EC1.setDiameter_mm(25.5);
     EC2.setDiameter_mm(25.5);  //測定輪半径//後で測定
-    info.nowX.enc = -2962; //初期位置の設定
+    info.nowX.enc = -3112; //初期位置の設定
     info.nowY.enc = 3500;
 }
 
@@ -476,7 +476,7 @@
         debug_printf("m1=%d m2=%d m3=%d m4=%d x=%f y=%f angle=%f\n\r",m1,m2,m3,m4,now_x,now_y,now_angle);
 
         if(((x2_point - now_x)*(x2_point - x1_point) + (y2_point - now_y)*(y2_point - y1_point)) < 0)break;
-        if(id1_value[0] != 1)break;
+        //if(id1_value[0] != 1)break;
     }
 }
 
@@ -520,29 +520,34 @@
     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)   //改良版 位置補正(使用前に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) { //機体の位置を目標領域（目標座標＋許容誤差）に収める
 
-        //first_speed = first_speed50 * r / 50;
+        first_speed = (first_speed50 - last_speed) * r / 50 + last_speed;
 
-        /*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(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);
 
@@ -574,4 +579,75 @@
         if(id1_value[0] != 1)break;
     }
     MaxonControl(0,0,0,0);
-}
\ No newline at end of file
+}*/
+
+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; 
+    //first_speed50:５センチのズレを補正するときの補正初速度 / last_speed:目標速度 / first_speed:first_speed50とlast_speedを元に計算した実際の補正初速度 / Max_speed:補正速度の上限
+    double r;  // 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);
+
+    while(1) { //機体の位置を目標領域（目標座標＋許容誤差）に収める
+
+        first_speed = (first_speed50 - last_speed) * r / 50 + last_speed;
+
+        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);
+
+        MaxonControl(0,0,0,0);
+
+        calc_xy(tgt_angle, u, v);
+
+        r=hypot(now_x - tgt_x, now_y - tgt_y);
+
+        if(r < R) break;
+        if(id1_value[0] != 1)break;
+    }
+
+    while(1) {
+
+        //calc_gyro();
+        now_angle=gyro.getAngle();
+        //printf("angle = %f\n\r",now_angle);
+
+        out = 10 * (tgt_angle - now_angle);
+
+        if(out > 300) {  //0～179°のときは時計回りに回転
+            MaxonControl(-300,-300,-300,-300);
+        } else if(out < -300) {
+            MaxonControl(300,300,300,300);
+        } else if(out <= 300 && out > -300) {
+            MaxonControl(-out,-out,-out,-out);
+        }
+
+        if(tgt_angle - 1 < now_angle && now_angle < tgt_angle + 1) break;  //目標角度からの許容誤差内に機体の角度が収まった時、補正終了
+        if(id1_value[0] != 1)break;
+    }
+    MaxonControl(0,0,0,0);
+}
+
+
+/*----------------------コメント------------------------*/
+/* 3/10(日):pos_correctionの中身を一部修正しました。（引数に変更はありません。）
+
+/*---------------------------------------------------*/
\ No newline at end of file