Diff: TVDCTRL.cpp

Revision:

54:f466964ceaa5

Parent:

53:bedb85ee45f7

Child:

55:e9ca699bec57

--- a/TVDCTRL.cpp	Mon Jan 01 10:13:01 2018 +0000
+++ b/TVDCTRL.cpp	Mon Jan 08 05:29:14 2018 +0000
@@ -640,16 +640,15 @@
     //Constant variables
     const double TGT_SLIP_RATIO = 0.3;
     const double TGT_VEHICLE_SPEED = 20.0 / 3.6;                    //この車速相当の回転数まで空転を制限する
-    const double CTRL_START_VEHICLE_SPEED = 10 / 3.6;               //トラコンONとなる車速[m/s]
+    const double CTRL_START_VEHICLE_SPEED = 5 / 3.6;               //トラコンONとなる車速[m/s]
     //------------------------------
 
     double reqMotorTrq[2] = {i_motorTrq[0] * LSB_MOTOR_TORQUE, i_motorTrq[1] * LSB_MOTOR_TORQUE};         //実数値へ変換
     double outputTrq[2] = {0.0};
-    double steeringAngle, R, V, Vb, Vw, wheelRpsRR, wheelRpsRL;
+    double steeringAngle, R, V, Vb, Vbw, Vw, wheelRpsRR, wheelRpsRL;
     static double lastMotorTrq[2] = {0.0};                          //前回の出力トルク
-    double motorTrq_wTCS[2] = {0.0};                                //TCSトルクベクタリングを含めたトルク
     static double e[2][3] = {0.0};                                  //3つ前の偏差まで保持
-//    double vectoringAmount = 0.0;
+    double tmpTcsMotorTrq;                                             //
 
     V = getVelocity();                                              //前輪回転方向における車速換算値
 
@@ -658,6 +657,9 @@
 
     steeringAngle = getSteerAngle()/127.0 * M_PI*STEER_RATIO;   //実舵角取得
     Vb = V * cos(steeringAngle);                                //2輪モデルにおける車体進行方向速度取得
+
+    Vb = 15.0/3.6;
+
     R = mySign(steeringAngle) * (1.0 + A*Vb*Vb) * WHEEL_BASE/myMax(myAbs(steeringAngle), 0.001);  //理論旋回半径取得
 
     if(myAbs(R) < 1.0)
