Diff: TVDCTRL.cpp

Revision:

39:c05074379713

Parent:

38:11753ee9734f

Child:

40:8e33c60c6590

--- a/TVDCTRL.cpp	Tue Oct 24 10:19:51 2017 +0000
+++ b/TVDCTRL.cpp	Thu Oct 26 02:04:02 2017 +0000
@@ -1,6 +1,7 @@
 #include "TVDCTRL.h"
 #include "MCP4922.h"
 #include "Steering.h"
+#include "Global.h"
 
 extern AnalogIn apsP;
 extern AnalogIn apsS;
@@ -12,10 +13,10 @@
 extern DigitalIn sdState;
 extern InterruptIn rightMotorPulse;
 extern InterruptIn leftMotorPulse;
-extern InterruptIn rightTirePulse1;
-extern InterruptIn rightTirePulse2;
-extern InterruptIn leftTirePulse1;
-extern InterruptIn leftTirePulse2;
+extern InterruptIn rightWheelPulse1;
+extern InterruptIn rightWheelPulse2;
+extern InterruptIn leftWheelPulse1;
+extern InterruptIn leftWheelPulse2;
 extern MCP4922 mcp;
 extern Serial pc;
 extern AnalogOut STR2AN;
@@ -23,13 +24,10 @@
 
 #define indicateSystem(x)       (indicatorLed.write(x))
 
-Timer RightPulseTimer;
-Timer LeftPulseTimer;
+Timer wheelPulseTimer;
 Ticker ticker1;
 Ticker ticker2;
 
-#define myAbs(x)    ((x>0)?(x):(-(x)))
-
 #define apsPVol() (apsP.read() * 3.3)
 #define apsSVol() (apsS.read() * 3.3)
 
@@ -233,9 +231,19 @@
     }
 }
 
+//車輪速計測は”【空転再粘着制御】山下道寛”を参照のこと(一部改修)
+//パルス数カウンタ
 volatile int gRightMotorPulseCounter = 0, gLeftMotorPulseCounter = 0;
+volatile int gRightWheelPulseCounter = 0, gLeftWheelPulseCounter = 0;
+//パルス入力までの時間
+volatile int gRightWheelPulse_dT2 = 0, gLeftWheelPulse_dT2 = 0;
+
+volatile float gRightWheelRPS = 0, gLeftWheelRPS = 0;
+
 volatile bool pulseTimeISRFlag = false;
 
+/***********************************/
+//モータパルスカウント
 void countRightMotorPulseISR(void)
 {
     gRightMotorPulseCounter++;
@@ -245,6 +253,57 @@
 {
     gLeftMotorPulseCounter++;
 }
+/***********************************/
+
+/***********************************/
+//ホイールパルスカウント
+void countRightWheelPulseISR(void)
+{
+    gRightWheelPulseCounter++;                                  //パルス数カウント
+    gRightWheelPulse_dT2 = wheelPulseTimer.read_us();      //現在の時間いただきます
+}
+
+void countLeftWheelPulseISR(void)
+{
+}
+/***********************************/
+
+void measRpsISR(void)
+{
+    int rwpCounter = gRightWheelPulseCounter;
+    static int rwp_dT1 = MAX_WHEEL_PULSE_TIME_US;  //初期設定は無限時間前にパルス入力があったと仮定
+    int rwp_dT2 = gRightWheelPulse_dT2;
+    int currentTime = wheelPulseTimer.read_us();
+
+    //計測周期内にパルス入力なければ
+    if(rwpCounter == 0) {
+        //以前のdT1に計測周期の時間を積算
+        rwp_dT1 = rwp_dT1 + CONTROL_CYCLE_US;
+        if(rwp_dT1 > MAX_WHEEL_PULSE_TIME_US)
+            rwp_dT1 = MAX_WHEEL_PULSE_TIME_US;      //overflow防止
+        //(dT2は0であるはず)
+    } else {
+        //パルス入力あれば直前のパルス入力からの経過時間取得
+        rwp_dT2 = currentTime - rwp_dT2;
+    }
+
+    //RPS計算[rps](1sec当たりパルス数/タイヤパルス数)
+    gRightWheelRPS = ((float)rwpCounter / (CONTROL_CYCLE_US + rwp_dT1 - rwp_dT2) / 1000000.0f) / WHEEL_PULSE_NUM;
+    
+    //パルス入力あれば次回のdT1はdT2を採用(パルス入力なければ現在値保持)
+    if(rwpCounter != 0)
+        rwp_dT1 = gRightWheelPulse_dT2;
+
+    //dT2の初期値はパルス入力ない状態 => 計測時間=0
+    gRightWheelPulse_dT2 = 0;
+
+    //パルス数クリア
+    gRightWheelPulseCounter = 0;
+    gLeftWheelPulseCounter = 0;
+
+    //次回計測周期までのパルス時間計測開始
+    wheelPulseTimer.reset();
+}
 
 void getPulseCounterISR(void)
 {
@@ -289,9 +348,9 @@
         //こっちはタイヤ回転数
         //そのうち対応
         if(rl == RIGHT)
-            rps = (float)sumRightMotorPulse / MOTOR_PULSE_NUM;    //過去1秒間のモータパルス数を使ってRPS算出
+            rps = gRightWheelRPS;    //過去1秒間のモータパルス数を使ってRPS算出
         else
-            rps = (float)sumLeftMotorPulse / MOTOR_PULSE_NUM;     //過去1秒間のモータパルス数を使ってRPS算出
+            rps = gLeftWheelRPS;     //過去1秒間のモータパルス数を使ってRPS算出
     }
     return (int)(rps / LSB_MOTORSPEED);     //LSB変換
 }
@@ -597,13 +656,14 @@
 
 void initTVD(void)
 {
-    RightPulseTimer.reset();
-    LeftPulseTimer.reset();
-    RightPulseTimer.start();
-    LeftPulseTimer.start();
+    wheelPulseTimer.reset();
+
+    wheelPulseTimer.start();
 
     rightMotorPulse.fall(&countRightMotorPulseISR);
     leftMotorPulse.fall(&countLeftMotorPulseISR);
+    rightWheelPulse1.fall(&countRightWheelPulseISR);
+    leftWheelPulse1.fall(&countLeftWheelPulseISR);
 
     ticker1.attach(&loadSensorsISR, CONTROL_CYCLE_S);   //制御周期毎にデータ読み込み(LPF演算のため)
     ticker2.attach(&getPulseCounterISR, CONTROL_CYCLE_S);  //