Diff: TVDCTRL.cpp

Revision:

22:95c1f753ecad

Parent:

21:bbf2ad7e6602

Child:

23:ee53d2658801

--- a/TVDCTRL.cpp	Fri Sep 02 22:41:09 2016 +0000
+++ b/TVDCTRL.cpp	Fri Nov 11 04:50:56 2016 +0000
@@ -302,24 +302,25 @@
 int distributeTorque_omega(float steering)
 {
     static float lastSteering=0.0f;
-    float omega;
-    int disTrq;
+    float omega=0;
+    int disTrq=0;
 
     omega = lastSteering - steering;    //舵角の差分算出
-    
+
     omega /= 0.01f;  //制御周期で角速度演算
 
-    omega = myAbs(omega);
-    
-    if(omega < 0.349f) { //20deg/s
+    if(myAbs(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 if(myAbs(omega) <= 8.727f) { //500deg/s
+        disTrq = (int)((0xFFFF/45.0f * 5.0f) / (8.727f-0.349f) * (myAbs(omega) - 0.349f));
     } else
-        disTrq = (int)(0xFFFF/45.0f * 20.0f);
-    
+        disTrq = (int)(0xFFFF/45.0f * 5.0f);
+
     lastSteering = steering;
 
+    if(omega >= 0)
+        disTrq = -disTrq;
+
     return disTrq;
 }
 
@@ -445,8 +446,6 @@
     int distributionTrq=0;  //分配トルク
     int disTrq_omega=0;
     int torqueRight, torqueLeft;    //トルクの右左
-    float currentSteering = getSteerAngle();
-    static float lastSteering = 0;
 
     static unsigned int preMcpA=0, preMcpB=0;
 
@@ -474,9 +473,11 @@
     //デバッグ中!!!
     //requestTorque = (int)(MAX_OUTPUT_TORQUE/2.0f);
 
-    distributionTrq = (int)(distributeTorque(getSteerAngle()) / 2.0);  //片モーターのトルク分配量計算
+    distributionTrq = (int)(distributeTorque(myAbs(getSteerAngle())) / 2.0);  //片モーターのトルク分配量計算
     disTrq_omega = (int)(distributeTorque_omega(getSteerAngle()) / 2.0);
     
+    //printf("%d\r\n", disTrq_omega);
+
     //distributionTrq = (int)(distributionTrq * limitTorqueDistribution());   //トルク配分の最低車速制限
 
     //デバッグ中
@@ -485,30 +486,13 @@
     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 {
-            /*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*/
-        }
-    }
+    torqueRight += disTrq_omega;
+    torqueLeft -= disTrq_omega;
 
     if(requestTorque < MIN_INNERWHEEL_MOTOR_TORQUE) {
         torqueRight = torqueLeft = requestTorque;     //内輪側モーター最低トルクより小さい要求トルクなら等配分
@@ -536,8 +520,6 @@
         }
     }
 
-    lastSteering = currentSteering;
-
     //printf("%d %d\r\n", torqueLow, torqueHigh);
 
     McpData.valA = calcTorqueToVoltage(torqueRight, RIGHT_MOTOR);