Diff: TVDCTRL.cpp

Revision:

47:949e6c2e69fc

Parent:

46:16f1a7a01f5f

Child:

48:45614aa0db15

--- a/TVDCTRL.cpp	Tue Dec 12 11:37:33 2017 +0000
+++ b/TVDCTRL.cpp	Wed Dec 13 09:20:32 2017 +0000
@@ -389,26 +389,6 @@
     return gRps[position];
 }
 
-//***********************************
-//スリップ率取得関数
-//select    :0---RIGHT WHEEL
-//           1---LEFT WHEEL
-float getSlipRatio(bool rl_select)
-{
-    float slipRatio;
-
-    if(rl_select == 0) {
-        slipRatio = 1.0f - getWheelRps(FR_WHEEL) / getWheelRps(RR_MOTOR);
-    } else {
-        slipRatio = 1.0f - getWheelRps(FL_WHEEL) / getWheelRps(RL_MOTOR);
-    }
-
-    if(slipRatio < 0.0f)
-        slipRatio = 0.0f;    //減速時は考慮しない
-
-    return slipRatio;
-}
-
 //車速取得関数[m/s]
 //左右従動輪回転数の平均値から車速を演算
 double getVelocity(void)
@@ -642,12 +622,12 @@
 {
     //------------------------------
     //Constant variables
-    const double KP = 1.0;
+    const double KP = 40.0;
     const double KI = 0.0;
     const double KD = 0.0;
     //------------------------------
 
-    return lastOutput + KP*(e[0] - e[1]) + KI*(e[1] + e[0]) + KD*(e[0] - 2*e[1] + e[2]); //PID controller
+    return lastOutput + KP*(e[0] - e[1]) + KI*(e[1] + e[0]) + KD*(e[0] - 2.0*e[1] + e[2]);    //PID controller
 }
 
 //--------------------------------------------------
