Diff: TVDCTRL.cpp

Revision:

6:26fa8c78500e

Parent:

5:a5462959b3ab

Child:

7:ad013d88a539

--- a/TVDCTRL.cpp	Mon Jul 25 07:12:49 2016 +0000
+++ b/TVDCTRL.cpp	Tue Jul 26 05:12:15 2016 +0000
@@ -314,25 +314,41 @@
 //トルク値線形補間関数
 int interpolateLinear(int torque, int currentMaxTorque)
 {
-    return (int)( ((double)DACOUTPUT_MAX-LINEAR_REGION_VOLTAGE)/((double)currentMaxTorque-LINEAR_REGION_TORQUE) * torque ) + LINEAR_REGION_VOLTAGE;
+    return (int)((((float)DACOUTPUT_MAX-LINEAR_REGION_VOLTAGE)/((float)currentMaxTorque-LINEAR_REGION_TORQUE)) * torque) + LINEAR_REGION_VOLTAGE;
 }
 
 unsigned int calcTorqueToVoltage(int torque, SelectMotor rl)
 {
     int outputVoltage=0;
-    int pulseTime=0;
     int rpm=0;
-    int index=0;
+    int currentMaxTorque=0;
 
     if(torque <= LINEAR_REGION_TORQUE) {         //要求トルク<=2.5Nmの時
         outputVoltage = (int)((double)LINEAR_REGION_VOLTAGE/LINEAR_REGION_TORQUE * torque);
     } else {
-        pulseTime = getPulseTime(rl);
-        rpm = (int)(1.0/(pulseTime*0.0000001) * 60.0);  //pulseTime:[us]
+        rpm = (int)(1.0/getPulseTime(rl)*1000000.0 * 60.0);  //pulseTime:[us]
+
+        rpm = 3001;
 
-        if(rpm < 3000) {        //3000rpm未満は回転数による出力制限がない領域
+        if(rpm < 3000) {        //3000rpm未満は回転数による出力制限がないフラットな領域
             outputVoltage = interpolateLinear(torque, MAX_MOTOR_TORQUE);
-            //outputVoltage = (int)((DACOUTPUT_MAX-LINEAR_REGION_VOLTAGE)/(MAX_MOTOR_TORQUE-LINEAR_REGION_TORQUE) * torque) + LINEAR_REGION_VOLTAGE;
+        } else {
+            if(rpm <= 11000) {
+                int index = (int)((rpm - 3000)/10.0);    //マップは10rpm刻みに作成
+                currentMaxTorque = calcMaxTorque[index];
+            } else {
+                currentMaxTorque = MAX_REVOLUTION_TORQUE;      //回転数上限時の最大トルク
+            }
+
+            if(currentMaxTorque < torque) {     //要求トルクが現在の回転数での最大値を超えている時
+                outputVoltage = DACOUTPUT_VALID_RANGE;  //現在の回転数での最大トルクにクリップ
+            } else {
+                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刻みに作成
 
@@ -340,16 +356,15 @@
                 outputVoltage = DACOUTPUT_VALID_RANGE;  //現在の回転数での最大トルクにクリップ
             } else {
                 outputVoltage = interpolateLinear(torque, calcMaxTorque[index]);
-                //outputVoltage = (int)((DACOUTPUT_MAX-LINEAR_REGION_VOLTAGE)/(calcMaxTorque[index]-LINEAR_REGION_TORQUE) * torque) + LINEAR_REGION_VOLTAGE;
             }
         } else {
             if(MAX_REVOLUTION_TORQUE < torque) {    //要求トルクが現在の回転数での最大値を超えている時
                 outputVoltage = DACOUTPUT_VALID_RANGE;
             } else {
                 outputVoltage = interpolateLinear(torque, MAX_REVOLUTION_TORQUE);
-                //outputVoltage = (int)((float)(DACOUTPUT_MAX-LINEAR_REGION_VOLTAGE)/(MAX_REVOLUTION_TORQUE-LINEAR_REGION_TORQUE) * torque) + LINEAR_REGION_VOLTAGE;
             }
         }
+        */
     }
 
     outputVoltage += DACOUTPUT_MIN;   //最低入力電圧でかさ上げ
@@ -373,8 +388,8 @@
     else
         requestTorque = (int)(((double)MAX_OUTPUT_TORQUE / APS_VALID_RANGE) * (currentAPS - APS_DEADBAND));
 
-    if(requestTorque > MAX_MOTOR_TORQUE)
-        requestTorque = MAX_MOTOR_TORQUE;
+    if(requestTorque > MAX_OUTPUT_TORQUE)
+        requestTorque = MAX_OUTPUT_TORQUE;
     else if(requestTorque < 0)
         requestTorque = 0;
 
@@ -383,30 +398,33 @@
 
 void driveTVD(void)
 {
-    int requestTorque=calcRequestTorque();  //ドライバーリクエストトルク
-    int distributionTor=0;  //分配トルク
-    float torqueHigh, torqueLow;
+    int requestTorque=0;    //ドライバー要求トルク
+    int distributionTrq=0;  //分配トルク
+    float torqueHigh, torqueLow;    //トルクの大きい方小さい方
 
     loadSensors();      //APS,BRAKE更新
     loadSteerAngle();   //舵角更新
     //getPulseTime(RIGHT_MOTOR);  //車速更新(更新時は片方指定コールでOK)
 
-    distributionTor = distributeTorque(getVelocity(), MAX_STEER_ANGLE / M_PI * getSteerAngle());  //トルク分配量計算
-    distributionTor /= 2.0f;
+    requestTorque=calcRequestTorque();  //ドライバー要求トルク取得
 
+    distributionTrq = distributeTorque(getVelocity(), MAX_STEER_ANGLE / M_PI * getSteerAngle());  //トルク分配量計算
+    distributionTrq /= 2.0f;
+
+    //配分の制御はなしとする
     //デバッグ終わったらこの行は消すこと!!!!!
-    distributionTor=0;
+    distributionTrq=0;
 
-    if(requestTorque + distributionTor > MAX_OUTPUT_TORQUE)  //片モーター上限時最大値にクリップ
+    if(requestTorque + distributionTrq > MAX_OUTPUT_TORQUE)  //片モーター上限時最大値にクリップ
         torqueHigh = MAX_OUTPUT_TORQUE;
     else
-        torqueHigh = requestTorque + distributionTor;
+        torqueHigh = requestTorque + distributionTrq;
 
-    if(requestTorque - distributionTor < 0) {
+    if(requestTorque - distributionTrq < 0) {
         torqueLow = 0;
         torqueHigh = (int)(requestTorque*2.0);   //片モーター下限値時，反対のモーターも出力クリップ
     } else
-        torqueLow = requestTorque - distributionTor;
+        torqueLow = requestTorque - distributionTrq;
 
     if(getSteerDirection()) {
         //steer left