Diff: TVDCTRL.cpp

Revision:

9:220e4e77e056

Parent:

8:a22aec357a64

Child:

10:87ad65eef0e9

--- a/TVDCTRL.cpp	Wed Jul 27 04:45:07 2016 +0000
+++ b/TVDCTRL.cpp	Wed Jul 27 05:42:33 2016 +0000
@@ -287,24 +287,26 @@
 
 int distributeTorque(float velocity, float steering)
 {
-    int disTor = 0;
+    int disTrq = 0;
     float sqrtVelocity = velocity*velocity;
     float Gy=0;
 
     Gy = (sqrtVelocity*steering) / ((1.0f+STABIRITY_FACTOR*sqrtVelocity)*WHEEL_BASE);
+    
+    printf("%f\r\n", Gy);
 
     if(Gy > 9.8f)
         Gy = 9.8f;
 
-    if(Gy < 1.96f) {
-        disTor = 0;
+    if(Gy < 0.98f) {
+        disTrq = 0;
     } else if(Gy < 4.9f) {
-        disTor = ((float)MAX_DISTRIBUTION_TORQUE / (9.8f-4.9f) * Gy);
+        disTrq = ((float)MAX_DISTRIBUTION_TORQUE / (4.9f-0.98f) * (Gy-0.98f));
     } else {    //0.5G以上は配分一定
-        disTor = MAX_DISTRIBUTION_TORQUE;
+        disTrq = MAX_DISTRIBUTION_TORQUE;
     }
 
-    return disTor;
+    return disTrq;
 }
 
 //トルク値線形補間関数
@@ -318,13 +320,13 @@
     int outputVoltage=0;
     int rpm=0;
     int currentMaxTorque=0;
-    
+
     if(torque <= LINEAR_REGION_TORQUE) {         //要求トルク<=2.5Nmの時
         outputVoltage = (int)((double)(LINEAR_REGION_VOLTAGE-DACOUTPUT_MIN)/LINEAR_REGION_TORQUE * torque);
     } else {
         rpm = (int)(1.0/getPulseTime(rl)*1000000.0 * 60.0);  //pulseTime:[us]
 
-        rpm = 12000;
+        //rpm = 12000;
 
         if(rpm < 3000) {        //3000rpm未満は回転数による出力制限がないフラットな領域
             outputVoltage = interpolateLinear(torque, MAX_MOTOR_TORQUE);
@@ -342,32 +344,11 @@
                 outputVoltage = interpolateLinear(torque, currentMaxTorque);
             }
         }
-        /*
-        if(rpm < 3000) {        //3000rpm未満は回転数による出力制限がないフラットな領域
-            outputVoltage = interpolateLinear(torque, MAX_MOTOR_TORQUE);
-        } else if(rpm <=11000) {
-            index = (int)((rpm - 3000)/10.0);    //マップは10rpm刻みに作成
-
-            if(calcMaxTorque[index] < torque) {   //要求トルクが現在の回転数での最大値を超えている時
-                outputVoltage = DACOUTPUT_VALID_RANGE;  //現在の回転数での最大トルクにクリップ
-            } else {
-                outputVoltage = interpolateLinear(torque, calcMaxTorque[index]);
-            }
-        } else {
-            if(MAX_REVOLUTION_TORQUE < torque) {    //要求トルクが現在の回転数での最大値を超えている時
-                outputVoltage = DACOUTPUT_VALID_RANGE;
-            } else {
-                outputVoltage = interpolateLinear(torque, MAX_REVOLUTION_TORQUE);
-            }
-        }
-        */
     }
 
     outputVoltage += DACOUTPUT_MIN;   //最低入力電圧でかさ上げ
-    
-    printf("%d\r\n", (unsigned int)(0xFFF*((double)outputVoltage/DACOUTPUT_MAX)));
-    
-    return (unsigned int)((double)0xFFF/DACOUTPUT_MAX * outputVoltage);  //DACの分解能に適応(16bit->12bit)
+
+    return (unsigned int)(0xFFF*((double)outputVoltage/0xFFFF));  //DACの分解能に適応(16bit->12bit)
 }
 
 int calcRequestTorque(void)
@@ -406,15 +387,9 @@
 
     requestTorque=calcRequestTorque();  //ドライバー要求トルク取得
 
-    //distributionTrq = distributeTorque(getVelocity(), MAX_STEER_ANGLE / M_PI * getSteerAngle());  //トルク分配量計算
-    distributionTrq /= 2.0f;
+    distributionTrq = distributeTorque(getVelocity(), MAX_STEER_ANGLE / M_PI * getSteerAngle());  //トルク分配量計算
+    distributionTrq /= 2.0;
 
-    //配分の制御はなしとする
-    //デバッグ終わったらこの行は消すこと!!!!!
-    distributionTrq=0;
-    torqueHigh = torqueLow = requestTorque;
-
-    /*
     if(requestTorque + distributionTrq > MAX_OUTPUT_TORQUE)  //片モーター上限時最大値にクリップ
         torqueHigh = MAX_OUTPUT_TORQUE;
     else
@@ -425,16 +400,15 @@
         torqueHigh = (int)(requestTorque*2.0);   //片モーター下限値時，反対のモーターも出力クリップ
     } else
         torqueLow = requestTorque - distributionTrq;
-    */
 
     if(getSteerDirection()) {
         //steer left
-        McpData.valA = calcTorqueToVoltage(requestTorque, RIGHT_MOTOR);
-        McpData.valB = calcTorqueToVoltage(requestTorque, LEFT_MOTOR);
+        McpData.valA = calcTorqueToVoltage(torqueHigh, RIGHT_MOTOR);
+        McpData.valB = calcTorqueToVoltage(torqueLow, LEFT_MOTOR);
     } else {
         //steer right
-        McpData.valA = calcTorqueToVoltage(requestTorque, RIGHT_MOTOR);
-        McpData.valB = calcTorqueToVoltage(requestTorque, LEFT_MOTOR);
+        McpData.valA = calcTorqueToVoltage(torqueLow, RIGHT_MOTOR);
+        McpData.valB = calcTorqueToVoltage(torqueHigh, LEFT_MOTOR);
     }
 
     mcp.writeA(McpData.valA);   //右モーター