@@ -661,43 +641,63 @@
 {
     //------------------------------
     //Constant variables
-    const double TGT_SLIP_RATIO = 0.3;
+    const double TGT_SLIP_RATIO = 0.5;
     //------------------------------
 
     double nowMotorTrq = i_motorTrq * LSB_MOTOR_TORQUE;         //実数値へ変換
-    double steeringAngle, R, Vb, Vw;    
+    double outputTrq;
+    double steeringAngle, R, V, Vb, Vw, wheelRpsRR, wheelRpsRL;
     static double lastMotorTrq[2] = {0};                        //前回の出力トルク
     static double e[2][3] = {0};                                //3つ前の偏差まで保持
 
-    steeringAngle = (getSteerAngle() - 127)/127.0 * M_PI*STEER_RATIO;   //実舵角取得
+    V = getVelocity();                                          //前輪回転方向における車速換算値
+    
+//    V = 5.6;
     
-    Vb = getVelocity() * cos(steeringAngle);                    //2輪モデルにおける車体進行方向速度取得
+    wheelRpsRR = getWheelRps(RR_MOTOR);
+    wheelRpsRL = getWheelRps(RL_MOTOR);
+
+    if(V <= 0.0 || wheelRpsRR <= 0.0 || wheelRpsRL <= 0.0)
+        return i_motorTrq;                                      //車体が止まっている時はトラコンOFF
 
-    R = mySign(steeringAngle) * (1.0 + A*Vb*Vb) * WHEEL_BASE/myMin(myAbs(steeringAngle), 0.001);    //理論旋回半径取得
-    
-    if(rlFlag) {
+    steeringAngle   = (getSteerAngle() - 127)/127.0 * M_PI*STEER_RATIO; //実舵角取得
+    steeringAngle   = 0;
+    Vb              = V * cos(steeringAngle);                           //2輪モデルにおける車体進行方向速度取得
+    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(rlFlag == 0) {
         Vb = Vb*(1.0 + TREAD/(2.0*R));                          //トレッドを考慮した従動輪速度[m/s]
-        Vw = 0.5*TIRE_DIAMETER*2.0*M_PI*getWheelRps(RR_MOTOR);  //駆動輪速度[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
-        Vw = 0.5*TIRE_DIAMETER*2.0*M_PI*getWheelRps(RL_MOTOR);  //駆動輪速度[m/s]
+        Vw = 0.5*TIRE_DIAMETER*2.0*M_PI*wheelRpsRL;             //駆動輪速度[m/s]
     }
 
     e[rlFlag][0] = Vb/(1.0 - TGT_SLIP_RATIO) - Vw;              //現在の偏差取得(目標スリップ率における車輪速と実駆動輪速度の差分)
 
+    if(rlFlag == false)
+        printf("%2.5f\r\n", e[rlFlag][0]);
+        
     lastMotorTrq[rlFlag] = calcPID(lastMotorTrq[rlFlag], e[rlFlag]);    //PIDコントローラへどーーーん
-    
-    if(lastMotorTrq[rlFlag] < 0.0)      //現状、マイナストルクは無しで
-        lastMotorTrq[rlFlag] = 0.0;
 
     e[rlFlag][2] = e[rlFlag][1];
     e[rlFlag][1] = e[rlFlag][0];
 
-    if(lastMotorTrq[rlFlag] > nowMotorTrq) {
-        lastMotorTrq[rlFlag] = nowMotorTrq;                      //要求トルクより大きいのはどーーーんなのでリミット
-    }
-    
-    return (int)(lastMotorTrq[rlFlag] / LSB_MOTOR_TORQUE + 0.5);    //四捨五入してreturn
+    outputTrq = myMin(nowMotorTrq, lastMotorTrq[rlFlag]);    //ちっさい方を採用
+
+    if(outputTrq < 0.0)                              //現状、マイナストルクは無しで
+        outputTrq = 0.0;
+
+    if(outputTrq > MAX_MOTOR_TORQUE_POWER*LSB_MOTOR_TORQUE)
+        outputTrq = MAX_MOTOR_TORQUE_POWER*LSB_MOTOR_TORQUE;
+
+    if(outputTrq < MAX_MOTOR_TORQUE_REGENERATIVE*LSB_MOTOR_TORQUE)
+        outputTrq = MAX_MOTOR_TORQUE_REGENERATIVE*LSB_MOTOR_TORQUE;
+
+    return (int)(outputTrq / LSB_MOTOR_TORQUE + 0.5);    //四捨五入してreturn
 }
 
 void driveTVD(int TVDmode, bool isRedyToDrive)
@@ -712,7 +712,8 @@
     loadSteerAngle();   //舵角更新
     loadRps();          //従動輪・モータ回転数更新
 
-    if(isRedyToDrive && isBrakeOn())
+//    if(isRedyToDrive && isBrakeOn())
+    if(isRedyToDrive)
         readyToDriveFlag = 0;
 
     if((errCounter.apsUnderVolt > ERRCOUNTER_DECISION)
@@ -745,37 +746,38 @@
 
     torqueRight += disTrq_omega;
     torqueLeft -= disTrq_omega;
-    
+
     torqueRight = getTractionCtrl(torqueRight, 0);
-    torqueLeft = getTractionCtrl(torqueLeft, 1);
+    torqueLeft = torqueRight;
+//    torqueLeft = getTractionCtrl(torqueLeft, 1);
 
     //現在バグあり
     //アクセル全開で旋回後、舵を中立に戻していくと加速する。旋回を優先するモード
-    if(requestTorque < MIN_INNERWHEEL_MOTOR_TORQUE) {
-        torqueRight = torqueLeft = requestTorque;     //内輪側モーター最低トルクより小さい要求トルクなら等配分
-    } else {
-        if(torqueLeft > MAX_OUTPUT_TORQUE_POWER) { //片モーター上限時最大値にクリップ
-            torqueLeft = MAX_OUTPUT_TORQUE_POWER;
-
-            if(((torqueRight + torqueLeft)/2.0f) > requestTorque) {
-                torqueRight = requestTorque - (MAX_OUTPUT_TORQUE_POWER-requestTorque);
-            }
-        }
-        if(torqueRight > MAX_OUTPUT_TORQUE_POWER) { //片モーター上限時最大値にクリップ
-            torqueRight = MAX_OUTPUT_TORQUE_POWER;
-            if(((torqueRight + torqueLeft)/2.0f) > requestTorque) {
-                torqueLeft = requestTorque - (MAX_OUTPUT_TORQUE_POWER-requestTorque);
-            }
-        }
-        if(torqueLeft < MIN_INNERWHEEL_MOTOR_TORQUE) {  //内輪最低トルク時
-            torqueLeft = MIN_INNERWHEEL_MOTOR_TORQUE;      //内輪最低トルクにクリップ
-            torqueRight = (int)((requestTorque-MIN_INNERWHEEL_MOTOR_TORQUE)*2.0) + MIN_INNERWHEEL_MOTOR_TORQUE;   //片モーター下限値時，トルク高側のモーターも出力クリップ
-        }
-        if(torqueRight < MIN_INNERWHEEL_MOTOR_TORQUE) {  //内輪最低トルク時
-            torqueRight = MIN_INNERWHEEL_MOTOR_TORQUE;      //内輪最低トルクにクリップ
-            torqueLeft = (int)((requestTorque-MIN_INNERWHEEL_MOTOR_TORQUE)*2.0) + MIN_INNERWHEEL_MOTOR_TORQUE;   //片モーター下限値時，トルク高側のモーターも出力クリップ
-        }
-    }
+//    if(requestTorque < MIN_INNERWHEEL_MOTOR_TORQUE) {
+//        torqueRight = torqueLeft = requestTorque;     //内輪側モーター最低トルクより小さい要求トルクなら等配分
+//    } else {
+//        if(torqueLeft > MAX_OUTPUT_TORQUE_POWER) { //片モーター上限時最大値にクリップ
+//            torqueLeft = MAX_OUTPUT_TORQUE_POWER;
+//
+//            if(((torqueRight + torqueLeft)/2.0f) > requestTorque) {
+//                torqueRight = requestTorque - (MAX_OUTPUT_TORQUE_POWER-requestTorque);
+//            }
+//        }
+//        if(torqueRight > MAX_OUTPUT_TORQUE_POWER) { //片モーター上限時最大値にクリップ
+//            torqueRight = MAX_OUTPUT_TORQUE_POWER;
+//            if(((torqueRight + torqueLeft)/2.0f) > requestTorque) {
+//                torqueLeft = requestTorque - (MAX_OUTPUT_TORQUE_POWER-requestTorque);
+//            }
+//        }
+//        if(torqueLeft < MIN_INNERWHEEL_MOTOR_TORQUE) {  //内輪最低トルク時
+//            torqueLeft = MIN_INNERWHEEL_MOTOR_TORQUE;      //内輪最低トルクにクリップ
+//            torqueRight = (int)((requestTorque-MIN_INNERWHEEL_MOTOR_TORQUE)*2.0) + MIN_INNERWHEEL_MOTOR_TORQUE;   //片モーター下限値時，トルク高側のモーターも出力クリップ
+//        }
+//        if(torqueRight < MIN_INNERWHEEL_MOTOR_TORQUE) {  //内輪最低トルク時
+//            torqueRight = MIN_INNERWHEEL_MOTOR_TORQUE;      //内輪最低トルクにクリップ
+//            torqueLeft = (int)((requestTorque-MIN_INNERWHEEL_MOTOR_TORQUE)*2.0) + MIN_INNERWHEEL_MOTOR_TORQUE;   //片モーター下限値時，トルク高側のモーターも出力クリップ
+//        }
+//    }
 
     gRightMotorTorque = torqueRight;
     gLeftMotorTorque = torqueLeft;