Diff: TVDCTRL.cpp

Revision:

52:6209efecde6f

Parent:

51:640198055ed6

Child:

53:bedb85ee45f7

--- a/TVDCTRL.cpp	Wed Dec 20 16:34:37 2017 +0000
+++ b/TVDCTRL.cpp	Mon Jan 01 06:33:52 2018 +0000
@@ -638,7 +638,7 @@
 {
     //------------------------------
     //Constant variables
-    const double TGT_SLIP_RATIO = 0.1;
+    const double TGT_SLIP_RATIO = 0.2;
     const double TGT_VEHICLE_SPEED = 20.0 / 3.6;                 //トラコンONとなる車速[m/s](これ未満は空転を抑える制御をする)
     //------------------------------
 
@@ -648,19 +648,24 @@
     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 vectoringAmount = 0.0;
 
     V = getVelocity();                                              //前輪回転方向における車速換算値
 
     wheelRpsRR = getWheelRps(RR_MOTOR);
     wheelRpsRL = getWheelRps(RL_MOTOR);
 
-    steeringAngle   = getSteerAngle()/127.0 * M_PI*STEER_RATIO; //実舵角取得
-    Vb              = V * cos(steeringAngle);                   //2輪モデルにおける車体進行方向速度取得
-    R               = mySign(steeringAngle) * (1.0 + A*Vb*Vb) * WHEEL_BASE/myMax(myAbs(steeringAngle), 0.01);  //理論旋回半径取得
+    steeringAngle = getSteerAngle()/127.0 * M_PI*STEER_RATIO;   //実舵角取得
+    Vb = V * cos(steeringAngle);                                //2輪モデルにおける車体進行方向速度取得
 
-    if(myAbs(R) < 5.0)
-        R = mySign(steeringAngle) * 5.0;
+    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)
+        R = mySign(steeringAngle) * 1.0;
     if(myAbs(R) > 100.0)
         R = mySign(steeringAngle) * 100.0;
 
@@ -673,11 +678,7 @@
             Vw = 0.5*TIRE_DIAMETER*2.0*M_PI*wheelRpsRL;             //駆動輪速度[m/s]
         }
 
-        if(Vb < TGT_VEHICLE_SPEED) {                                //対地車速が一定値より小さい場合(停車状態)
-            e[rlFlag][0] = TGT_VEHICLE_SPEED/(1.0 - TGT_SLIP_RATIO) - Vw;   //Rev limit制御(一定回転数以上回転させない)
-        } else {
-            e[rlFlag][0] = Vb/(1.0 - TGT_SLIP_RATIO) - Vw;          //現在の偏差取得(目標スリップ率における車輪速と駆動輪速度の差分)
-        }
+        e[rlFlag][0] = Vb/(1.0 - TGT_SLIP_RATIO) - Vw;          //現在の偏差取得(目標スリップ率における車輪速と駆動輪速度の差分)
 
         lastMotorTrq[rlFlag] = calcPID(lastMotorTrq[rlFlag], e[rlFlag]);    //PIDコントローラへどーーーん
         e[rlFlag][2] = e[rlFlag][1];
@@ -758,7 +759,7 @@
 
     if((errCounter.brakeOverRide > ERRCOUNTER_DECISION) || (readyToDriveFlag == 1))
         requestTorque = 0;
-        
+
     distributionTrq = (int)(distributeTorque(M_PI * getSteerAngle() / 127.0f) / 2.0f);  //片モーターのトルク分配量計算
     disTrq_omega = (int)((distributeTorque_omega(M_PI * getSteerAngle() / 127.0f)*limitTorqueDistribution()) / 2.0f);      //微分制御