Revision 16:c17613f5ecf3, committed 2020-03-31

Comitter:

yoshikawaryota

Date:

Tue Mar 31 09:06:43 2020 +0000

Parent:

14:4c49218bb9fb

Commit message:

fix OEI

Changed in this revision

--- a/main.cpp	Thu Mar 26 06:59:05 2020 +0000
+++ b/main.cpp	Tue Mar 31 09:06:43 2020 +0000
@@ -15,12 +15,28 @@
 
 /* ロボットの加速度 */
 ROCATION NowAcc;
+/* ロボットの加速度 */
+ROCATION NowVel;
 /* ロボットの座標 */
 ROCATION NowLoc;
-/* ロボットの目標座標 */
-ROCATION Target;
 
 /* 定数定義 ------------------------------------------------------------------*/
+
+/* スラローム自動移動の座標最大数 */
+const int SlalomPointMAX=10;
+/* [0][1][2]…X,Y,Theta(deg) [3]許容残り距離 */
+const int SlalomPoint[SlalomPointMAX][4] = {
+    { 500,  500,  0,   0},  //初期
+    {1950, 1250,  0, 600},
+    {1950, 2750,  0, 700},
+    { 510, 2750,  0, 700},
+    { 510, 4250,  0, 700},
+    {1950, 4250,  0, 700},
+    {1950, 5750,  0, 700},  
+    {1210, 5750,  0, 300},  //橋前
+    {1210, 9000,  0, 600},  //橋後
+    {5515, 9000,  0,   0}
+};
 /* マクロ定義 ----------------------------------------------------------------*/
 /* 関数プロトタイプ宣言 -------------------------------------------------------*/
 
@@ -33,8 +49,11 @@
 /* オムニ関係 */
 void SubOmuni(int X,int Y,int R);
 
+/* ジャイロ補整 */
+void CompYaw(void);
+
 /* 台形制御値代入 */
-int  omuni_control(int tar_x, int tar_y, int tar_theta, int error);
+int omuni_control(int X0,int Y0,int X1,int Y1,double tar_theta, int error);
 
 /* 移動する点を探す */
 int lncl(double *pt1, double *pt2, double *xyr, double *xy);
@@ -46,6 +65,10 @@
 
 /* 自動シーケンス */
 int auto_mode=0;
+int auto_sequence=0;
+
+/* ゾーン指定 0…青，1…赤 */
+bool zone=0;
 
 /* 直読みエンコーダ角度保存(degree) */
 double EncoderDeg[EncoderMAX] = {0};
@@ -56,9 +79,6 @@
 /* 自動yaw補整目標角度 */
 double TarTheta=0;
 
-/* 補正値用定数 */
-int cor=4;
-
 /* クラス定義 ----------------------------------------------------------------*/
 
 /* 割り込み用クラス */
@@ -67,11 +87,8 @@
 /* gyro用タイマ */
 Timer yawCnt;
 
-/* P制御終了タイマ */
-Timer P_fin;
-
-/* タイマ加速 */
-Timer Acc_time;
+/* メインループ時間計測 */
+Timer MainTime;
 
 /* UART (Tx,Rx) */
 Serial telemetry(USBTX, USBRX, 115200);
@@ -86,9 +103,9 @@
 DigitalIn UB(PC_13,PullDown);
 
 /* エンコーダーピン CS */
-DigitalOut CS[] = {PA_2,PA_3};
-DigitalOut CL[] = {PA_4,PA_5};
-DigitalIn  DO[] = {PA_6,PA_7};
+DigitalOut CS[] = {PA_4,PC_0};
+DigitalOut CL[] = {PB_0,PC_3};
+DigitalIn  DO[] = {PC_1,PC_2};
 
 /* 足回り動作クラス定義 */
 Move omuni(MovMotor,NowLoc.theta);
@@ -118,6 +135,13 @@
     MD.Init(2,MD_SMB);
     MD.Init(3,MD_SMB);
 
