Diff: main.cpp

Revision:

13:e4de7896f808

Parent:

12:1daa4d490a12

Child:

14:4c49218bb9fb

--- a/main.cpp	Tue Mar 24 07:22:31 2020 +0000
+++ b/main.cpp	Thu Mar 26 06:30:34 2020 +0000
@@ -36,6 +36,9 @@
 /* 台形制御値代入 */
 int  omuni_control(int tar_x, int tar_y, int tar_theta, int error);
 
+/* 移動する点を探す */
+int lncl(double *pt1, double *pt2, double *xyr, double *xy);
+
 /* 変数定義 ------------------------------------------------------------------*/
 
 /* 操作権 0…なし 1…手動 2…自動 */
@@ -393,8 +396,7 @@
 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 int be_tar_x = NowLoc.X, be_tar_y = NowLoc.Y;
     static double log_theta = 0;
     static bool dis_flag = 0;
 
@@ -409,80 +411,36 @@
         log_theta = NowLoc.theta;
         /* 現在座標と目標座標との距離を記録しておく */
         log_distance = (int)sqrt(pow((double)tar_x - NowLoc.X, 2.0) + pow((double)tar_y - NowLoc.Y, 2.0));
-        /* 目標地点間を結ぶ直線の傾きと切片を求める */
-        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;
     }
 
-    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 x_val = 0, x_dec_val = 0;
+    int y_val = 0, y_dec_val = 0;
     int t_val = 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[0] = tar_x;
-        Min[0] = NowLoc.X;
-    } else {
-        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の決定 */
-    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;
-            }
-        }
-    }
-    /* 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;
-            if(P_dis == r && ABS(tar_y - y) < ABS(tar_y - NowLoc.Y)) {
-                Py = y;
-                break;
-            }
-            if(y == Max[1]) Py = tar_y;
-        }
-    }
+    double start[2] = {0}, end[2] = {0}, circle[3] = {0}, P_tar[2] = {0};
+    
+    /* 線の始点代入 */
+    start[0] = be_tar_x;
+    start[1] = be_tar_y;
+    /* 線の終点代入 */
+    end[0] = tar_x;
+    end[1] = tar_y;
+    /* 探査円の中心と半径代入 */
+    circle[0] = NowLoc.X;
+    circle[1] = NowLoc.Y;
+    circle[2] = sqrt(pow(NowAcc.X,2)+pow(NowAcc.Y,2)) * 1.5;
+    /* 半径の制限 */
+    circle[2] = Rest(circle[2], 100);
+    
+    /* 目標点の探索、代入 */
+    lncl(start, end, circle, P_tar);
+    
     /* 減速処理の値代入  */
-    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_dec_val = ABS((P_tar[0] - NowLoc.X) * 0.06);
+    y_dec_val = ABS((P_tar[1] - NowLoc.Y) * 0.06);
     x_val = x_dec_val;
     y_val = y_dec_val;
 
@@ -495,9 +453,9 @@
     y_val = Rest(y_val, 100);
 
     /* 正負を決める */
-    if ((Px - NowLoc.X) < 0)
+    if ((P_tar[0] - NowLoc.X) < 0)
         x_val *= -1;
-    if ((Py - NowLoc.Y) < 0)
+    if ((P_tar[1] - NowLoc.Y) < 0)
         y_val *= -1;
 
     /* 残り距離計算 */
@@ -534,4 +492,113 @@
         return 1;
     }
     return 0;
+}
+
+
+
+
+
+/*******************************************************************************
+ * @概要   円と線の交点を求める
+ *         なんかもうわかんない．あんまコメントアウトは参考にしないように
+ * @引数   *pt1     :直線始まりの座標配列の先頭アドレス
+ * @引数   *pt2     :直線終わりの座標配列の先頭アドレス
+ * @引数   *xyr     :円のXY半径の座標配列の先頭アドレス
+ * @引数   xy       :直線と円の交点の座標配列の先頭アドレス
+ * @返り値 交点の個数(-1…エラー)
+*******************************************************************************/
+int lncl(double *pt1, double *pt2, double *xyr, double *xy)
+{
+    double  root, factor, xo, yo, f, g, fsq, gsq, fgsq, xjo, yjo, a, b, c;
+    double  fygx, fxgy, t, fginv, t1, t2, x1, y1, x2, y2, sqdst1, sqdst2;
+    double  accy = 1.0E-15;
+
+ 
+
+
+    /* X,Yそれぞれの変化量 */
+    double xlk = pt2[0] - pt1[0];
+    double ylk = pt2[1] - pt1[1];
+    /* 偏差 */
+    double rsq = sqrt(pow(xlk, 2.0) + pow(ylk, 2.0));
+
+ 
+
+    /* 0の除算時異常終了 */
+    if (rsq < accy) return (-1);
+    double rinv = 1.0 / rsq;
+
+ 
+
+    /* ax+by+cの式の係数をそれぞれ */
+    a = -ylk * rinv;
+    b = xlk * rinv;
+    c = (pt1[0] * pt2[1] - pt2[0] * pt1[1]) * rinv;
+
+ 
+
+    /* rootは1になる */
+    root = 1.0 / (a * a + b * b);
+    /* 直線の法線ベクトルと直線上の点の位置ベクトルとの内積 */
+    factor = -c * root;
+    xo = a * factor;
+    yo = b * factor;
+    root = sqrt(root);
+    /* xについての関数fとyについてg */
+    f = b * root;
+    g = -a * root;
+    /* 2乗バージョン */
+    fsq = f * f;
+    gsq = g * g;
+    /* 2つの合成 */
+    fgsq = fsq + gsq;
+
+ 
+
+
+    xjo = xyr[0] - xo;
+    yjo = xyr[1] - yo;
+    fygx = f * yjo - g * xjo;
+    root = xyr[2] * xyr[2] * fgsq - fygx * fygx;
+
+ 
+
+    /* 2つの関数が近い 3点で接する時異常終了 */
+    if (fgsq < accy) return (3);
+    /* 円と線が接さない 0点で接する時異常終了 */
+    else if (root < -accy)  return (-1);
+    /* 円と線が正接 1点で接する */
+    else if (root < accy) {
+        fxgy = f * xjo + g * yjo;
+        t = fxgy / fgsq;
+        xy[0] = xo + f * t;
+        xy[1] = yo + g * t;
+        return (1);
+    }
+    /* 円と線が交差する 2点で接する */
+    else {
+        fxgy = f * xjo + g * yjo;
+        root = sqrt(root);
+        fginv = 1.0 / fgsq;
+        /* 根号の+-の2通り求める */
+        t1 = (fxgy - root)*fginv;
+        t2 = (fxgy + root)*fginv;
+        x1 = xo + f * t1;
+        y1 = yo + g * t1;
+        x2 = xo + f * t2;
+        y2 = yo + g * t2;
+        /*終点と距離をとる √は省略 */
+        sqdst1 = pow((pt2[0] - x1), 2.0) + pow((pt2[1] - y1), 2.0);
+        sqdst2 = pow((pt2[0] - x2), 2.0) + pow((pt2[1] - y2), 2.0);
+        /* 終点までの距離が近い方を採用 */
+        if (sqdst1 < sqdst2) {
+            xy[0] = x1;
+            xy[1] = y1;
+        }
+        else {
+            xy[0] = x2;
+            xy[1] = y2;
+        }
+        return (2);
+    }
 }
\ No newline at end of file