Diff: TVDCTRL.cpp

Revision:

44:d433bb5f77c0

Parent:

43:5da6b1574227

Child:

45:8e5d35beb957

diff -r 5da6b1574227 -r d433bb5f77c0 TVDCTRL.cpp
--- a/TVDCTRL.cpp	Thu Nov 02 01:56:46 2017 +0000
+++ b/TVDCTRL.cpp	Sat Dec 02 10:52:00 2017 +0000
@@ -111,7 +111,7 @@
     }
 }
 
-bool loadSensorFlag = false;
+volatile bool loadSensorFlag = false;
 
 //タイマー割り込みでコールされる
 void loadSensorsISR(void)
@@ -128,11 +128,16 @@
         static int preBrake=0;
         int tmpApsP=0, tmpApsS=0, tmpBrake=0;       //補正後のセンサ値
         int tmpApsErrCountU=0, tmpApsErrCountE=0;   //APSの一時的なエラーカウンタ
+        int tmpRawApsP, tmpRawApsS, tmpRawBrake;
 
+        tmpRawApsP = (int)apsP.read_u16();
+        tmpRawApsS = (int)apsS.read_u16();
+        tmpRawBrake = (int)brake.read_u16();
+        
         //Low Pass Filter
-        tmpApsP = (int)(apsP.read_u16()*ratioLPF + preApsP*(1.0f-ratioLPF));
-        tmpApsS = (int)(apsS.read_u16()*ratioLPF + preApsS*(1.0f-ratioLPF));
-        tmpBrake = (int)(brake.read_u16()*ratioLPF + preBrake*(1.0f-ratioLPF));
+        tmpApsP = (int)(tmpRawApsP * ratioLPF + preApsP * (1.0-ratioLPF));
+        tmpApsS = (int)(tmpRawApsS * ratioLPF + preApsS * (1.0-ratioLPF));
+        tmpBrake = (int)(tmpRawBrake * ratioLPF + preBrake * (1.0-ratioLPF));
 
         //生のセンサ値取得
         rawApsP = tmpApsP;
@@ -390,6 +395,26 @@
     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]
 //左右従動輪回転数の平均値から車速を演算
 float getVelocity(void)
@@ -423,34 +448,56 @@
     return disTrq;
 }
 