+    /* タイマ-スタート */
+    yawCnt.start();
+    MainTime.start();
+    /* タイマーリセット */
+    yawCnt.reset();
+    MainTime.reset();
+
     telemetry.printf("\n\rMainLoopStart");
     /* メインループ --------------------------------------------------------------*/
     while(1) {
@@ -128,9 +152,11 @@
         telemetry.printf("\n\r");
 
         /* 自動処理関連テレメトリ */
-        telemetry.printf("ope:%d ",operate);
+//        telemetry.printf("ope:%d ",operate);
         /* 座標テレメトリ */
         telemetry.printf("X:%4.0f Y:%4.0f T:%4.0f ",NowLoc.X,NowLoc.Y,NowLoc.theta);
+//        telemetry.printf("X:%4f Y:%4f T:%4f ",ABS(NowVel.X),ABS(NowVel.Y),ABS(NowVel.theta));
+
 
         /* 自己位置推定更新 */
         LocEstimate();
@@ -142,11 +168,14 @@
         /* I2CMD実行 */
         MD.Exe();
 
-        /* タイマ-スタート */
-        yawCnt.start();
-        /* タイマーリセット */
-        yawCnt.reset();
-
+        /* Startで各種初期化 */
+        if(Start) {
+            NowLoc.X=500;
+            NowLoc.Y=500;
+            NowLoc.theta=0;
+            TarTheta=0;
+            auto_sequence=0;
+        }
 
         /* 操縦権変更 ×停止 △手動 〇自動 */
         if(DS3.CROSS)     operate=0;
@@ -169,14 +198,12 @@
             int y_val = (double)(64-DS3.LY)*100/64;
             int r_val = (double)(DS3.RX-64)*100/64;
 
-            /* 目標角更新 */
+            SubOmuni(x_val, y_val, r_val);
+
+            /* 目標角更新・ジャイロ補整 */
             if(DS3.RX!=64) yawCnt.reset();
             if(yawCnt.read_ms()<1000) TarTheta=NowLoc.theta;
-
-            /* gyro値による補正 */
-            r_val += (TarTheta-NowLoc.theta)*cor;
-
-            SubOmuni(x_val, y_val, r_val);
+            CompYaw();
 
         }
         /* 操縦権：自動 */
@@ -184,68 +211,30 @@
             switch(auto_mode) {
                 /* スタート待機処理 */
                 case 0:
-                    /* オンボードSWで次のステップに */
-                    if(UB) auto_mode++;
+                    /* オンボードSWで終了処理，次のステップに */
+                    if(UB) {
+                        auto_mode++;
+                        auto_sequence=1;
+                    }
                     break;
 
-                /* 〇〇の処理(青ゾーン) */
+                /* 最初のスラローム */
                 case 1:
-                    if(omuni_control(1950, 1250, 0, 100) == 1) auto_mode++;
-                    break;
-                case 2:
-                    if(omuni_control(1950, 2750, 0, 100) == 1) auto_mode++;
-                    break;
-                case 3:
-                    if(omuni_control(510, 2750, 0, 100) == 1) auto_mode++;
-                    break;
-                case 4:
-                    if(omuni_control(510, 4250, 0, 100) == 1) auto_mode++;
-                    break;
-                case 5:
-                    if(omuni_control(1950, 4250, 0, 100) == 1) auto_mode++;
-                    break;
-                case 6:
-                    if(omuni_control(1950, 5750, 0, 100) == 1) auto_mode++;
-                    break;
-                case 7:
-                    if(omuni_control(1210, 5750, 0, 100) == 1) auto_mode++;
-                    break;
-                case 8:
-                    if(omuni_control(1210, 9000, 0, 100) == 1) auto_mode++;
-                    break;
-                case 9:
-                    if(omuni_control(5515, 9000, 0, 100) == 1) auto_mode++;
-                    break;
+                    /* 経路追従移動 */
+                    double remain=omuni_control(
+                                      SlalomPoint[auto_sequence-1][0],SlalomPoint[auto_sequence-1][1],
+                                      SlalomPoint[auto_sequence][0],SlalomPoint[auto_sequence][1],
+                                      SlalomPoint[auto_sequence][2],SlalomPoint[auto_sequence][3]);
 
-                /* 〇〇の処理(赤ゾーン) */
-                /*case 1:
-                    if(omuni_control(-1950, 1250, 0, 100) == 1) auto_mode++;
-                    break;
-                case 2:
-                    if(omuni_control(-1950, 2750, 0, 100) == 1) auto_mode++;
-                    break;
-                case 3:
-                    if(omuni_control(-510, 2750, 0, 100) == 1) auto_mode++;
-                    break;
-                case 4:
-                    if(omuni_control(-510, 4250, 0, 100) == 1) auto_mode++;
+                    /* 次のポイントを目標に */
+                    if(remain==0||(auto_sequence==SlalomPointMAX-1&&remain<100)) auto_sequence++;
+
+                    /* 目標が最大数に達したら終了処理 */
+                    if(auto_sequence==SlalomPointMAX) {
+                        auto_sequence=1;
+                        auto_mode++;
+                    }
                     break;
-                case 5:
-                    if(omuni_control(-1950, 4250, 0, 100) == 1) auto_mode++;
-                    break;
-                case 6:
-                    if(omuni_control(-1950, 5750, 0, 100) == 1) auto_mode++;
-                    break;
-                case 7:
-                    if(omuni_control(-1210, 5750, 0, 100) == 1) auto_mode++;
-                    break;
-                case 8:
-                    if(omuni_control(-1210, 9000, 0, 100) == 1) auto_mode++;
-                    break;
-                case 9:
-                    if(omuni_control(-5515, 9000, 0, 100) == 1) auto_mode++;
-                    break;
-                    */
 
                 /* 終了処理 */
                 default:
@@ -258,51 +247,6 @@
     }
 }
 
