Diff: main.cpp

Revision:

7:93ab5505ac1b

Parent:

6:201e3de9777d

Child:

8:ddedee42c253

--- a/main.cpp	Wed Mar 18 05:11:36 2020 +0000
+++ b/main.cpp	Thu Mar 19 04:53:20 2020 +0000
@@ -30,8 +30,11 @@
 /* 自己位置推定処理 */
 void LocEstimate(void);
 
-/* P制御値代入 */
-void Pro_control(int tar_x, int tar_y, int tar_rev);
+/* 台形制御値代入 */
+void omuni_control(int tar_x, int tar_y, int tar_rev);
+
+/* オムニ関係 */
+void SubOmuni(int X,int Y,int R);
 
 
 /* 変数定義 ------------------------------------------------------------------*/
@@ -68,6 +71,9 @@
 /* P制御終了タイマ */
 Timer P_fin;
 
+/* タイマ加速 */
+Timer Acc_time;
+
 /* UART (Tx,Rx) */
 Serial telemetry(USBTX, USBRX, 115200);
 
@@ -130,6 +136,10 @@
         /* 自己位置推定更新 */
         LocEstimate();
 
+        for(int i=0; i<4; i++) {
+            MD.Set(i,MovMotor[i]);
+            MovMotor[i]=0;
+        }
         /* I2CMD実行 */
         MD.Exe();
 
@@ -167,10 +177,7 @@
             /* gyro値による補正 */
             r_val += (TarTheta-NowLoc.theta)*cor;
 
-            omuni.XmarkOmni_Move(x_val,y_val,r_val);
-            for (int i = 0; i < 4; i++) {
-                MD.Set(i,MovMotor[i]);
-            }
+            SubOmuni(x_val, y_val, r_val);
 
         }
         /* 操縦権：自動 */
@@ -184,7 +191,7 @@
 
                 /* 〇〇の処理 */
                 case 1:
-                    Pro_control(1000, 1000, 360);
+                    omuni_control(1000, 1000, 360);
                     /* 〇〇の時次のステップに */
                     if(fin_flag) {
                         fin_flag=0;
@@ -215,6 +222,8 @@
     static double EncDeg[2][2]= {0};
     static double disp[3]= {0};
 
+
+
     /* ジャイロの値取得 */
     bno.get_angles();
     GyroDeg[1]=GyroDeg[0];
@@ -231,12 +240,12 @@
     for(int i=0; i<2; i++) {
         EncDeg[i][1]=EncDeg[i][0];
         EncDeg[i][0]=EncoderDeg[i];
-        disp[i]=EncDeg[i][1]-EncDeg[i][0];
+        disp[i]=DEG_TO_DIS(EncDeg[i][1]-EncDeg[i][0]);
     }
     /* 差分を加速度として保存 */
     NowAcc.theta = disp[2];
-    NowAcc.X =  disp[0] * cos(NowLoc.theta) + disp[1] * sin(NowLoc.theta)+200*disp[2];
-    NowAcc.Y = -disp[0] * sin(NowLoc.theta) + disp[1] * cos(NowLoc.theta)+200*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;
@@ -293,6 +302,31 @@
 
 
 
+
+/*******************************************************************************
+* @概要   オムニの値計算する(加算式)
+* @引数   X,Y,Rotationそれぞれの操作量
+* @返り値 なし
+*******************************************************************************/
+void SubOmuni(int X,int Y,int R)
+{
+    /* 入力を100%に制限 */
+    X=Rest(X,100);
+    Y=Rest(Y,100);
+    R=Rest(R,100);
+
+    /* オムニ計算結果をtmpに */
+    int tmp[4]= {0};
+    omuni.XmarkOmni_Move(X,Y,R);
+
+    /* 計算結果を加算する */
+    for(int i=0; i<4; i++) MovMotor[i]+=tmp[i];
+}
+
+
+
+
+
 /*******************************************************************************
  * @概要   P制御
  * @引数   tar_x  :目標のx座標
@@ -300,13 +334,19 @@
  * @引数   tar_rev: 目標角
  * @返り値 なし
 *******************************************************************************/
-void Pro_control(int tar_x, int tar_y, int tar_theta)
+void omuni_control(int tar_x, int tar_y, int tar_theta)
 {
     static int log_distance = 0;
     static double 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;
         /* 現在座標と目標座標との距離を記録しておく */
@@ -315,20 +355,25 @@
         dis_flag=1;
     }
 
-    int x_val = 0;
-    int y_val = 0;
+    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 t_val = 0;
 
     double theta_tar = 0.0;
-
-    x_val = ABS((tar_x - NowLoc.X) * 0.06);
-    y_val = ABS((tar_y - NowLoc.Y) * 0.06);
-
+    /* 減速処理の値代入  */
+    x_dec_val = ABS((tar_x - NowLoc.X) * 0.06);
+    y_dec_val = ABS((tar_y - 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);
+    
     /* 最低を決める */
-    if (x_val < 20)
-        x_val = 20;
-    if (y_val < 20)
-        y_val = 20;
+    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);
@@ -348,8 +393,7 @@
 
     t_val = Rest(t_val, 20);
 
-    omuni.XmarkOmni_Move(x_val,y_val,t_val);
-
+    SubOmuni(x_val, y_val, t_val);
 
     /* 終了処理 */
     if ((5 > ABS(remain)) && ABS(tar_theta - NowLoc.theta) < 0.1) {
@@ -364,7 +408,6 @@
     if (P_fin.read_ms() > 100) {
         /* タイマー停止 */
         P_fin.stop();
-        P_fin.reset();
         /* フラグを建て直す */
         dis_flag=0;
         fin_flag=1;