-int distributeTorque(float steeringWheelAngle, float velocity)
+//int distributeTorque(float steeringWheelAngle, float velocity)
+//{
+//    double V2 = velocity * velocity;
+//    double R = 0.0;     //旋回半径
+//    double Gy = 0.0;    //横G
+//    double deadband = 0.0;
+//    double steeringAngle = (double)steeringWheelAngle * STEER_RATIO;
+//    double steeringSign = 1.0;
+//    int disTrq = 0;
+//
+//    if(steeringAngle > 0)
+//        steeringSign = 1.0;
+//    else
+//        steeringSign = -1.0;
+//
+//    steeringAngle = myAbs(steeringAngle);
+//
+//    if(steeringAngle <= 0.0001)
+//        steeringAngle = 0.0001;
+//
+//    R = (1.0 + A*V2)*WHEEL_BASE / steeringAngle;   //理論旋回半径 計算
+//    Gy = V2 / R / 9.81;                                    //理論横G
+//
+//    if(Gy <= deadband)
+//        disTrq = 0;
+//    else if(Gy <= 1.5) {
+//        Gy -= deadband;
+//        disTrq = (int)(MAX_DISTRIBUTION_TORQUE / (1.5 - deadband) * Gy);
+//    } else {
+//        disTrq = MAX_DISTRIBUTION_TORQUE;
+//    }
+//
+//    return (int)(disTrq * steeringSign);
+//}
+
+int distributeTorque(float steeringWheelAngle)
 {
-    double V2 = velocity * velocity;
-    double R = 0.0;     //旋回半径
-    double Gy = 0.0;    //横G
-    double deadband = 0.0;
-    double steeringAngle = (double)steeringWheelAngle * STEER_RATIO;
+    float deadband = 0.0;
     double steeringSign = 1.0;
     int disTrq = 0;
 
-    if(steeringAngle > 0)
+    if(steeringWheelAngle > 0)
         steeringSign = 1.0;
     else
         steeringSign = -1.0;
 
-    steeringAngle = myAbs(steeringAngle);
-
-    if(steeringAngle <= 0.0001)
-        steeringAngle = 0.0001;
-
-    R = (1.0 + A*V2)*WHEEL_BASE / steeringAngle;   //理論旋回半径 計算
-    Gy = V2 / R / 9.81;                                    //理論横G
-
-    if(Gy <= deadband)
+    if(myAbs(steeringWheelAngle) <= deadband)
         disTrq = 0;
-    else if(Gy <= 1.5) {
-        Gy -= deadband;
-        disTrq = (int)(MAX_DISTRIBUTION_TORQUE / (1.5 - deadband) * Gy);
+    else if(myAbs(steeringWheelAngle) <= (0.5f*M_PI)) {
+        disTrq = (int)(MAX_DISTRIBUTION_TORQUE / (0.5f*M_PI - deadband) * (myAbs(steeringWheelAngle) - deadband));
     } else {
         disTrq = MAX_DISTRIBUTION_TORQUE;
     }
@@ -458,6 +505,7 @@
     return (int)(disTrq * steeringSign);
 }
 
+
 //rpm +++++ モータ回転数
 //regSwitch +++++ 回生=1 力行=0
 inline int calcMaxTorque(int rpm, bool regSwitch)
@@ -604,9 +652,9 @@
 
     loadSensors();      //APS,BRAKE更新
     loadSteerAngle();   //舵角更新
-    loadRps();       //従動輪・モータ回転数更新
+    loadRps();          //従動輪・モータ回転数更新
 
-    printf("%f %f %f\r\n", getWheelRps(RR_MOTOR), 1.0f, 0.0f);
+//    printf("%d\r\n", getSteerAngle());
 
     if(isRedyToDrive && isBrakeOn())
         readyToDriveFlag = 0;
@@ -627,7 +675,7 @@
 
     requestTorque=calcRequestTorque();  //ドライバー要求トルク取得
 
-    distributionTrq = (int)((distributeTorque(M_PI * getSteerAngle() / 127.0f, getVelocity())*limitTorqueDistribution()) / 2.0f);  //片モーターのトルク分配量計算
+    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;
@@ -639,40 +687,39 @@
     torqueRight += disTrq_omega;
     torqueLeft -= disTrq_omega;
 
-    if(torqueRight < 0) {
-        if((getRps(RR_MOTOR) * 60.0f) < 600.0f) {
-            torqueLeft = requestTorque + torqueRight;
-            torqueRight = 0;
-        } 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(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(torqueLeft < 0) {
-        if((getRps(RL_MOTOR) * 60.0f) < 600.0f) {
-            torqueRight = requestTorque + torqueLeft;
-            torqueLeft = 0;
-        } 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));
+        if(torqueRight > MAX_OUTPUT_TORQUE_POWER) { //片モーター上限時最大値にクリップ
+            torqueRight = MAX_OUTPUT_TORQUE_POWER;
+            if(((torqueRight + torqueLeft)/2.0f) > requestTorque) {
+                torqueLeft = requestTorque - (MAX_OUTPUT_TORQUE_POWER-requestTorque);
+            }
         }
-    }
-
-    //アクセルべた踏みでトルクMAX、旋回より駆動を優先(加速番長モード)
-    if(torqueLeft > MAX_OUTPUT_TORQUE_POWER) { //片モーター上限時最大値にクリップ
-        torqueLeft = MAX_OUTPUT_TORQUE_POWER;
-        torqueRight = requestTorque - (MAX_OUTPUT_TORQUE_POWER-requestTorque);
-    }
-    if(torqueRight > MAX_OUTPUT_TORQUE_POWER) { //片モーター上限時最大値にクリップ
-        torqueRight = MAX_OUTPUT_TORQUE_POWER;
-        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;
 
-    McpData.valA = calcTorqueToVoltage(torqueRight, getRps(RR_MOTOR));
-    McpData.valB = calcTorqueToVoltage(torqueLeft, getRps(RL_MOTOR));
+    McpData.valA = calcTorqueToVoltage(torqueRight, getRps(RR_MOTOR) * 60.0f);
+    McpData.valB = calcTorqueToVoltage(torqueLeft, getRps(RL_MOTOR) * 60.0f);
 
     preMcpA = McpData.valA;
     preMcpB = McpData.valB;