Diff: TVDCTRL.cpp

Revision:

45:8e5d35beb957

Parent:

44:d433bb5f77c0

Child:

46:16f1a7a01f5f

--- a/TVDCTRL.cpp	Sat Dec 02 10:52:00 2017 +0000
+++ b/TVDCTRL.cpp	Wed Dec 06 08:02:28 2017 +0000
@@ -135,9 +135,9 @@
         tmpRawBrake = (int)brake.read_u16();
         
         //Low Pass Filter
-        tmpApsP = (int)(tmpRawApsP * ratioLPF + preApsP * (1.0-ratioLPF));
-        tmpApsS = (int)(tmpRawApsS * ratioLPF + preApsS * (1.0-ratioLPF));
-        tmpBrake = (int)(tmpRawBrake * ratioLPF + preBrake * (1.0-ratioLPF));
+        tmpApsP = (int)(tmpRawApsP * ratioLPF_ACC_BRK + preApsP * (1.0-ratioLPF_ACC_BRK));
+        tmpApsS = (int)(tmpRawApsS * ratioLPF_ACC_BRK + preApsS * (1.0-ratioLPF_ACC_BRK));
+        tmpBrake = (int)(tmpRawBrake * ratioLPF_ACC_BRK + preBrake * (1.0-ratioLPF_ACC_BRK));
 
         //生のセンサ値取得
         rawApsP = tmpApsP;
@@ -285,13 +285,13 @@
 }
 //***********************************
 
-//回転数構造体初期化関数
-rps_t initRps(void)
-{
-    rps_t initResult = {0, MAX_WHEEL_PULSE_TIME_US, 0, false, 0, 0.0f};
-    return initResult;
-}
-
+////回転数構造体初期化関数
+//rps_t initRps(void)
+//{
+//    rps_t initResult = {0, MAX_WHEEL_PULSE_TIME_US, 0, false, 0, 0.0f};
+//    return initResult;
+//}
+//
 //RPS読み込み許可設定関数
 void loadRpsISR(void)
 {
@@ -301,7 +301,9 @@
 //RPS読み込み関数
 void loadRps(void)
 {
-    static rps_t rps[4] = {initRps(), initRps(), initRps(), initRps()};
+    const rps_t INITRPS = {0, MAX_WHEEL_PULSE_TIME_US, 0, false, 0, 0.0f};  //構造体初期化用定数
+//    static rps_t rps[4] = {initRps(), initRps(), initRps(), initRps()};
+    static rps_t rps[4] = {INITRPS, INITRPS, INITRPS, INITRPS};
     static int currentTime[4] = {0};
     float pulseNumPerRev;
 
@@ -334,7 +336,7 @@
         gWheelPulse_dT2[i]    = 0;
 
         //一回転当たりのパルス数設定
-        if(i <= 1)
+        if(i <= FL_WHEEL)
             pulseNumPerRev = WHEEL_PULSE_NUM;   //Front車輪パルス数*割込み回数
         else
             pulseNumPerRev = MOTOR_PULSE_NUM;   //モータパルス数*割込み回数
@@ -376,12 +378,12 @@
     float deltaN;   //左右モータ回転数差
     float aveN;     //左右モータ回転数平均値
 
-    if(position < 2)
-        return gRps[position];
-    else {
+    if(position <= FL_WHEEL)                                                //前輪の場合
+        return gRps[position];                                              //演算結果をそのまま適用
+    else {                                                                  //後輪の場合,モータ回転数から速度線図に従い演算
         //右車輪回転数が大きいと仮定
-        aveN    = ((gRps[RR_MOTOR] + gRps[RL_MOTOR]) / GEAR_RATIO) / 2.0f;    //平均回転数計算
-        deltaN  = ((gRps[RR_MOTOR] - gRps[RL_MOTOR]) / GEAR_RATIO) / ALPHA;   //速度線図上の車輪回転数差計算
+        aveN    = ((gRps[RR_MOTOR] + gRps[RL_MOTOR]) / GEAR_RATIO) / 2.0f;  //平均回転数計算
+        deltaN  = ((gRps[RR_MOTOR] - gRps[RL_MOTOR]) / GEAR_RATIO) / ALPHA; //速度線図上の車輪回転数差計算
 
         if(position == RR_MOTOR)
             return aveN + deltaN / 2.0f;  //右車輪回転数
@@ -619,9 +621,6 @@
     else if(requestTorque < MAX_OUTPUT_TORQUE_REGENERATIVE)
         requestTorque = MAX_OUTPUT_TORQUE_REGENERATIVE;
 
-    if((errCounter.brakeOverRide > ERRCOUNTER_DECISION) || (readyToDriveFlag == 1))
-        requestTorque = 0;
-
     return requestTorque;
 }
 
@@ -654,8 +653,6 @@
     loadSteerAngle();   //舵角更新
     loadRps();          //従動輪・モータ回転数更新
 
-//    printf("%d\r\n", getSteerAngle());
-
     if(isRedyToDrive && isBrakeOn())
         readyToDriveFlag = 0;
 
@@ -675,10 +672,13 @@
 
     requestTorque=calcRequestTorque();  //ドライバー要求トルク取得
 
+    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);      //微分制御
 
-//    distributionTrq = 0;
+    distributionTrq = 0;
     disTrq_omega = 0;
 
     torqueRight = requestTorque + distributionTrq;