Diff: TVDCTRL.cpp

Revision:

3:821e2f07a260

Parent:

2:9d69f27a3d3b

Child:

4:d7778cde0aff

diff -r 9d69f27a3d3b -r 821e2f07a260 TVDCTRL.cpp
--- a/TVDCTRL.cpp	Sun Jul 24 02:48:39 2016 +0000
+++ b/TVDCTRL.cpp	Sun Jul 24 08:02:18 2016 +0000
@@ -211,11 +211,11 @@
     //Do not use "printf" in interrupt!!!
     static int preTime=0;
     int currentTime = RightPulseTimer.read_us();
-
+    
     gRightPulseTime = currentTime - preTime;
 
-    if(gRightPulseTime < 1)  //最低パルス時間にクリップ
-        gRightPulseTime = 1;
+    if(gRightPulseTime < 40)  //25000rpm上限より早い場合
+        gRightPulseTime = 40;
 
     if(currentTime < 1800000000) {
         preTime = currentTime;
@@ -233,8 +233,8 @@
 
     gLeftPulseTime = currentTime - preTime;
 
-    if(gLeftPulseTime < 1)  //最低パルス時間にクリップ
-        gLeftPulseTime = 1;
+    if(gLeftPulseTime < 40)  //25000rpm上限より早い場合
+        gLeftPulseTime = 40;
 
     if(currentTime < 1800000000) {
         preTime = currentTime;
@@ -256,8 +256,8 @@
     if(loadVelocityFlag == true) {
         loadVelocityFlag = false;
 
-        preRightPulse = (int)(gRightPulseTime*ratioLPF + preRightPulse*(1.0f-ratioLPF));
-        preLeftPulse = (int)(gLeftPulseTime*ratioLPF + preLeftPulse*(1.0f-ratioLPF));
+        preRightPulse = (int)(gRightPulseTime*ratioLPF_V + preRightPulse*(1.0f-ratioLPF_V));
+        preLeftPulse = (int)(gLeftPulseTime*ratioLPF_V + preLeftPulse*(1.0f-ratioLPF_V));
     }
 
     if(rl == RIGHT_MOTOR)
@@ -281,8 +281,8 @@
 
     avePulseTime = (int)((rightPulse+leftPulse)/2.0f);
 
-    if(avePulseTime < 1)    //最低パルス時間にクリップ
-        avePulseTime = 1;
+    if(avePulseTime < 40)    //最低パルス時間にクリップ
+        avePulseTime = 40;
 
     return (M_PI*TIRE_DIAMETER / ((avePulseTime/1000000.0f)*TVD_GEAR_RATIO));
 }
@@ -333,8 +333,8 @@
         outputVoltage = (int)((double)LINEAR_REGION_VOLTAGE/LINEAR_REGION_TORQUE * torque);
     } else {
         pulseTime = getPulseTime(rl);
-        rpm = (int)((1.0/pulseTime) * 1000.0 * 60.0);  //pulseTime:[ms]
-
+        rpm = (int)((1.0/pulseTime) * 1000000.0 * 60.0);  //pulseTime:[ms]
+        
         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;
@@ -398,10 +398,12 @@
     loadSteerAngle();   //舵角更新
     getPulseTime(RIGHT_MOTOR);  //車速更新(更新時は片方指定コールでOK)
 
-    //distributionTor = distributeTorque(getVelocity(), MAX_STEER_ANGLE / M_PI * getSteerAngle());  //トルク分配量計算
-    distributionTor = distributeTorque(0, MAX_STEER_ANGLE / M_PI * getSteerAngle());  //トルク分配量計算
+    distributionTor = distributeTorque(getVelocity(), MAX_STEER_ANGLE / M_PI * getSteerAngle());  //トルク分配量計算
     distributionTor /= 2.0f;
-
+    
+    //デバッグ終わったらこの行は消すこと!!!!!
+    distributionTor=0;
+    
     if(requestTorque + distributionTor > MAX_OUTPUT_TORQUE)  //片モーター上限時最大値にクリップ
         torqueHigh = MAX_OUTPUT_TORQUE;
     else