Diff: TVDCTRL.cpp

Revision:

53:bedb85ee45f7

Parent:

52:6209efecde6f

Child:

54:f466964ceaa5

--- a/TVDCTRL.cpp	Mon Jan 01 06:33:52 2018 +0000
+++ b/TVDCTRL.cpp	Mon Jan 01 10:13:01 2018 +0000
@@ -638,8 +638,9 @@
 {
     //------------------------------
     //Constant variables
-    const double TGT_SLIP_RATIO = 0.2;
-    const double TGT_VEHICLE_SPEED = 20.0 / 3.6;                 //トラコンONとなる車速[m/s](これ未満は空転を抑える制御をする)
+    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]
     //------------------------------
 
     double reqMotorTrq[2] = {i_motorTrq[0] * LSB_MOTOR_TORQUE, i_motorTrq[1] * LSB_MOTOR_TORQUE};         //実数値へ変換
@@ -657,11 +658,6 @@
 
     steeringAngle = getSteerAngle()/127.0 * M_PI*STEER_RATIO;   //実舵角取得
     Vb = V * cos(steeringAngle);                                //2輪モデルにおける車体進行方向速度取得
-
-    if(Vb < TGT_VEHICLE_SPEED) {                                //対地車速が一定値より小さい場合(停車状態)
-        return;                                                 //制御なし
-    }
-
     R = mySign(steeringAngle) * (1.0 + A*Vb*Vb) * WHEEL_BASE/myMax(myAbs(steeringAngle), 0.001);  //理論旋回半径取得
 
     if(myAbs(R) < 1.0)
@@ -678,7 +674,11 @@
             Vw = 0.5*TIRE_DIAMETER*2.0*M_PI*wheelRpsRL;             //駆動輪速度[m/s]
         }
 
-        e[rlFlag][0] = Vb/(1.0 - TGT_SLIP_RATIO) - 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;          //現在の偏差取得(目標スリップ率における車輪速と駆動輪速度の差分)
+        }
 
         lastMotorTrq[rlFlag] = calcPID(lastMotorTrq[rlFlag], e[rlFlag]);    //PIDコントローラへどーーーん
         e[rlFlag][2] = e[rlFlag][1];