Diff: TVDCTRL.cpp

Revision:

36:dc33a3a194c9

Parent:

35:b75595b1da36

Child:

37:ba10cf09c151

--- a/TVDCTRL.cpp	Thu Aug 17 06:01:28 2017 +0000
+++ b/TVDCTRL.cpp	Thu Aug 24 03:16:56 2017 +0000
@@ -254,12 +254,12 @@
 //default argument  : switchWheel=false
 int getRPS(Select rl, bool switchWheel)
 {
-    static int rightMotorPulse[100]= {0}, leftMotorPulse[100]= {0};     //過去1秒間のパルス数格納
+    static int rightMotorPulse[25]= {0}, leftMotorPulse[25]= {0};     //過去0.5秒間のパルス数格納
     static int sumRightMotorPulse, sumLeftMotorPulse;
     float rps;
 
     if(pulseTimeISRFlag == true) {
-        for(int i=99; i>0; i--) {
+        for(int i=24; i>0; i--) {
             rightMotorPulse[i] = rightMotorPulse[i-1];
             leftMotorPulse[i] = leftMotorPulse[i-1];
         }
@@ -273,7 +273,7 @@
         sumRightMotorPulse = 0;
         sumLeftMotorPulse = 0;
 
-        for(int i=0; i<100; i++) {
+        for(int i=0; i<25; i++) {
             sumRightMotorPulse += rightMotorPulse[i];
             sumLeftMotorPulse += leftMotorPulse[i];
         }
@@ -282,9 +282,9 @@
 
     if(switchWheel == false) {
         if(rl == RIGHT)
-            rps = (float)sumRightMotorPulse / MOTOR_PULSE_NUM / GEAR_RATIO;    //過去1秒間のモータパルス数を使ってRPS算出
+            rps = (float)4.0f*sumRightMotorPulse / MOTOR_PULSE_NUM / GEAR_RATIO;    //過去0.5秒間のモータパルス数を使ってRPS算出
         else
-            rps = (float)sumLeftMotorPulse / MOTOR_PULSE_NUM / GEAR_RATIO;     //過去1秒間のモータパルス数を使ってRPS算出
+            rps = (float)4.0f*sumLeftMotorPulse / MOTOR_PULSE_NUM / GEAR_RATIO;     //過去0.5秒間のモータパルス数を使ってRPS算出
     } else {
         //こっちはタイヤ回転数
         //そのうち対応
@@ -311,7 +311,7 @@
 
     omega = steeringWheelAngle - lastSteering;    //舵角の差分算出
     omega /= 0.01f;  //制御周期で角速度演算
-    
+
     if(myAbs(omega) < 0.349f) { //20deg/s
         disTrq = 0;
     } else if(myAbs(omega) <= 8.727f) { //500deg/s
@@ -359,7 +359,7 @@
         disTrq = MAX_DISTRIBUTION_TORQUE;
     }
 
-    return disTrq * steeringSign;
+    return (int)(disTrq * steeringSign);
 }
 
 //rpm +++++ モータ回転数
@@ -488,15 +488,24 @@
     float limitRate;
     float currentVelocity = getVelocity() * 3.6f;   //km/hで車速取得
 
+    /*
     if(currentVelocity < 5.0f)
+            limitRate = 0.0f;
+        else if(currentVelocity < 15.0f)
+            limitRate = (currentVelocity - 5.0f) / (15.0f - 5.0f);
+        else
+            limitRate = 1.0f;
+    */
+    
+    if(currentVelocity < 10.0f)
         limitRate = 0.0f;
-    else if(currentVelocity < 15.0f)
-        limitRate = (currentVelocity - 5.0f) / (15.0f - 5.0f);
+    else if(currentVelocity < 30.0f)
+        limitRate = (currentVelocity - 10.0f) / (30.0f - 10.0f);
     else
         limitRate = 1.0f;
-    
+
     //printf("rate:%1.3f\r\n", limitRate);
-    
+
     return limitRate;
 }