Diff: TVDCTRL.cpp

Revision:

56:78bd99bf995a

Parent:

55:e9ca699bec57

Child:

57:02b7178cd083

--- a/TVDCTRL.cpp	Mon Jan 08 10:06:12 2018 +0000
+++ b/TVDCTRL.cpp	Mon Jan 08 10:21:11 2018 +0000
@@ -613,24 +613,6 @@
     return limitRate;
 }
 
-//------------------------------
-//参考文献：デジタルPI制御と離散化
-//------------------------------
-//速度アルゴリズム型PID制御器
-//lastOutput    : 前回の制御出力
-//*e            : 過去3つ分の偏差
-double calcPID(double lastOutput, double* e)
-{
-    //------------------------------
-    //Constant variables
-    const double KP = 10.0;
-    const double KI = 0.0;
-    const double KD = 0.0;
-    //------------------------------
-
-    return lastOutput + KP*(e[0] - e[1]) + KI*e[0] + KD*(e[0] - 2.0*e[1] + e[2]);    //PID controller
-}
-
 //--------------------------------------------------
 //参考文献：モデルを用いたトラクションコントロールの基礎研究
 //--------------------------------------------------
@@ -641,13 +623,17 @@
     const double TGT_SLIP_RATIO = 0.3;
     const double TGT_VEHICLE_SPEED = 20.0 / 3.6;                    //この車速相当の回転数まで空転を制限する
     const double CTRL_START_VEHICLE_SPEED = 0.0 / 3.6;               //トラコンONとなる車速[m/s]
+    const double KP = 10.0;
+    const double KI = 0.0;
+    const double KD = 0.0;
     //------------------------------
 
     double reqMotorTrq[2] = {i_motorTrq[0] * LSB_MOTOR_TORQUE, i_motorTrq[1] * LSB_MOTOR_TORQUE};         //実数値へ変換
     double outputTrq[2] = {0.0};
     double steeringAngle, R, Vb, Vbw, Vw, wheelRpsRR, wheelRpsRL;
     static double lastMotorTrq[2] = {0.0};                          //前回の出力トルク
-    static double e[2][3] = {0.0};                                  //3つ前の偏差まで保持
+    static double e[2][2] = {0.0};                                  //1つ前の偏差まで保持
+    static double integral = 0.0;
     double tmpTcsMotorTrq;                                          //
 
     wheelRpsRR = getWheelRps(RR_MOTOR);
@@ -679,9 +665,9 @@
         } else {
             e[rlFlag][0] = Vbw/(1.0 - TGT_SLIP_RATIO) - Vw;          //現在の偏差取得(目標スリップ率における車輪速と駆動輪速度の差分)
         }
-
-        lastMotorTrq[rlFlag] = calcPID(lastMotorTrq[rlFlag], e[rlFlag]);    //PIDコントローラへどーーーん
-        e[rlFlag][2] = e[rlFlag][1];
+        
+        integral = integral + (e[rlFlag][0] + e[rlFlag][1]);
+        lastMotorTrq[rlFlag] = KP * e[rlFlag][0] + KI*integral + KD*(e[rlFlag][1] - e[rlFlag][0]); //PIDコントローラへどーーーん
         e[rlFlag][1] = e[rlFlag][0];
     }
 
@@ -697,20 +683,20 @@
             outputTrq[rlFlag] = -10.0;
     }
 
-    if(myAbs(e[0][0]) > myAbs(e[1][1])) {                       //右輪の空転が大きい場合
-        tmpTcsMotorTrq = outputTrq[1] * (ALPHA-1.0)/(ALPHA+1.0);
-        if(tmpTcsMotorTrq > outputTrq[0])                       //空転輪側車軸トルクを下げ過ぎていた場合
-            if(reqMotorTrq[0] > tmpTcsMotorTrq)
-                outputTrq[0] = tmpTcsMotorTrq;                  //右車軸トルクを0へ制限
-    } else if(myAbs(e[0][0]) < myAbs(e[1][1])) {                //左輪の空転が大きい場合
-        tmpTcsMotorTrq = outputTrq[0] * (ALPHA-1.0)/(ALPHA+1.0);
-        if(tmpTcsMotorTrq > outputTrq[1])                       //空転輪側車軸トルクを下げ過ぎていた場合
-            if(reqMotorTrq[1] > tmpTcsMotorTrq)
-                outputTrq[1] = tmpTcsMotorTrq;                  //左車軸トルクを0へ制限
-    }
+//    if(myAbs(e[0][0]) > myAbs(e[1][1])) {                       //右輪の空転が大きい場合
+//        tmpTcsMotorTrq = outputTrq[1] * (ALPHA-1.0)/(ALPHA+1.0);
+//        if(tmpTcsMotorTrq > outputTrq[0])                       //空転輪側車軸トルクを下げ過ぎていた場合
+//            if(reqMotorTrq[0] > tmpTcsMotorTrq)
+//                outputTrq[0] = tmpTcsMotorTrq;                  //右車軸トルクを0へ制限
+//    } else if(myAbs(e[0][0]) < myAbs(e[1][1])) {                //左輪の空転が大きい場合
+//        tmpTcsMotorTrq = outputTrq[0] * (ALPHA-1.0)/(ALPHA+1.0);
+//        if(tmpTcsMotorTrq > outputTrq[1])                       //空転輪側車軸トルクを下げ過ぎていた場合
+//            if(reqMotorTrq[1] > tmpTcsMotorTrq)
+//                outputTrq[1] = tmpTcsMotorTrq;                  //左車軸トルクを0へ制限
+//    }
 
     for (int rlFlag = 0; rlFlag < 2; rlFlag++) {
-        if(e[rlFlag][0] < 0.0)                                  //駆動時のみ制御ON
+//        if(e[rlFlag][0] < 0.0)                                  //駆動時のみ制御ON
             i_motorTrq[rlFlag] = (int)(outputTrq[rlFlag] / LSB_MOTOR_TORQUE + 0.5);
     }
 //    printf("%f %f\r\n", outputTrq[0], outputTrq[1]);