-
-/*******************************************************************************
-* @概要   自己位置推定関数
-* @引数   なし
-* @返り値 なし
-*******************************************************************************/
-void LocEstimate(void)
-{
-    static double GyroDeg[2]= {0};
-    static double EncDeg[2][2]= {0};
-    static double disp[3]= {0};
-
-
-
-    /* ジャイロの値取得 */
-    bno.get_angles();
-    GyroDeg[1]=GyroDeg[0];
-    GyroDeg[0]=bno.euler.yaw;
-    if(GyroDeg[0]!=0) {
-        /* 359→0を跨いだ時，前回の値を0から逆回転で負の値で表記 */
-        if(GyroDeg[1]<90 && GyroDeg[0]>270) GyroDeg[1]+=360;
-        /* 0→359を跨いだ時，前回の値を360以上の値で表記 */
-        else if(GyroDeg[1]>270 && GyroDeg[0]<90) GyroDeg[1]-=360;
-        /* 差を求める*/
-        disp[2]=GyroDeg[1]-GyroDeg[0];
-    }
-    /* Enc2つの差分求める */
-    for(int i=0; i<2; i++) {
-        EncDeg[i][1]=EncDeg[i][0];
-        EncDeg[i][0]=EncoderDeg[i];
-        disp[i]=DEG_TO_DIS(EncDeg[i][1]-EncDeg[i][0]);
-    }
-    /* 差分を加速度として保存 */
-    NowAcc.theta = disp[2];
-    NowAcc.X = -disp[0] * cos(DEG_TO_RAD(NowLoc.theta)) - disp[1] * sin(DEG_TO_RAD(NowLoc.theta));
-    NowAcc.Y = -disp[0] * sin(DEG_TO_RAD(NowLoc.theta)) + disp[1] * cos(DEG_TO_RAD(NowLoc.theta));
-    /* 差分を累積して現在位置を保存 */
-    NowLoc.X += NowAcc.X;
-    NowLoc.Y += NowAcc.Y;
-    NowLoc.theta += NowAcc.theta;
-}
-
-
-
-
 /* 割り込み(100us) *************************************************************/
 void IT_CallBack(void)
 {
@@ -348,7 +292,50 @@
 }
 
 