@@ -667,17 +669,17 @@
 
     for (int rlFlag = 0; rlFlag < 2; rlFlag++) {
         if(rlFlag == 0) {
-            Vb = Vb*(1.0 + TREAD/(2.0*R));                          //トレッドを考慮した従動輪速度[m/s]
+            Vbw = Vb*(1.0 + TREAD/(2.0*R));                          //トレッドを考慮した従動輪速度[m/s]
             Vw = 0.5*TIRE_DIAMETER*2.0*M_PI*wheelRpsRR;             //駆動輪速度[m/s]
         } else {
-            Vb = Vb*(1.0 - TREAD/(2.0*R));                          //トレッドを考慮した従動輪速度[m/s
+            Vbw = Vb*(1.0 - TREAD/(2.0*R));                          //トレッドを考慮した従動輪速度[m/s
             Vw = 0.5*TIRE_DIAMETER*2.0*M_PI*wheelRpsRL;             //駆動輪速度[m/s]
         }
 
-        if(Vb < CTRL_START_VEHICLE_SPEED) {                                //対地車速が一定値より小さい場合(停車状態)
-            e[rlFlag][0] = TGT_VEHICLE_SPEED - Vw;                         //空転抑制制御
+        if(Vb < CTRL_START_VEHICLE_SPEED) {                         //対地車速が一定値より小さい場合(停車状態)
+            e[rlFlag][0] = TGT_VEHICLE_SPEED - Vw;                  //空転抑制制御
         } else {
-            e[rlFlag][0] = Vb/(1.0 - TGT_SLIP_RATIO) - Vw;          //現在の偏差取得(目標スリップ率における車輪速と駆動輪速度の差分)
+            e[rlFlag][0] = Vbw/(1.0 - TGT_SLIP_RATIO) - Vw;          //現在の偏差取得(目標スリップ率における車輪速と駆動輪速度の差分)
         }
 
         lastMotorTrq[rlFlag] = calcPID(lastMotorTrq[rlFlag], e[rlFlag]);    //PIDコントローラへどーーーん
@@ -685,37 +687,31 @@
         e[rlFlag][1] = e[rlFlag][0];
     }
 
-//    if((lastMotorTrq[0] < reqMotorTrq[0]) && (lastMotorTrq[1] >= reqMotorTrq[1])) {          //TCS R:L => ON:OFF
-//        vectoringAmount = reqMotorTrq[0] - lastMotorTrq[0];
-//
-//        if(myAbs(vectoringAmount) > 10.0)
-//            vectoringAmount = mySign(vectoringAmount) * 10.0;
-//
-//        motorTrq_wTCS[1] = reqMotorTrq[1] + vectoringAmount;                                //TCSで制限された分を左へベクタリング
-//        motorTrq_wTCS[0] = reqMotorTrq[0];
-//
-//    } else if((lastMotorTrq[1] < reqMotorTrq[1]) && (lastMotorTrq[0] >= reqMotorTrq[0])) {   //TCS R:L => OFF:ON
-//        vectoringAmount = reqMotorTrq[0] - lastMotorTrq[0];
-//
-//        if(myAbs(vectoringAmount) > 10.0)
-//            vectoringAmount = mySign(vectoringAmount) * 10.0;
-//
-//        motorTrq_wTCS[0] = reqMotorTrq[0] + vectoringAmount;                                //TCSで制限された分を右へベクタリング
-//        motorTrq_wTCS[1] = reqMotorTrq[1];
-//
-//    } else {
-//        motorTrq_wTCS[0] = reqMotorTrq[0];
-//        motorTrq_wTCS[1] = reqMotorTrq[1];
-//    }
+    printf("%f %f\r\n", e[0][0], e[1][0]);
 
-    motorTrq_wTCS[0] = reqMotorTrq[0];
-    motorTrq_wTCS[1] = reqMotorTrq[1];
-
-//    printf("%f %f\r\n", e[0][0], e[1][0]);
+    outputTrq[0] = myMin(reqMotorTrq[0], lastMotorTrq[0]);      //ちっさい方を出力トルクとして採用
+    outputTrq[1] = myMin(reqMotorTrq[1], lastMotorTrq[1]);      //ちっさい方を出力トルクとして採用
 
     for (int rlFlag = 0; rlFlag < 2; rlFlag++) {
+        if(outputTrq[rlFlag] > 45.0)
+            outputTrq[rlFlag] = 45.0;
+        if(outputTrq[rlFlag] < -10.0)
+            outputTrq[rlFlag] = -10.0;
+    }
 
-        outputTrq[rlFlag] = myMin(reqMotorTrq[rlFlag], myMin(lastMotorTrq[rlFlag], motorTrq_wTCS[rlFlag]));    //ちっさい方を採用
+    if(e[0][0] > e[1][1]) {                                     //右輪の空転が大きい場合
+        tmpTcsMotorTrq = outputTrq[1] * (ALPHA-1.0)/(ALPHA+1.0);
+        if(reqMotorTrq[0] > tmpTcsMotorTrq)
+            if(tmpTcsMotorTrq > outputTrq[0])                   //空転輪側車軸トルクを下げ過ぎていた場合
+                outputTrq[0] = tmpTcsMotorTrq;                  //右車軸トルクを0へ制限
+    } else if(e[0][0] < e[1][1]) {                              //左輪の空転が大きい場合
+        tmpTcsMotorTrq = outputTrq[0] * (ALPHA-1.0)/(ALPHA+1.0);
+        if(reqMotorTrq[1] > tmpTcsMotorTrq)
+            if(tmpTcsMotorTrq > outputTrq[1])                   //空転輪側車軸トルクを下げ過ぎていた場合
+                outputTrq[1] = tmpTcsMotorTrq;                  //左車軸トルクを0へ制限
+    }
+
+    for (int rlFlag = 0; rlFlag < 2; rlFlag++) {
 
         if(outputTrq[rlFlag] > 45.0)
             outputTrq[rlFlag] = 45.0;