Diff: TVDCTRL.cpp

Revision:

21:bbf2ad7e6602

Parent:

20:3c5061281a7a

Child:

22:95c1f753ecad

diff -r 3c5061281a7a -r bbf2ad7e6602 TVDCTRL.cpp
--- a/TVDCTRL.cpp	Sat Aug 27 22:08:24 2016 +0000
+++ b/TVDCTRL.cpp	Fri Sep 02 22:41:09 2016 +0000
@@ -299,6 +299,30 @@
     return (float)(M_PI*TIRE_DIAMETER / ((avePulseTime/1000000.0)*TVD_GEAR_RATIO));
 }
 
+int distributeTorque_omega(float steering)
+{
+    static float lastSteering=0.0f;
+    float omega;
+    int disTrq;
+
+    omega = lastSteering - steering;    //舵角の差分算出
+    
+    omega /= 0.01f;  //制御周期で角速度演算
+
+    omega = myAbs(omega);
+    
+    if(omega < 0.349f) { //20deg/s
+        disTrq = 0;
+    } else if(omega <= 8.727f) { //500deg/s
+        disTrq = (int)((0xFFFF/45.0f * 20.0f) / (8.727f-0.349f) * (omega - 0.349f));
+    } else
+        disTrq = (int)(0xFFFF/45.0f * 20.0f);
+    
+    lastSteering = steering;
+
+    return disTrq;
+}
+
 int distributeTorque(float steering)
 {
     int disTrq = 0;
@@ -419,7 +443,10 @@
 {
     int requestTorque=0;    //ドライバー要求トルク
     int distributionTrq=0;  //分配トルク
-    int torqueHigh, torqueLow;    //トルクの大きい方小さい方
+    int disTrq_omega=0;
+    int torqueRight, torqueLeft;    //トルクの右左
+    float currentSteering = getSteerAngle();
+    static float lastSteering = 0;
 
     static unsigned int preMcpA=0, preMcpB=0;
 
@@ -444,43 +471,79 @@
     LED[0] = readyToDriveFlag | (errCounter.brakeOverRide > ERRCOUNTER_DECISION);
 
     requestTorque=calcRequestTorque();  //ドライバー要求トルク取得
+    //デバッグ中!!!
+    //requestTorque = (int)(MAX_OUTPUT_TORQUE/2.0f);
 
     distributionTrq = (int)(distributeTorque(getSteerAngle()) / 2.0);  //片モーターのトルク分配量計算
-
+    disTrq_omega = (int)(distributeTorque_omega(getSteerAngle()) / 2.0);
+    
     //distributionTrq = (int)(distributionTrq * limitTorqueDistribution());   //トルク配分の最低車速制限
 
     //デバッグ中
     //distributionTrq = 0;
 
-    if(requestTorque < MIN_INNERWHEEL_MOTOR_TORQUE) {
-        torqueHigh = torqueLow = requestTorque;     //内輪側モーター最低トルクより小さい要求トルクなら等配分
-    } else {
-        if(requestTorque + distributionTrq > MAX_OUTPUT_TORQUE) { //片モーター上限時最大値にクリップ
-            torqueHigh = MAX_OUTPUT_TORQUE;
+    if(getSteerDirection()) { //steer left
+        torqueRight = requestTorque + distributionTrq;
+        torqueLeft = requestTorque - distributionTrq;
+
+        if(currentSteering - lastSteering > 0) {
+            torqueRight += disTrq_omega;
+            torqueLeft -= disTrq_omega;
+        } else if(currentSteering - lastSteering < 0) {
+            torqueRight -= disTrq_omega;
+            torqueLeft += disTrq_omega;
         } else {
-            torqueHigh = requestTorque + distributionTrq;
+            /*No operation*/
+        }
+    } else {    //steer right
+        torqueRight = requestTorque - distributionTrq;
+        torqueLeft = requestTorque + distributionTrq;
+
+        if(currentSteering - lastSteering > 0) {
+            torqueLeft += disTrq_omega;
+            torqueRight -= disTrq_omega;
+        } else if(currentSteering - lastSteering < 0) {
+            torqueLeft -= disTrq_omega;
+            torqueRight += disTrq_omega;
+        } else {
+            /*No operation*/
         }
+    }
 
-        if(requestTorque - distributionTrq < MIN_INNERWHEEL_MOTOR_TORQUE) {
-            torqueLow = MIN_INNERWHEEL_MOTOR_TORQUE;      //内輪最低トルクにクリップ
-            torqueHigh = (int)((requestTorque-MIN_INNERWHEEL_MOTOR_TORQUE)*2.0) + MIN_INNERWHEEL_MOTOR_TORQUE;   //片モーター下限値時，トルク高側のモーターも出力クリップ
-        } else
-            torqueLow = requestTorque - distributionTrq;
+    if(requestTorque < MIN_INNERWHEEL_MOTOR_TORQUE) {
+        torqueRight = torqueLeft = requestTorque;     //内輪側モーター最低トルクより小さい要求トルクなら等配分
+    } else {
+        if(torqueLeft > MAX_OUTPUT_TORQUE) { //片モーター上限時最大値にクリップ
+            torqueLeft = MAX_OUTPUT_TORQUE;
+
+            if(((torqueRight + torqueLeft)/2.0) > requestTorque) {
+                torqueRight = requestTorque - (MAX_OUTPUT_TORQUE-requestTorque);
+            }
+        }
+        if(torqueRight > MAX_OUTPUT_TORQUE) { //片モーター上限時最大値にクリップ
+            torqueRight = MAX_OUTPUT_TORQUE;
+            if(((torqueRight + torqueLeft)/2.0) > requestTorque) {
+                torqueLeft = requestTorque - (MAX_OUTPUT_TORQUE-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;   //片モーター下限値時，トルク高側のモーターも出力クリップ
+        }
     }
 
+    lastSteering = currentSteering;
+
     //printf("%d %d\r\n", torqueLow, torqueHigh);
 
-    if(getSteerDirection()==1) {
-        //steer left
-        McpData.valA = calcTorqueToVoltage(torqueHigh, RIGHT_MOTOR);
-        McpData.valB = calcTorqueToVoltage(torqueLow, LEFT_MOTOR);
-    } else {
-        //steer right
-        McpData.valA = calcTorqueToVoltage(torqueLow, RIGHT_MOTOR);
-        McpData.valB = calcTorqueToVoltage(torqueHigh, LEFT_MOTOR);
-    }
+    McpData.valA = calcTorqueToVoltage(torqueRight, RIGHT_MOTOR);
+    McpData.valB = calcTorqueToVoltage(torqueLeft, LEFT_MOTOR);
 
-    //pc.printf("%u %u\r\n\r\n", McpData.valA, McpData.valB);
+    //pc.printf("%u %u\r\n", McpData.valA, McpData.valB);
 
     preMcpA = (unsigned int)(McpData.valA * 0.456 + preMcpA * 0.544);
     preMcpB = (unsigned int)(McpData.valB * 0.456 + preMcpB * 0.544);