+/*******************************************************************************
+* @概要   自己位置推定関数
+* @引数   なし
+* @返り値 なし
+*******************************************************************************/
+void LocEstimate(void)
+{
+    static double GyroDeg[2]= {0};
+    static double EncDeg[2][2]= {0};
+    static double disp[3]= {0};
+    
 
+    /* ジャイロの値取得 */
+    bno.get_angles();
+    GyroDeg[1]=GyroDeg[0];
+    GyroDeg[0]=bno.euler.yaw;
+    if(GyroDeg[0]!=0) {
+        /* 359→0を跨いだ時，前回の値を0から逆回転で負の値で表記 */
+        if(GyroDeg[1]<90 && GyroDeg[0]>270) GyroDeg[1]+=360;
+        /* 0→359を跨いだ時，前回の値を360以上の値で表記 */
+        else if(GyroDeg[1]>270 && GyroDeg[0]<90) GyroDeg[1]-=360;
+        /* 差を求める*/
+        disp[2]=GyroDeg[1]-GyroDeg[0];
+    }
+    /* Enc2つの差分求める */
+    for(int i=0; i<2; i++) {
+        EncDeg[i][1]=EncDeg[i][0];
+        EncDeg[i][0]=EncoderDeg[i];
+        disp[i]=DEG_TO_DIS(EncDeg[i][1]-EncDeg[i][0]);
+    }
+    /* 差分を加速度として保存 */
+    NowAcc.theta = disp[2];
+    NowAcc.X = -disp[0] * cos(DEG_TO_RAD(NowLoc.theta)) - disp[1] * sin(DEG_TO_RAD(NowLoc.theta));
+    NowAcc.Y = -disp[0] * sin(DEG_TO_RAD(NowLoc.theta)) + disp[1] * cos(DEG_TO_RAD(NowLoc.theta));
+    /* 差分を累積して現在位置を保存 */
+    NowLoc.X += NowAcc.X;
+    NowLoc.Y += NowAcc.Y;
+    NowLoc.theta += NowAcc.theta;
+    /* 差分を計算して速度を算出 */
+    NowVel.X = NowAcc.X/MainTime.read_ms();
+    NowVel.Y = NowAcc.Y/MainTime.read_ms();
+    NowVel.theta = NowAcc.theta/MainTime.read_ms();
+    MainTime.reset();
+}
 
 
 /*******************************************************************************
@@ -363,9 +350,6 @@
     Y=Rest(Y,100);
     R=Rest(R,100);
 
-
-
-
     /* オムニ計算結果をtmpに */
     int tmp[4]= {0};
 
@@ -382,120 +366,109 @@
 }
 
 
-
+/*******************************************************************************
+* @概要   yaw角補整
+* @引数   tar 目標角度
+* @返り値 なし
+*******************************************************************************/
+void CompYaw(void)
+{
+    double r_val = (TarTheta-NowLoc.theta)*-4;
+    SubOmuni(0,0, r_val);
+}
 
 
 /*******************************************************************************
  * @概要   OEI式座標追従・P制御
- * @引数   tar_x    :目標のx座標
- * @引数   tar_y    :目標のy座標
- * @引数   tar_theta:目標角
- * @引数   error    :次ステップに進む基準とする距離
+ * @引数   X0,Y0      :直線始点のx,y座標
+ * @引数   X1,Y1      :直線終点のx,y座標
+ * @引数   tar_theta  :目標角
+ * @引数   error      :次ステップに進む基準とする距離
  * @返り値 0(継続) 1(移動完了)
 *******************************************************************************/
-int omuni_control(int tar_x, int tar_y, int tar_theta, int error)
+int omuni_control(int X0,int Y0,int X1,int Y1,double tar_theta, int error)
 {
-    static int log_distance = 0;
-    static int be_tar_x = NowLoc.X, be_tar_y = NowLoc.Y;
-    static double log_theta = 0;
+
+    /* 最初の角度・移動距離を記録しておく */
+    static int log_distance = 0, log_theta = 0;
     static bool dis_flag = 0;
 
     if(dis_flag==0) {
-        /* タイマー初期化 */
-        Acc_time.reset();
-        P_fin.reset();
-        /* タイマー始動 */
-        Acc_time.start();
-        P_fin.start();
-        /* 現在角度を記録しておく */
+        /* 現在の角度・移動距離を記録しておく */
         log_theta = NowLoc.theta;
-        /* 現在座標と目標座標との距離を記録しておく */
-        log_distance = (int)sqrt(pow((double)tar_x - NowLoc.X, 2.0) + pow((double)tar_y - NowLoc.Y, 2.0));
+        log_distance = sqrt(pow((double)X1 - NowLoc.X, 2.0) + pow((double)Y1 - NowLoc.Y, 2.0))-error;
         /* フラグ回収 */
         dis_flag=1;
     }
 
-    int x_val = 0, x_dec_val = 0;
-    int y_val = 0, y_dec_val = 0;
-    int t_val = 0;
-
-    double theta_tar = 0.0;
-    double start[2] = {0}, end[2] = {0}, circle[3] = {0}, P_tar[2] = {0};
-    
     /* 線の始点代入 */
-    start[0] = be_tar_x;
-    start[1] = be_tar_y;
+    double start[2] = {X0,Y0};
     /* 線の終点代入 */
-    end[0] = tar_x;
-    end[1] = tar_y;
+    double end[2] = {X1,Y1};
     /* 探査円の中心と半径代入 */
-    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);
-    
+    double circle[3] = {
+        NowLoc.X,NowLoc.Y,
+        sqrt(pow(NowVel.X,2)+pow(NowVel.Y,2)) * 1500.0
+    };
+    /* 残り距離計算 */
+    double remain = (int)sqrt(pow((double)X1 - NowLoc.X, 2.0) + pow((double)Y1 - NowLoc.Y, 2.0));
+
+    /* 探査円半径を残り距離以下に */
+    if(circle[2]>remain)
+        circle[2]=remain;
+//    circle[2]=M_MAX(circle[2],100);
+
+
+    /* 探査円と経路直線との交点 */
+    double P_tar[2] = {0};
+
     /* 目標点の探索、代入 */
