Diff: TVDCTRL.cpp

Revision:

31:042c08a7434f

Parent:

30:c596a0f5d685

Child:

32:9688c30ac38b

--- a/TVDCTRL.cpp	Fri Jul 21 10:20:57 2017 +0000
+++ b/TVDCTRL.cpp	Thu Jul 27 12:34:06 2017 +0000
@@ -280,11 +280,11 @@
         pulseTimeISRFlag = false;
     }
 
-    if(switchWheel == 0) {
+    if(switchWheel == false) {
         if(rl == RIGHT)
-            rps = (float)sumRightMotorPulse / MOTOR_PULSE_NUM;    //過去1秒間のモータパルス数を使ってRPS算出
+            rps = (float)sumRightMotorPulse / MOTOR_PULSE_NUM / GEAR_RATIO;    //過去1秒間のモータパルス数を使ってRPS算出
         else
-            rps = (float)sumLeftMotorPulse / MOTOR_PULSE_NUM;     //過去1秒間のモータパルス数を使ってRPS算出
+            rps = (float)sumLeftMotorPulse / MOTOR_PULSE_NUM / GEAR_RATIO;     //過去1秒間のモータパルス数を使ってRPS算出
     } else {
         //こっちはタイヤ回転数
         //そのうち対応
@@ -298,7 +298,7 @@
 
 float getVelocity(void)
 {
-    return TIRE_DIAMETER*M_PI*(getRPS(RIGHT) + getRPS(LEFT))*LSB_MOTORSPEED/2.0f;
+    return (0.5f*TIRE_DIAMETER*2*M_PI*(getRPS(RIGHT) + getRPS(LEFT))/2.0f)*LSB_MOTORSPEED;
 }
 
 int distributeTorque_omega(float steering)
@@ -327,24 +327,34 @@
     return disTrq;
 }
 
-int distributeTorque(float steering)
+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 = steeringWheelAngle / 7.0;
+    double steeringSign = 1.0;
     int disTrq = 0;
-    const float deadband = (M_PI/180.0f)*5.0f;
-    int steeringSign;
 
-    if(steering > 0)
+    if(steeringAngle > 0)
         steeringSign = 1;
     else
         steeringSign = -1;
 
-    steering = myAbs(steering);
+    steeringAngle = myAbs(steeringAngle);
+    
+    if(steeringAngle <= 0.0)
+        steeringAngle = 0.0001;
 
-    if(steering < deadband)
+    R = (1.0 + A*V2)*WHEEL_BASE / steeringAngle;   //理論旋回半径 計算
+    Gy = V2 / R;                                    //理論横G
+
+    if(Gy < deadband)
         disTrq = 0;
-    else if(steering < M_PI*0.5) {
-        steering -= deadband;
-        disTrq = (int)(MAX_DISTRIBUTION_TORQUE / (M_PI*0.5 - deadband) * steering);
+    else if(Gy < 0.5) {
+        Gy -= deadband;
+        disTrq = (int)(MAX_DISTRIBUTION_TORQUE / (0.5 - deadband) * Gy);
     } else {
         disTrq = MAX_DISTRIBUTION_TORQUE;
     }
@@ -522,13 +532,13 @@
     LED[0] = readyToDriveFlag | (errCounter.brakeOverRide > ERRCOUNTER_DECISION);
 
     requestTorque=calcRequestTorque();  //ドライバー要求トルク取得
-    
-    distributionTrq = 0;
 
-    //distributionTrq = (int)(distributeTorque(M_PI * getSteerAngle() / 127.0f) / 2.0);  //片モーターのトルク分配量計算
+    distributionTrq = (int)(distributeTorque(M_PI * getSteerAngle() / 127.0f, getVelocity()) / 2.0);  //片モーターのトルク分配量計算
     //disTrq_omega = (int)(distributeTorque_omega(M_PI * getSteerAngle() / 127.0f) / 2.0);      //微分制御
     //distributionTrq = (int)(distributionTrq * limitTorqueDistribution());     //トルク配分の最低車速制限
 
+    distributionTrq = 0;
+
     torqueRight = requestTorque + distributionTrq;
     torqueLeft = requestTorque - distributionTrq;