Diff: TVDCTRL.cpp

Revision:

43:5da6b1574227

Parent:

42:3ab09d0e3071

Child:

44:d433bb5f77c0

diff -r 3ab09d0e3071 -r 5da6b1574227 TVDCTRL.cpp
--- a/TVDCTRL.cpp	Sat Oct 28 06:44:09 2017 +0000
+++ b/TVDCTRL.cpp	Thu Nov 02 01:56:46 2017 +0000
@@ -24,7 +24,7 @@
 
 #define indicateSystem(x)       (indicatorLed.write(x))
 
-Timer wheelPulseTimer;
+Timer wheelPulseTimer[4];
 Ticker ticker1;
 Ticker ticker2;
 
@@ -47,9 +47,9 @@
 
 int gRightMotorTorque=0, gLeftMotorTorque=0;
 
-int getMotorTorque(Select rl)
+int getMotorTorque(select_t rl)
 {
-    return ((rl==LEFT) ? gLeftMotorTorque : gRightMotorTorque);
+    return ((rl==RL_MOTOR) ? gLeftMotorTorque : gRightMotorTorque);
 }
 
 //エラーカウンタ外部参照用関数
@@ -231,165 +231,170 @@
     }
 }
 
+//*******************************************************
 //車輪速計測は”【空転再粘着制御】山下道寛”を参照のこと(一部改修)
+//*******************************************************
+//wheelNumbler:ホイール識別番号
+//FR:0 FL:1 RR:2 RL:3
+typedef struct {
+    int counter;
+    int dT1;
+    int dT2;
+    bool preInputState;
+    int stopCounter;
+    float preRps;
+} rps_t;
+
 //パルス数カウンタ
-volatile int gRightMotorPulseCounter = 0, gLeftMotorPulseCounter = 0;
-volatile int gRightWheelPulseCounter = 0, gLeftWheelPulseCounter = 0;
+volatile int gWheelPulseCounter[4] = {0};
 //パルス入力までの時間
-volatile int gRightWheelPulse_dT2 = 0, gLeftWheelPulse_dT2 = 0;
-volatile float gRightWheelRPS = 0, gLeftWheelRPS = 0;
+volatile int gWheelPulse_dT2[4] = {0};
+//現在の回転数[rps]
+float gRps[4] = {0};
+//車輪速計測周期フラグ
+volatile bool gloadWheelRpsFlag = false;
 
-volatile bool pulseTimeISRFlag = false;
-
-/***********************************/
+//***********************************
 //モータパルスカウント
 void countRightMotorPulseISR(void)
 {
-    gRightMotorPulseCounter++;
+    gWheelPulseCounter[RR_MOTOR]++;
+    gWheelPulse_dT2[RR_MOTOR] = wheelPulseTimer[RR_MOTOR].read_us();      //現在の時間いただきます
 }
-
 void countLeftMotorPulseISR(void)
 {
-    gLeftMotorPulseCounter++;
+    gWheelPulseCounter[RL_MOTOR]++;
+    gWheelPulse_dT2[RL_MOTOR] = wheelPulseTimer[RL_MOTOR].read_us();      //現在の時間いただきます
 }
-/***********************************/
-
-/***********************************/
+//***********************************
 //ホイールパルスカウント
 void countRightWheelPulseISR(void)
 {
-    gRightWheelPulseCounter++;                                  //パルス数カウント
-    gRightWheelPulse_dT2 = wheelPulseTimer.read_us();      //現在の時間いただきます
+    gWheelPulseCounter[FR_WHEEL]++;
+    gWheelPulse_dT2[FR_WHEEL] = wheelPulseTimer[FR_WHEEL].read_us();      //現在の時間いただきます
 }
-
 void countLeftWheelPulseISR(void)
 {
+    gWheelPulseCounter[FL_WHEEL]++;
+    gWheelPulse_dT2[FL_WHEEL] = wheelPulseTimer[FL_WHEEL].read_us();      //現在の時間いただきます
 }
