Diff: TVDCTRL.cpp

Revision:

34:594ddb4008b2

Parent:

33:8bebba17a749

Child:

35:b75595b1da36

--- a/TVDCTRL.cpp	Thu Jul 27 15:37:59 2017 +0000
+++ b/TVDCTRL.cpp	Fri Aug 04 02:58:51 2017 +0000
@@ -301,27 +301,27 @@
     return (0.5f*TIRE_DIAMETER*2*M_PI*(getRPS(RIGHT) + getRPS(LEFT))*0.5f)*LSB_MOTORSPEED;
 }
 
-int distributeTorque_omega(float steering)
+int distributeTorque_omega(float steeringWheelAngle)
 {
     static float lastSteering=0.0f;
     float omega=0;
     int disTrq=0;
 
-    steering = ratioLPF * steering + (1.0f - ratioLPF) * lastSteering;
+    steeringWheelAngle = ratioLPF * steeringWheelAngle + (1.0f - ratioLPF) * lastSteering;
 
-    omega = lastSteering - steering;    //舵角の差分算出
+    omega = steeringWheelAngle - lastSteering;    //舵角の差分算出
     omega /= 0.01f;  //制御周期で角速度演算
-
+    
     if(myAbs(omega) < 0.349f) { //20deg/s
         disTrq = 0;
     } else if(myAbs(omega) <= 8.727f) { //500deg/s
-        disTrq = (int)((0xFFFF/45.0f * 5.0f) / (8.727f-0.349f) * (myAbs(omega) - 0.349f));
+        disTrq = (int)(MAX_DISTRIBUTION_TORQUE_OMEGA / (8.727f-0.349f) * (myAbs(omega) - 0.349f));
     } else
-        disTrq = (int)(0xFFFF/45.0f * 5.0f);
+        disTrq = (int)MAX_DISTRIBUTION_TORQUE_OMEGA;
 
-    lastSteering = steering;
+    lastSteering = steeringWheelAngle;
 
-    if(omega >= 0)
+    if(omega < 0)
         disTrq = -disTrq;
 
     return disTrq;
@@ -338,9 +338,9 @@
     int disTrq = 0;
 
     if(steeringAngle > 0)
-        steeringSign = 1;
+        steeringSign = 1.0;
     else
-        steeringSign = -1;
+        steeringSign = -1.0;
 
     steeringAngle = myAbs(steeringAngle);
 
@@ -350,11 +350,11 @@
     R = (1.0 + A*V2)*WHEEL_BASE / steeringAngle;   //理論旋回半径 計算
     Gy = V2 / R / 9.81;                                    //理論横G
 
-    if(Gy < deadband)
+    if(Gy <= deadband)
         disTrq = 0;
-    else if(Gy < 0.5) {
+    else if(Gy <= 1.5) {
         Gy -= deadband;
-        disTrq = (int)(MAX_DISTRIBUTION_TORQUE / (0.5 - deadband) * Gy);
+        disTrq = (int)(MAX_DISTRIBUTION_TORQUE / (1.5 - deadband) * Gy);
     } else {
         disTrq = MAX_DISTRIBUTION_TORQUE;
     }
@@ -488,13 +488,15 @@
     float limitRate;
     float currentVelocity = getVelocity() * 3.6f;   //km/hで車速取得
 
-    if(currentVelocity < 5.0f)
+    if(currentVelocity < 1.0f)
         limitRate = 0.0f;
-    else if(currentVelocity < 15.0f)
-        limitRate = (currentVelocity - 5.0f) / (15.0f - 5.0f);
+    else if(currentVelocity < 5.0f)
+        limitRate = (currentVelocity - 1.0f) / (5.0f - 1.0f);
     else
         limitRate = 1.0f;
-
+    
+    printf("rate:%1.3f\r\n", limitRate);
+    
     return limitRate;
 }
 
@@ -533,11 +535,12 @@
 
     requestTorque=calcRequestTorque();  //ドライバー要求トルク取得
 
-    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 = (int)((distributeTorque(M_PI * getSteerAngle() / 127.0f, getVelocity()) / 2.0) * limitTorqueDistribution());  //片モーターのトルク分配量計算
+    disTrq_omega = (int)((distributeTorque_omega(M_PI * getSteerAngle() / 127.0f) / 2.0f) * limitTorqueDistribution());      //微分制御
 
-    distributionTrq = 0;
+    //printf("%d\r\n", distributionTrq);
+
+    //distributionTrq = 0;
 
     torqueRight = requestTorque + distributionTrq;
     torqueLeft = requestTorque - distributionTrq;
@@ -554,7 +557,10 @@
         torqueRight = MAX_OUTPUT_TORQUE_POWER;
         torqueLeft = requestTorque - (MAX_OUTPUT_TORQUE_POWER-requestTorque);
     }
-    
+
+    gRightMotorTorque = torqueRight;
+    gLeftMotorTorque = torqueLeft;
+
     McpData.valA = calcTorqueToVoltage(torqueRight, getRPS(RIGHT));
     McpData.valB = calcTorqueToVoltage(torqueLeft, getRPS(LEFT));