Diff: main.cpp

Revision:

10:e19d1d12f0ed

Parent:

9:8c550d496736

Child:

11:80b1daeb243f

--- a/main.cpp	Mon Mar 23 04:45:54 2020 +0000
+++ b/main.cpp	Mon Mar 23 07:31:25 2020 +0000
@@ -331,9 +331,10 @@
 
 /*******************************************************************************
  * @概要   台形制御
- * @引数   tar_x  :目標のx座標
- * @引数   tar_y  :目標のy座標
- * @引数   tar_theta: 目標角
+ * @引数   tar_x    :目標のx座標
+ * @引数   tar_y    :目標のy座標
+ * @引数   tar_theta:目標角
+ * @引数   error    :次ステップに進む基準とする距離 
  * @返り値 0(継続) 1(移動完了)
 *******************************************************************************/
 int omuni_control(int tar_x, int tar_y, int tar_theta, int error)
@@ -356,7 +357,7 @@
         /* 現在座標と目標座標との距離を記録しておく */
         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);
+        if(tar_x != be_tar_x) a = (tar_y - be_tar_y) / (tar_x - be_tar_x);
         b = tar_y - a * tar_x;
         /* フラグ回収 */
         dis_flag=1;
@@ -365,33 +366,59 @@
     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;
+    int P_dis = 0, r = 0;
+    int Max[2] = {0}, Min[2] = {0}, x = 0, y = 0;
 
     double theta_tar = 0.0;
 
-    /* 全探索の最大値と最小値の決定 */
+    /* x,yの値域を代入(0=x,1=y) */
     if(NowLoc.X <= tar_x) {
-        Max = tar_x;
-        Min = NowLoc.X;
+        Max[0] = tar_x;
+        Min[0] = NowLoc.X;
     } else {
-        Max = NowLoc.X;
-        Min = tar_x;
+        Max[0] = NowLoc.X;
+        Min[0] = tar_x;
+    }
+    if(NowLoc.Y <= tar_y) {
+        Max[1] = tar_y;
+        Min[1] = NowLoc.Y;
+    } else {
+        Max[1] = NowLoc.Y;
+        Min[1] = tar_y;
     }
     /* 目標にする点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(Max[0] != Min[0]) {
+        r = sqrt(pow(NowAcc.X,2)+pow(NowAcc.Y,2)) * 1.5;
+        /* 半径の最大を設定 */
+        r = Rest(r, 100);
+        for(x=Min[0]; x<=Max[0]; x++) {
+            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[0]) {
+                Px = tar_x;
+                Py = tar_y;
+            }
         }
-        if(x == Max) {
+    }
+    /* x=定数の式になった時 */ 
+    else {
+        r = sqrt(pow(NowAcc.X,2)+pow(NowAcc.Y,2)) * 1.5;
+        /* 半径の最大を設定 */
+        r = Rest(r, 100);
+        for(y=Min[1]; y<=Max[1]; y++) {
+            P_dis = y;
+            
             Px = tar_x;
-            Py = tar_y;
+            if(P_dis == r && ABS(tar_y - y) < ABS(tar_y - NowLoc.Y)) {
+                Py = y;
+                break;
+            }
+            if(y == Max[1]) Py = tar_y;
         }
     }
     /* 減速処理の値代入  */