Diff: movement/movement.cpp

Revision:

9:63924280272d

Parent:

8:2ba338b4590e

--- a/movement/movement.cpp	Thu Mar 07 02:54:55 2019 +0000
+++ b/movement/movement.cpp	Thu Mar 07 05:47:18 2019 +0000
@@ -264,6 +264,18 @@
     }*/
 }
 
+void enc_correction(int x_select,int y_select){ //エンコーダの座標を超音波センサの座標で上書き
+//x_select,y_select → (0:上書きしない/1:上書きする)
+
+    if(x_select == 1){
+        info.nowX.enc = info.nowX.usw;
+    }
+    if(y_select == 1){
+        info.nowY.enc = info.nowY.usw;
+    }
+    
+}
+
 //ここからそれぞれのプログラム/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 //now_x(現在のx座標),now_y(現在のy座標),now_angle(機体角度(ラジアンではない)(0~360や-180~180とは限らない))(反時計回りが正)
 //ジャイロの出力は角度だが三角関数はラジアンとして計算する
@@ -448,7 +460,7 @@
     }
 }
 
-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 r, R=10;  // r:一回補正が入るごとの機体の位置と目標位置の距離(ズレ) R:補正終了とみなす目標位置からの機体の位置のズレ
@@ -486,4 +498,56 @@
         if(id1_value[0] != 1)break;
     }
     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 = 200, last_speed = 50, Max_speed = 1000;
+    double r, R=10;  // r:一回補正が入るごとの機体の位置と目標位置の距離(ズレ) R:補正終了とみなす目標位置からの機体の位置のズレ
+    double out;
+
+    calc_xy(tgt_angle, u, v);
+    
+    r = hypot(now_x - tgt_x, now_y - tgt_y);
+
+    while(1) { //機体の位置を目標領域（目標座標＋許容誤差）に収める
+    
+        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);
+        }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);
+        }
+        
+        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();
+
+        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 - 0.5 < now_angle && now_angle < tgt_angle + 0.5) break;  //目標角度からの許容誤差内に機体の角度が収まった時、補正終了
+        if(id1_value[0] != 1)break;
+    }
+    MaxonControl(0,0,0,0);
 }
\ No newline at end of file