2014 sift / Mbed 2 deprecated TVDctrller2017_brdRev1_PandA

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

Fork of TVDctrller2017_brdRev1_ver6 by 2014 sift

Diff: TVDCTRL.cpp

Revision:
37:ba10cf09c151
Parent:
36:dc33a3a194c9
Child:
38:11753ee9734f
--- a/TVDCTRL.cpp	Thu Aug 24 03:16:56 2017 +0000
+++ b/TVDCTRL.cpp	Wed Aug 30 10:18:23 2017 +0000
@@ -254,12 +254,12 @@
 //default argument  : switchWheel=false
 int getRPS(Select rl, bool switchWheel)
 {
-    static int rightMotorPulse[25]= {0}, leftMotorPulse[25]= {0};     //過去0.5秒間のパルス数格納
+    static int rightMotorPulse[100]= {0}, leftMotorPulse[100]= {0};     //過去1.0秒間のパルス数格納
     static int sumRightMotorPulse, sumLeftMotorPulse;
     float rps;
 
     if(pulseTimeISRFlag == true) {
-        for(int i=24; i>0; i--) {
+        for(int i=99; 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<25; i++) {
+        for(int i=0; i<100; i++) {
             sumRightMotorPulse += rightMotorPulse[i];
             sumLeftMotorPulse += leftMotorPulse[i];
         }
@@ -282,9 +282,9 @@
 
     if(switchWheel == false) {
         if(rl == RIGHT)
-            rps = (float)4.0f*sumRightMotorPulse / MOTOR_PULSE_NUM / GEAR_RATIO;    //過去0.5秒間のモータパルス数を使ってRPS算出
+            rps = (float)1.0f*sumRightMotorPulse / MOTOR_PULSE_NUM / GEAR_RATIO;    //過去0.5秒間のモータパルス数を使ってRPS算出
         else
-            rps = (float)4.0f*sumLeftMotorPulse / MOTOR_PULSE_NUM / GEAR_RATIO;     //過去0.5秒間のモータパルス数を使ってRPS算出
+            rps = (float)1.0f*sumLeftMotorPulse / MOTOR_PULSE_NUM / GEAR_RATIO;     //過去0.5秒間のモータパルス数を使ってRPS算出
     } else {
         //こっちはタイヤ回転数
         //そのうち対応
@@ -496,11 +496,11 @@
         else
             limitRate = 1.0f;
     */
-    
-    if(currentVelocity < 10.0f)
+
+    if(currentVelocity < 15.0f)
         limitRate = 0.0f;
     else if(currentVelocity < 30.0f)
-        limitRate = (currentVelocity - 10.0f) / (30.0f - 10.0f);
+        limitRate = (currentVelocity - 15.0f) / (30.0f - 10.0f);
     else
         limitRate = 1.0f;
 
@@ -544,8 +544,8 @@
 
     requestTorque=calcRequestTorque();  //ドライバー要求トルク取得
 
-    distributionTrq = (int)((distributeTorque(M_PI * getSteerAngle() / 127.0f, getVelocity()) / 2.0) * limitTorqueDistribution());  //片モーターのトルク分配量計算
-    disTrq_omega = (int)((distributeTorque_omega(M_PI * getSteerAngle() / 127.0f) / 2.0f) * limitTorqueDistribution());      //微分制御
+    distributionTrq = (int)((distributeTorque(M_PI * getSteerAngle() / 127.0f, getVelocity())*limitTorqueDistribution()) / 2.0f);  //片モーターのトルク分配量計算
+    //disTrq_omega = (int)(distributeTorque_omega(M_PI * getSteerAngle() / 127.0f) / 2.0f);      //微分制御
 
     //printf("%d\r\n", distributionTrq);
 
@@ -574,11 +574,8 @@
     McpData.valA = calcTorqueToVoltage(torqueRight, getRPS(RIGHT));
     McpData.valB = calcTorqueToVoltage(torqueLeft, getRPS(LEFT));
 
-//    preMcpA = (unsigned int)(McpData.valA * 0.456 + preMcpA * 0.544);
-//    preMcpB = (unsigned int)(McpData.valB * 0.456 + preMcpB * 0.544);
-
-    preMcpA = McpData.valA;
-    preMcpB = McpData.valB;
+    preMcpA = (unsigned int)(McpData.valA * 0.456 + preMcpA * 0.544);
+    preMcpB = (unsigned int)(McpData.valB * 0.456 + preMcpB * 0.544);
 
     mcp.writeA(preMcpA);   //右モーター
     mcp.writeB(preMcpB);   //左モーター