Diff: TVDCTRL.cpp

Revision:

42:3ab09d0e3071

Parent:

41:0c53acd31247

Child:

43:5da6b1574227

--- a/TVDCTRL.cpp	Fri Oct 27 09:11:31 2017 +0000
+++ b/TVDCTRL.cpp	Sat Oct 28 06:44:09 2017 +0000
@@ -274,41 +274,52 @@
     int rwp_dT2 = gRightWheelPulse_dT2;
     static bool preInputState = false;
     static int rwpStopCounter = 0;                //タイヤが止まっている間カウントアップ
+    static int currentTime = 0;
+    static float preRightWheelRPS = 0.0f;
 
+    //前回パルス入力がない場合
+    if(preInputState == false) {
+        //以前のdT1に前回の計測周期の時間を積算
+        rwp_dT1 = rwp_dT1 + currentTime;
+    }
+
+    //現在の時間取得
+    currentTime = wheelPulseTimer.read_us();
+
+    //次回計測周期までのパルス時間計測開始
+    wheelPulseTimer.reset();
     //パルス数クリア
     gRightWheelPulseCounter = 0;
     gLeftWheelPulseCounter = 0;
     //dT2の初期値はパルス入力ない状態 => 計測時間=0
     gRightWheelPulse_dT2 = 0;
-    //次回計測周期までのパルス時間計測開始
-    wheelPulseTimer.reset();
 
-    //前回パルス入力がない場合
-    if(preInputState == false) {
-        //以前のdT1に計測周期の時間を積算
-        rwp_dT1 = rwp_dT1 + TIRE_MEAS_CYCLE_US;
-        if(rwp_dT1 > MAX_WHEEL_PULSE_TIME_US)
-            rwp_dT1 = MAX_WHEEL_PULSE_TIME_US;  //overflow防止
-    }
+    //overflow防止処理
+    if(rwp_dT1 > MAX_WHEEL_PULSE_TIME_US)
+        rwp_dT1 = MAX_WHEEL_PULSE_TIME_US;
 
     //パルス入力あれば直前のパルス入力からの経過時間取得
     if(rwpCounter != 0) {
-        rwp_dT2 = TIRE_MEAS_CYCLE_US - rwp_dT2;
+        rwp_dT2 = currentTime - rwp_dT2;
     }
 
+    //ピーク値保持したい
     //パルス入力ない場合---(設定回数未満)前回値保持/(設定回数以上)疑似パルス入力判定
     if(rwpCounter == 0) {
-        if(rwpStopCounter < 100) {
+        if(rwpStopCounter < 5) {
             rwpStopCounter++;
         } else {
-            gRightWheelRPS = 1.0f / ((TIRE_MEAS_CYCLE_US + rwp_dT1 - rwp_dT2) / 1000000.0f) / WHEEL_PULSE_NUM;
+            gRightWheelRPS = (((float)rwpCounter / ((currentTime + rwp_dT1 - rwp_dT2) / 1000000.0f)) / (WHEEL_PULSE_NUM*4)) * 0.27f + (1.0f-0.27f)*preRightWheelRPS;
         }
     } else {
         //RPS計算[rps](1sec当たりパルス数/タイヤパルス数)
-        gRightWheelRPS = ((float)rwpCounter / ((TIRE_MEAS_CYCLE_US + rwp_dT1 - rwp_dT2) / 1000000.0f)) / WHEEL_PULSE_NUM;
+        gRightWheelRPS = (((float)rwpCounter / ((currentTime + rwp_dT1 - rwp_dT2) / 1000000.0f)) / (WHEEL_PULSE_NUM*4)) * 0.27f + (1.0f-0.27f)*preRightWheelRPS;
         rwpStopCounter = 0;
     }
 
+//    gRightWheelRPS = (((float)rwpCounter / ((currentTime + rwp_dT1 - rwp_dT2) / 1000000.0f)) / (WHEEL_PULSE_NUM*4)) * 0.2f + (1.0f-0.2f)*preRightWheelRPS;
+    preRightWheelRPS = gRightWheelRPS;
+
     //パルス入力あれば次回のdT1はdT2を採用(パルス入力なければ現在値保持)
     if(rwpCounter != 0)
         rwp_dT1 = rwp_dT2;
@@ -587,12 +598,13 @@
     int disTrq_omega=0;
     int torqueRight, torqueLeft;    //トルクの右左
     static unsigned int preMcpA=0, preMcpB=0;
+    float tempRWP = gRightWheelRPS;
 
     loadSensors();      //APS,BRAKE更新
     loadSteerAngle();   //舵角更新
 
 //    printf("%04d\r\n",gRightWheelPulseCounter);
-    printf("%f\r\n",gRightWheelRPS*60.0f);
+    printf("%f %f %f\r\n",tempRWP*60.0f, 300.0f, 0.0f);
 //    printf("%09d %09d %09d\r\n",rwp_dT1, rwp_dT2, rwpCounter);
 
     if(isRedyToDrive && isBrakeOn())
@@ -681,9 +693,9 @@
     rightMotorPulse.fall(&countRightMotorPulseISR);
     leftMotorPulse.fall(&countLeftMotorPulseISR);
     rightWheelPulse1.fall(&countRightWheelPulseISR);
-//    rightWheelPulse2.fall(&countRightWheelPulseISR);
-//    rightWheelPulse1.rise(&countRightWheelPulseISR);
-//    rightWheelPulse2.rise(&countRightWheelPulseISR);
+    rightWheelPulse2.fall(&countRightWheelPulseISR);
+    rightWheelPulse1.rise(&countRightWheelPulseISR);
+    rightWheelPulse2.rise(&countRightWheelPulseISR);
     leftWheelPulse1.fall(&countLeftWheelPulseISR);
 
     ticker1.attach(&loadSensorsISR, CONTROL_CYCLE_S);   //制御周期毎にデータ読み込み(LPF演算のため)