-/***********************************/
-
-void measRpsISR(void)
-{
-    int rwpCounter = gRightWheelPulseCounter;
-    static int rwp_dT1 = MAX_WHEEL_PULSE_TIME_US;  //初期設定は無限時間前にパルス入力があったと仮定
-    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;
+//***********************************
 
-    //overflow防止処理
-    if(rwp_dT1 > MAX_WHEEL_PULSE_TIME_US)
-        rwp_dT1 = MAX_WHEEL_PULSE_TIME_US;
-
-    //パルス入力あれば直前のパルス入力からの経過時間取得
-    if(rwpCounter != 0) {
-        rwp_dT2 = currentTime - rwp_dT2;
-    }
+//回転数構造体初期化関数
+rps_t initRps(void)
+{
+    rps_t initResult = {0, MAX_WHEEL_PULSE_TIME_US, 0, false, 0, 0.0f};
+    return initResult;
+}
 
-    //ピーク値保持したい
-    //パルス入力ない場合---(設定回数未満)前回値保持/(設定回数以上)疑似パルス入力判定
-    if(rwpCounter == 0) {
-        if(rwpStopCounter < 5) {
-            rwpStopCounter++;
-        } else {
-            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 / ((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;
-
-    if(rwpCounter == 0)
-        preInputState = false;
-    else
-        preInputState = true;
+//RPS読み込み許可設定関数
+void loadRpsISR(void)
+{
+    gloadWheelRpsFlag = true;
 }
 
-void getPulseCounterISR(void)
-{
-    pulseTimeISRFlag = true;
-}
-
-//default argument  : switchWheel=false
-int getRPS(Select rl, bool switchWheel)
+//RPS読み込み関数
+void loadRps(void)
 {
-    static int rightMotorPulse[100]= {0}, leftMotorPulse[100]= {0};     //過去1.0秒間のパルス数格納
-    static int sumRightMotorPulse, sumLeftMotorPulse;
-    float rps;
+    static rps_t rps[4] = {initRps(), initRps(), initRps(), initRps()};
+    static int currentTime[4] = {0};
+    float pulseNumPerRev;
+
+    if(false == gloadWheelRpsFlag)
+        return;
+    else
+        gloadWheelRpsFlag = false;
+
+    for(int i=0; i<4; i++) {
+        rps[i].counter  = gWheelPulseCounter[i];
+        rps[i].dT2      = gWheelPulse_dT2[i];
 
-    if(pulseTimeISRFlag == true) {
-        for(int i=99; i>0; i--) {
-            rightMotorPulse[i] = rightMotorPulse[i-1];
-            leftMotorPulse[i] = leftMotorPulse[i-1];
+        //前回パルス入力がない場合
+        if(rps[i].preInputState == false) {
+            //以前のdT1に前回の計測周期の時間を積算
+            rps[i].dT1 = rps[i].dT1 + currentTime[i];
+            //overflow防止処理
+            if(rps[i].dT1 > MAX_WHEEL_PULSE_TIME_US)
+                rps[i].dT1 = MAX_WHEEL_PULSE_TIME_US;
+        }
+
+        //現在の時間取得
+        currentTime[i] = wheelPulseTimer[i].read_us();
+
+        //次回計測周期までのパルス時間計測開始
+        wheelPulseTimer[i].reset();
+        //パルス数クリア
+        gWheelPulseCounter[i] = 0;
+        //dT2の初期値はパルス入力ない状態 => 計測時間=0
+        gWheelPulse_dT2[i]    = 0;
+
+        //一回転当たりのパルス数設定
+        if(i <= 1)
+            pulseNumPerRev = WHEEL_PULSE_NUM;   //Front車輪パルス数*割込み回数
+        else
+            pulseNumPerRev = MOTOR_PULSE_NUM;   //モータパルス数*割込み回数
+
+        //パルス入力あれば直前のパルス入力からの経過時間取得
+        if(rps[i].counter != 0) {
+            rps[i].dT2 = currentTime[i] - rps[i].dT2;
         }
 
-        rightMotorPulse[0] = gRightMotorPulseCounter;
-        leftMotorPulse[0] = gLeftMotorPulseCounter;
-
-        gRightMotorPulseCounter = 0;
-        gLeftMotorPulseCounter = 0;
+        //パルス入力ない場合---(設定回数未満)前回値保持/(設定回数以上)疑似パルス入力判定 (ピーク値を保存したい)
+        if(rps[i].counter == 0) {
+            if(rps[i].stopCounter < 50) //低回転数時、急に0rpsと演算しないように前回値保持(設定値はだいたい)
+                rps[i].stopCounter++;
+            else
+                gRps[i] = 0.0f;
+        } else {
+            //RPS計算[rps](1sec当たりパルス数/タイヤパルス数)
+            gRps[i] = ((float)rps[i].counter / ((currentTime[i] + rps[i].dT1 - rps[i].dT2) / 1000000.0f)) / pulseNumPerRev;
+            gRps[i] = gRps[i] * ratioLPF_RPS + (1.0f-ratioLPF_RPS)*rps[i].preRps;
+            rps[i].stopCounter = 0;
+        }
 
-        sumRightMotorPulse = 0;
-        sumLeftMotorPulse = 0;
+        rps[i].preRps = gRps[i];
 
-        for(int i=0; i<100; i++) {
-            sumRightMotorPulse += rightMotorPulse[i];
-            sumLeftMotorPulse += leftMotorPulse[i];
-        }
-        pulseTimeISRFlag = false;
+        //パルス入力あれば次回のdT1はdT2を採用(パルス入力なければ現在値保持)
+        if(rps[i].counter != 0)
+            rps[i].dT1 = rps[i].dT2;
+
+        if(rps[i].counter == 0)
+            rps[i].preInputState = false;
+        else
+            rps[i].preInputState = true;
     }
-
-    if(switchWheel == false) {
-        if(rl == RIGHT)
-            rps = (float)1.0f*sumRightMotorPulse / MOTOR_PULSE_NUM / GEAR_RATIO;    //過去0.5秒間のモータパルス数を使ってRPS算出
-        else
-            rps = (float)1.0f*sumLeftMotorPulse / MOTOR_PULSE_NUM / GEAR_RATIO;     //過去0.5秒間のモータパルス数を使ってRPS算出
-    } else {
-        //こっちはタイヤ回転数
-        //そのうち対応
-        if(rl == RIGHT)
-            rps = gRightWheelRPS;    //過去1秒間のモータパルス数を使ってRPS算出
-        else
-            rps = gLeftWheelRPS;     //過去1秒間のモータパルス数を使ってRPS算出
-    }
-    return (int)(rps / LSB_MOTORSPEED);     //LSB変換
 }
 
+//車輪RPS取得関数
+float getWheelRps(select_t position)
+{
+    float deltaN;   //左右モータ回転数差
+    float aveN;     //左右モータ回転数平均値
+
+    if(position < 2)
+        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;   //速度線図上の車輪回転数差計算
+
+        if(position == RR_MOTOR)
+            return aveN + deltaN / 2.0f;  //右車輪回転数
+        else
+            return aveN - deltaN / 2.0f;  //左車輪回転数
+    }
+}
+
+float getRps(select_t position)
+{
+    return gRps[position];
+}
+
+//車速取得関数[m/s]
+//左右従動輪回転数の平均値から車速を演算
 float getVelocity(void)
 {
-//    static float debugVelocity = 0.0f;
-//    debugVelocity += 0.002;
-//
-//    printf("%1.2f\r\n", debugVelocity);
-//
-//    return debugVelocity;
-    return (0.5f*TIRE_DIAMETER*2*M_PI*(getRPS(RIGHT) + getRPS(LEFT))*0.5f)*LSB_MOTORSPEED;
-//    return 15.0f;
+    return (0.5f*TIRE_DIAMETER*2.0f*M_PI*(getWheelRps(FR_WHEEL) + getWheelRps(FL_WHEEL))*0.5f);
 }
 
 int distributeTorque_omega(float steeringWheelAngle)
@@ -586,8 +591,6 @@
     else
         limitRate = 1.0f;
 
-    //printf("rate:%1.3f\r\n", limitRate);
-
     return limitRate;
 }
 
@@ -598,14 +601,12 @@
     int disTrq_omega=0;
     int torqueRight, torqueLeft;    //トルクの右左
     static unsigned int preMcpA=0, preMcpB=0;
-    float tempRWP = gRightWheelRPS;
 
     loadSensors();      //APS,BRAKE更新
     loadSteerAngle();   //舵角更新
+    loadRps();       //従動輪・モータ回転数更新
 
-//    printf("%04d\r\n",gRightWheelPulseCounter);
-    printf("%f %f %f\r\n",tempRWP*60.0f, 300.0f, 0.0f);
-//    printf("%09d %09d %09d\r\n",rwp_dT1, rwp_dT2, rwpCounter);
+    printf("%f %f %f\r\n", getWheelRps(RR_MOTOR), 1.0f, 0.0f);
 
     if(isRedyToDrive && isBrakeOn())
         readyToDriveFlag = 0;
@@ -629,8 +630,6 @@
     distributionTrq = (int)((distributeTorque(M_PI * getSteerAngle() / 127.0f, getVelocity())*limitTorqueDistribution()) / 2.0f);  //片モーターのトルク分配量計算
     disTrq_omega = (int)((distributeTorque_omega(M_PI * getSteerAngle() / 127.0f)*limitTorqueDistribution()) / 2.0f);      //微分制御
 
-    //printf("%d\r\n", distributionTrq);
-
 //    distributionTrq = 0;
     disTrq_omega = 0;
 
@@ -641,21 +640,21 @@
     torqueLeft -= disTrq_omega;
 
     if(torqueRight < 0) {
-        if((getRPS(RIGHT) * LSB_MOTORSPEED * 60.0f) < 600.0f) {
+        if((getRps(RR_MOTOR) * 60.0f) < 600.0f) {
             torqueLeft = requestTorque + torqueRight;
             torqueRight = 0;
-        } else if((getRPS(RIGHT) * LSB_MOTORSPEED * 60.0f) <= 1250.0f) {
-            torqueLeft = requestTorque + torqueRight*((getRPS(RIGHT) * GEAR_RATIO * LSB_MOTORSPEED * 60.0f - 600.0f)/(1250.0f - 600.0f));
-            torqueRight = torqueRight*((getRPS(RIGHT)-600.0f)/(1250.0f - 600.0f));
+        } else if((getRps(RR_MOTOR) * 60.0f) <= 1250.0f) {
+            torqueLeft = requestTorque + torqueRight*((getRps(RR_MOTOR) * GEAR_RATIO * 60.0f - 600.0f)/(1250.0f - 600.0f));
+            torqueRight = torqueRight*((getRps(RR_MOTOR)-600.0f)/(1250.0f - 600.0f));
         }
     }
     if(torqueLeft < 0) {
-        if((getRPS(LEFT) * LSB_MOTORSPEED * 60.0f) < 600.0f) {
+        if((getRps(RL_MOTOR) * 60.0f) < 600.0f) {
             torqueRight = requestTorque + torqueLeft;
             torqueLeft = 0;
-        } else if((getRPS(LEFT) * LSB_MOTORSPEED * 60.0f) <= 1250.0f) {
-            torqueRight = requestTorque + torqueLeft*((getRPS(LEFT) * GEAR_RATIO * LSB_MOTORSPEED * 60.0f - 600.0f)/(1250.0f - 600.0f));
-            torqueLeft = torqueLeft*((getRPS(LEFT)-600.0f)/(1250.0f - 600.0f));
+        } else if((getRps(RL_MOTOR) * 60.0f) <= 1250.0f) {
+            torqueRight = requestTorque + torqueLeft*((getRps(RL_MOTOR) * GEAR_RATIO * 60.0f - 600.0f)/(1250.0f - 600.0f));
+            torqueLeft = torqueLeft*((getRps(RL_MOTOR)-600.0f)/(1250.0f - 600.0f));
         }
     }
 
@@ -672,11 +671,9 @@
     gRightMotorTorque = torqueRight;
     gLeftMotorTorque = torqueLeft;
 
-    McpData.valA = calcTorqueToVoltage(torqueRight, getRPS(RIGHT));
-    McpData.valB = calcTorqueToVoltage(torqueLeft, getRPS(LEFT));
+    McpData.valA = calcTorqueToVoltage(torqueRight, getRps(RR_MOTOR));
+    McpData.valB = calcTorqueToVoltage(torqueLeft, getRps(RL_MOTOR));
 
-//    preMcpA = (unsigned int)(McpData.valA * 0.456 + preMcpA * 0.544);
-//    preMcpB = (unsigned int)(McpData.valB * 0.456 + preMcpB * 0.544);
     preMcpA = McpData.valA;
     preMcpB = McpData.valB;
 
@@ -686,27 +683,37 @@
 
 void initTVD(void)
 {
-    wheelPulseTimer.reset();
+    mcp.writeA(0);   //右モーター初期値
+    mcp.writeB(0);   //左モーター初期値
 
-    wheelPulseTimer.start();
+    wheelPulseTimer[FR_WHEEL].reset();
+    wheelPulseTimer[FL_WHEEL].reset();
+    wheelPulseTimer[RR_MOTOR].reset();
+    wheelPulseTimer[RL_MOTOR].reset();
 
-    rightMotorPulse.fall(&countRightMotorPulseISR);
-    leftMotorPulse.fall(&countLeftMotorPulseISR);
-    rightWheelPulse1.fall(&countRightWheelPulseISR);
-    rightWheelPulse2.fall(&countRightWheelPulseISR);
+    wheelPulseTimer[FR_WHEEL].start();
+    wheelPulseTimer[FL_WHEEL].start();
+    wheelPulseTimer[RR_MOTOR].start();
+    wheelPulseTimer[RL_MOTOR].start();
+
+    rightMotorPulse.rise(&countRightMotorPulseISR);
+    leftMotorPulse.rise(&countLeftMotorPulseISR);
+
+//    rightWheelPulse1.fall(&countRightWheelPulseISR);  //パルス測定は立ち上がりor立下りのどちらかを計測するのが吉
+//    rightWheelPulse2.fall(&countRightWheelPulseISR);  //立下り特性悪すぎなので測定誤差が増える
     rightWheelPulse1.rise(&countRightWheelPulseISR);
-    rightWheelPulse2.rise(&countRightWheelPulseISR);
-    leftWheelPulse1.fall(&countLeftWheelPulseISR);
+//    rightWheelPulse2.rise(&countRightWheelPulseISR);
 
-    ticker1.attach(&loadSensorsISR, CONTROL_CYCLE_S);   //制御周期毎にデータ読み込み(LPF演算のため)
-//    ticker2.attach(&getPulseCounterISR, CONTROL_CYCLE_S);  //
-    ticker2.attach(&measRpsISR, TIRE_MEAS_CYCLE_US / 1000000.0f);  //
+//    leftWheelPulse1.fall(&countLeftWheelPulseISR);
+//    leftWheelPulse2.fall(&countLeftWheelPulseISR);
+    leftWheelPulse1.rise(&countLeftWheelPulseISR);
+//    leftWheelPulse2.rise(&countLeftWheelPulseISR);    //AB相の位相差が90度から離れすぎなので測定誤差が増える(スリットが一定間隔でないことになる)
 
-    mcp.writeA(0);   //右モーター
-    mcp.writeB(0);   //左モーター
+    ticker1.attach(&loadSensorsISR, CONTROL_CYCLE_S);               //制御周期毎にデータ読み込み(LPF演算のため)
+    ticker2.attach(&loadRpsISR, RPS_MEAS_CYCLE_S);   //RPS計測周期設定
 
     printf("MAX OUTPUT TORQUE:\t\t%1.2f[Nm]\r\n", LSB_MOTOR_TORQUE * MAX_OUTPUT_TORQUE_POWER);
     printf("MAX OUTPUT REG-TORQUE:\t\t%1.2f[Nm]\r\n", LSB_MOTOR_TORQUE * MAX_OUTPUT_TORQUE_REGENERATIVE);
     printf("MAX DISTRIBUTION TORQUE:\t%1.2f[Nm]\r\n", LSB_MOTOR_TORQUE * MAX_DISTRIBUTION_TORQUE);
     printf("MIN INNERWHEEL-MOTOR TORQUE:\t%1.2f[Nm]\r\n", LSB_MOTOR_TORQUE * MIN_INNERWHEEL_MOTOR_TORQUE);
-}
+}
\ No newline at end of file