Diff: TVDCTRL.cpp

Revision:

5:a5462959b3ab

Parent:

4:d7778cde0aff

Child:

6:26fa8c78500e

--- a/TVDCTRL.cpp	Sun Jul 24 08:23:56 2016 +0000
+++ b/TVDCTRL.cpp	Mon Jul 25 07:12:49 2016 +0000
@@ -203,8 +203,8 @@
     }
 }
 
-int gRightPulseTime=100000, gLeftPulseTime=100000;
-bool loadVelocityFlag = false;
+volatile int gRightPulseTime=100000, gLeftPulseTime=100000;
+volatile bool loadVelocityFlag = false;
 
 void countRightPulseISR(void)
 {
@@ -214,8 +214,8 @@
 
     gRightPulseTime = currentTime - preTime;
 
-    if(gRightPulseTime < 40)  //25000rpm上限より早い場合
-        gRightPulseTime = 40;
+    if(gRightPulseTime < MAX_PULSE_TIME)  //12000rpm上限より早い場合
+        gRightPulseTime = MAX_PULSE_TIME;
 
     if(currentTime < 1800000000) {
         preTime = currentTime;
@@ -233,8 +233,8 @@
 
     gLeftPulseTime = currentTime - preTime;
 
-    if(gLeftPulseTime < 40)  //25000rpm上限より早い場合
-        gLeftPulseTime = 40;
+    if(gLeftPulseTime < MAX_PULSE_TIME)  //12000rpm上限より早い場合
+        gLeftPulseTime = MAX_PULSE_TIME;
 
     if(currentTime < 1800000000) {
         preTime = currentTime;
@@ -274,17 +274,12 @@
     rightPulse = getPulseTime(RIGHT_MOTOR);
     leftPulse = getPulseTime(LEFT_MOTOR);
 
-    if(rightPulse > 100000)
-        rightPulse = 100000;
-    if(leftPulse > 100000)
-        leftPulse = 100000;
+    avePulseTime = (int)((rightPulse+leftPulse)/2.0);
 
-    avePulseTime = (int)((rightPulse+leftPulse)/2.0f);
+    if(avePulseTime < MAX_PULSE_TIME)    //最低パルス時間にクリップ
+        avePulseTime = MAX_PULSE_TIME;
 
-    if(avePulseTime < 40)    //最低パルス時間にクリップ
-        avePulseTime = 40;
-
-    return (M_PI*TIRE_DIAMETER / ((avePulseTime/1000000.0f)*TVD_GEAR_RATIO));
+    return (M_PI*TIRE_DIAMETER / ((avePulseTime/1000000.0)*TVD_GEAR_RATIO));
 }
 
 void generatePulse(void)
@@ -305,7 +300,7 @@
     if(Gy > 9.8f)
         Gy = 9.8f;
 
-    if(Gy < 1.0f) {
+    if(Gy < 1.96f) {
         disTor = 0;
     } else if(Gy < 4.9f) {
         disTor = ((float)MAX_DISTRIBUTION_TORQUE / (9.8f-4.9f) * Gy);
@@ -319,7 +314,7 @@
 //トルク値線形補間関数
 int interpolateLinear(int torque, int currentMaxTorque)
 {
-    return (int)( ((double)(DACOUTPUT_MAX-LINEAR_REGION_VOLTAGE)/(currentMaxTorque-LINEAR_REGION_TORQUE)) * torque ) + LINEAR_REGION_VOLTAGE;
+    return (int)( ((double)DACOUTPUT_MAX-LINEAR_REGION_VOLTAGE)/((double)currentMaxTorque-LINEAR_REGION_TORQUE) * torque ) + LINEAR_REGION_VOLTAGE;
 }
 
 unsigned int calcTorqueToVoltage(int torque, SelectMotor rl)
@@ -333,7 +328,7 @@
         outputVoltage = (int)((double)LINEAR_REGION_VOLTAGE/LINEAR_REGION_TORQUE * torque);
     } else {
         pulseTime = getPulseTime(rl);
-        rpm = (int)((1.0/pulseTime) * 1000000.0 * 60.0);  //pulseTime:[ms]
+        rpm = (int)(1.0/(pulseTime*0.0000001) * 60.0);  //pulseTime:[us]
 
         if(rpm < 3000) {        //3000rpm未満は回転数による出力制限がない領域
             outputVoltage = interpolateLinear(torque, MAX_MOTOR_TORQUE);
@@ -347,15 +342,13 @@
                 outputVoltage = interpolateLinear(torque, calcMaxTorque[index]);
                 //outputVoltage = (int)((DACOUTPUT_MAX-LINEAR_REGION_VOLTAGE)/(calcMaxTorque[index]-LINEAR_REGION_TORQUE) * torque) + LINEAR_REGION_VOLTAGE;
             }
-        } else if(rpm < 12000) {
+        } 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;
             }
-        } else {
-            //outputVoltage = 0;  //回転上限のため出力停止
         }
     }
 
@@ -396,7 +389,7 @@
 
     loadSensors();      //APS,BRAKE更新
     loadSteerAngle();   //舵角更新
-    getPulseTime(RIGHT_MOTOR);  //車速更新(更新時は片方指定コールでOK)
+    //getPulseTime(RIGHT_MOTOR);  //車速更新(更新時は片方指定コールでOK)
 
     distributionTor = distributeTorque(getVelocity(), MAX_STEER_ANGLE / M_PI * getSteerAngle());  //トルク分配量計算
     distributionTor /= 2.0f;