Diff: main.cpp

Revision:

9:8c550d496736

Parent:

8:ddedee42c253

Child:

10:e19d1d12f0ed

--- a/main.cpp	Thu Mar 19 06:53:48 2020 +0000
+++ b/main.cpp	Mon Mar 23 04:45:54 2020 +0000
@@ -34,7 +34,7 @@
 void SubOmuni(int X,int Y,int R);
 
 /* 台形制御値代入 */
-int  omuni_control(int tar_x, int tar_y, int tar_rev);
+int  omuni_control(int tar_x, int tar_y, int tar_theta, int error);
 
 /* 変数定義 ------------------------------------------------------------------*/
 
@@ -188,7 +188,7 @@
                 /* 〇〇の処理 */
                 case 1:
                     /* 〇〇の時次のステップに */
-                    if(omuni_control(1000, 1000, 360) == 1) auto_mode++;
+                    if(omuni_control(1000, 1000, 360, 100) == 1) auto_mode++;
                     break;
 
                 /* 終了処理 */
@@ -307,12 +307,22 @@
     Y=Rest(Y,100);
     R=Rest(R,100);
 
+
+
+
     /* オムニ計算結果をtmpに */
     int tmp[4]= {0};
+
+    /* 一度データをtmpに保存 */
+    for(int i=0; i<4; i++)
+        tmp[i]=MovMotor[i];
+
+    /* オムニ計算 */
     omuni.XmarkOmni_Move(X,Y,R);
 
     /* 計算結果を加算する */
-    for(int i=0; i<4; i++) MovMotor[i]+=tmp[i];
+    for(int i=0; i<4; i++)
+        MovMotor[i]+=tmp[i];
 }
 
 
@@ -320,15 +330,17 @@
 
 
 /*******************************************************************************
- * @概要   P制御
+ * @概要   台形制御
  * @引数   tar_x  :目標のx座標
  * @引数   tar_y  :目標のy座標
  * @引数   tar_theta: 目標角
- * @返り値 なし
+ * @返り値 0(継続) 1(移動完了)
 *******************************************************************************/
-int omuni_control(int tar_x, int tar_y, int tar_theta)
+int omuni_control(int tar_x, int tar_y, int tar_theta, int error)
 {
     static int log_distance = 0;
+    static int be_tar_x = 0, be_tar_y = 0;
+    static int a = 0, b = 0;
     static double log_theta = 0;
     static bool dis_flag = 0;
 
@@ -343,37 +355,67 @@
         log_theta = NowLoc.theta;
         /* 現在座標と目標座標との距離を記録しておく */
         log_distance = (int)sqrt(pow((double)tar_x - NowLoc.X, 2.0) + pow((double)tar_y - NowLoc.Y, 2.0));
+        /* 目標地点間を結ぶ直線の傾きと切片を求める */
+        a = (tar_y - be_tar_y) / (tar_x - be_tar_x);
+        b = tar_y - a * tar_x;
         /* フラグ回収 */
         dis_flag=1;
     }
 
-    int x_val = 0, x_acc_val = 0, x_dec_val = 0;
-    int y_val = 0, y_acc_val = 0, y_dec_val = 0;
+    int x_val = 0, x_acc_val = 0, x_dec_val = 0, Px = 0;
+    int y_val = 0, y_acc_val = 0, y_dec_val = 0, Py = 0;
     int t_val = 0;
+    int P_dis = 0;
+    int Max = 0, Min = 0, x = 0, y = 0;
 
     double theta_tar = 0.0;
+
+    /* 全探索の最大値と最小値の決定 */
+    if(NowLoc.X <= tar_x) {
+        Max = tar_x;
+        Min = NowLoc.X;
+    } else {
+        Max = NowLoc.X;
+        Min = tar_x;
+    }
+    /* 目標にする点Pの決定 */
+    for(x=Min; x<=Max; x++) {
+        int r = sqrt(pow(NowAcc.X,2)+pow(NowAcc.Y,2)) * 1.5;
+
+        y = a * x + b;
+        P_dis = sqrt(pow(x-NowLoc.X,2)+pow(y-NowLoc.Y,2));
+        if(P_dis == r && ABS(tar_x - x) < ABS(tar_x - NowLoc.X) || P_dis == r && ABS(tar_y - y) < ABS(tar_y - NowLoc.Y)) {
+            Px = x;
+            Py = y;
+            break;
+        }
+        if(x == Max) {
+            Px = tar_x;
+            Py = tar_y;
+        }
+    }
     /* 減速処理の値代入  */
-    x_dec_val = ABS((tar_x - NowLoc.X) * 0.06);
-    y_dec_val = ABS((tar_y - NowLoc.Y) * 0.06);
+    x_dec_val = ABS((Px - NowLoc.X) * 0.06);
+    y_dec_val = ABS((Py - NowLoc.Y) * 0.06);
     /* 加速処理の値代入 */
     x_acc_val=ABS((int)Acc_time.read_ms() * 0.15);
     y_acc_val=ABS((int)Acc_time.read_ms() * 0.15);
     /* 低い方の値代入 */
     x_val = M_MIN(x_acc_val, x_dec_val);
     y_val = M_MIN(y_acc_val, y_dec_val);
-    
+
     /* 最低を決める */
     x_val = M_MAX(x_val, 20);
     y_val = M_MAX(y_val, 20);
-    
+
     /* 100以内に収める */
     x_val = Rest(x_val, 100);
     y_val = Rest(y_val, 100);
 
     /* 正負を決める */
-    if ((tar_x - NowLoc.X) < 0)
+    if ((Px - NowLoc.X) < 0)
         x_val *= -1;
-    if ((tar_y - NowLoc.Y) < 0)
+    if ((Py - NowLoc.Y) < 0)
         y_val *= -1;
 
     /* 残り距離計算 */
@@ -388,7 +430,11 @@
     SubOmuni(x_val, y_val, t_val);
 
     /* 終了処理 */
-    if ((5 > ABS(remain)) && ABS(tar_theta - NowLoc.theta) < 0.1) {
+    if(error != 0 && error >= remain) {
+        be_tar_x = tar_x;
+        be_tar_y = tar_y;
+        return 1;
+    } else if ((5 > ABS(remain)) && ABS(tar_theta - NowLoc.theta) < 0.1) {
         x_val = 0;
         y_val = 0;
     }
@@ -402,7 +448,7 @@
         P_fin.stop();
         /* フラグを建て直す */
         dis_flag=0;
-        
+
         return 1;
     }
     return 0;