Diff: TVDCTRL.cpp

Revision:

16:7afd623ef48a

Parent:

15:3255cbe4ff34

Child:

17:a2246ce3333f

--- a/TVDCTRL.cpp	Tue Aug 09 13:17:43 2016 +0000
+++ b/TVDCTRL.cpp	Tue Aug 09 15:08:30 2016 +0000
@@ -339,7 +339,7 @@
         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 = 0;
 
         if(rpm < 3000) {        //3000rpm未満は回転数による出力制限がないフラットな領域
@@ -364,7 +364,7 @@
 
     //preOutputVol = (int)(outputVoltage*0.1 + preOutputVol*0.9);
     preOutputVol = outputVoltage;
-    
+
     //printf("%d\r\n", (int)(0xFFF*((double)preOutputVol/0xFFFF)));
 
     return (unsigned int)(0xFFF*((double)preOutputVol/0xFFFF));  //DACの分解能に適応(16bit->12bit)
@@ -423,32 +423,33 @@
             || (errCounter.brakeExceedVolt > ERRCOUNTER_DECISION)
             || (errCounter.brakeFuzzyVolt > ERRCOUNTER_DECISION)
       ) {
-          readyToDriveFlag = 1;
+        readyToDriveFlag = 1;
     }
-    
+
     indicateSystem(readyToDriveFlag | (errCounter.brakeOverRide > ERRCOUNTER_DECISION));
-    
+
     requestTorque=calcRequestTorque();  //ドライバー要求トルク取得
 
-    distributionTrq = distributeTorque(getSteerAngle());  //トルク分配量計算
-    distributionTrq /= 2.0;
-    
-    //printf("%d\r\n", distributionTrq);
+    distributionTrq = (int)(distributeTorque(getSteerAngle()) / 2.0);  //トルク分配量計算
 
     //デバッグ中
     //distributionTrq = 0;
 
-    if(requestTorque + distributionTrq > MAX_OUTPUT_TORQUE)  //片モーター上限時最大値にクリップ
-        torqueHigh = MAX_OUTPUT_TORQUE;
-    else
-        torqueHigh = requestTorque + distributionTrq;
+    if(requestTorque < MIN_INNERWHEEL_TORQUE) {
+        torqueHigh = torqueLow = requestTorque;     //内輪最低トルクより小さい要求トルクなら等配分
+    } else {
+        if(requestTorque + distributionTrq > MAX_OUTPUT_TORQUE)  //片モーター上限時最大値にクリップ
+            torqueHigh = MAX_OUTPUT_TORQUE;
+        else
+            torqueHigh = requestTorque + distributionTrq;
+            
+        if(requestTorque - distributionTrq < MIN_INNERWHEEL_TORQUE) {
+            torqueLow = MIN_INNERWHEEL_TORQUE;      //内輪最低トルクにクリップ
+            torqueHigh = (int)((requestTorque-MIN_INNERWHEEL_TORQUE)*2.0) + MIN_INNERWHEEL_TORQUE;   //片モーター下限値時，反対のモーターも出力クリップ
+        } else
+            torqueLow = requestTorque - distributionTrq;
+    }
 
-    if(requestTorque - distributionTrq < 0) {
-        torqueLow = 0;
-        torqueHigh = (int)(requestTorque*2.0);   //片モーター下限値時，反対のモーターも出力クリップ
-    } else
-        torqueLow = requestTorque - distributionTrq;
-    
     //printf("%d %d\r\n", torqueLow, torqueHigh);
 
     if(getSteerDirection()==1) {
@@ -460,7 +461,7 @@
         McpData.valA = calcTorqueToVoltage(torqueLow, RIGHT_MOTOR);
         McpData.valB = calcTorqueToVoltage(torqueHigh, LEFT_MOTOR);
     }
-    
+
     pc.printf("%u %u\r\n", McpData.valA, McpData.valB);
 
     mcp.writeA(McpData.valA);   //右モーター