-    lncl(start, end, circle, P_tar);
-    
+    if(lncl(start, end, circle, P_tar)==-1) {
+        P_tar[0]=X0;
+        P_tar[1]=Y0;
+    }
+
+    telemetry.printf("Cx:%4.0f Cy:%4.0f Cr:%4.0f ",circle[0],circle[1],circle[2]);
+//    telemetry.printf("Px:%4.0f Py:%4.0f ",P_tar[0],P_tar[1]);
+
+
+    /* X,Y,Rそれぞれの操作量 */
+    double x_val = 0,y_val = 0;
+
     /* 減速処理の値代入  */
-    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;
+    x_val = ABS(P_tar[0] - NowLoc.X) * 0.12;
+    y_val = ABS(P_tar[1] - NowLoc.Y) * 0.12;
 
-    /* 最低を決める */
-    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);
+    /* 最低・最高を決める */
+    x_val = M_MAX(x_val, 3);
+    x_val = M_MIN(x_val, 100);
+    y_val = M_MAX(y_val, 3);
+    y_val = M_MIN(y_val, 100);
 
     /* 正負を決める */
     if ((P_tar[0] - NowLoc.X) < 0)
         x_val *= -1;
     if ((P_tar[1] - NowLoc.Y) < 0)
         y_val *= -1;
-
-    /* 残り距離計算 */
-    int remain = (int)sqrt(pow((double)tar_x - NowLoc.X, 2.0) + pow((double)tar_y - NowLoc.Y, 2.0));
-    /* 移動量に応じたthetaを求める */
-    theta_tar = (((double)log_distance - remain) / log_distance) * (tar_theta - log_theta) + log_theta;
-    /* 合わせるthetaと現在thetaの差分からtheta補整をかける */
-    t_val = (theta_tar - NowLoc.theta) * 1.5;
-
-    t_val = Rest(t_val, 20);
-
-    SubOmuni(x_val, y_val, t_val);
+    telemetry.printf("X:%4.0f Y:%4.0f ",x_val,y_val);
 
-    /* 終了処理 */
-    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;
-    }
-    if (x_val == 0 && y_val == 0)
-        P_fin.start();
-    else
-        P_fin.reset();
-
-    if (P_fin.read_ms() > 100) {
-        /* タイマー停止 */
-        P_fin.stop();
-        /* フラグを建て直す */
-        dis_flag=0;
-
-        return 1;
-    }
-    return 0;
-}
+    /* オムニ計算 */
+    SubOmuni(x_val, y_val,0);
 
 
+    /* 移動量に応じたその瞬間の合わせるthetaを求める */
+    TarTheta = (((double)log_distance - (remain - error)) / log_distance) * (tar_theta - log_theta) + log_theta;
+    /* thetaに合わせる */
+    CompYaw();
 
+    /* 残り距離がほぼないなら終了 */
+    if(ABS(error - remain)<30) {
+        /* Theta基準値を最終に */
+        TarTheta=tar_theta;
+        return 0;
+    }
+    return remain;
+}
 
 
 /*******************************************************************************
@@ -513,30 +486,21 @@
     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);
     /* 直線の法線ベクトルと直線上の点の位置ベクトルとの内積 */
@@ -553,16 +517,11 @@
     /* 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点で接する時異常終了 */
@@ -594,8 +553,7 @@
         if (sqdst1 < sqdst2) {
             xy[0] = x1;
             xy[1] = y1;
-        }
-        else {
+        } else {
             xy[0] = x2;
             xy[1] = y2;
         }