Diff: TVDCTRL.cpp

Revision:

20:3c5061281a7a

Parent:

19:571a4d00b89c

Child:

21:bbf2ad7e6602

--- a/TVDCTRL.cpp	Wed Aug 17 14:17:14 2016 +0000
+++ b/TVDCTRL.cpp	Sat Aug 27 22:08:24 2016 +0000
@@ -306,17 +306,17 @@
 
     if(steering < deadband)
         disTrq = 0;
-    else if(steering < M_PI*0.5){
+    else if(steering < M_PI*0.5) {
         steering -= deadband;
         disTrq = (int)(MAX_DISTRIBUTION_TORQUE / (M_PI*0.5 - deadband) * steering);
-    }
-    else
+    } else
         disTrq = MAX_DISTRIBUTION_TORQUE;
-/*    else {
-        steering -= deadband;
-        disTrq = (int)(MAX_DISTRIBUTION_TORQUE / (M_PI - deadband) * steering);
-    }
-*/    //pc.printf("%d\r\n", disTrq);
+    /*else {
+            steering -= deadband;
+            disTrq = (int)(MAX_DISTRIBUTION_TORQUE / (M_PI - deadband) * steering);
+        }
+    */
+    //pc.printf("%1.2f\r\n", 45.0/0xffff*disTrq);
 
     return disTrq;
 }
@@ -333,8 +333,6 @@
     int rpm=0;
     int currentMaxTorque=0;
 
-    static int preOutputVol=0;
-
     if(torque <= LINEAR_REGION_TORQUE) {         //要求トルク<=2.5Nmの時
         outputVoltage = (int)((double)(LINEAR_REGION_VOLTAGE-DACOUTPUT_MIN)/LINEAR_REGION_TORQUE * torque);
     } else {
@@ -362,11 +360,9 @@
 
     outputVoltage += DACOUTPUT_MIN;   //最低入力電圧でかさ上げ
 
-    preOutputVol = (int)(outputVoltage*0.2676 + preOutputVol*0.7324); //cutoff:5Hz
+    //printf("%d\r\n", (int)(0xFFF*((double)outputVoltage/0xFFFF)));
 
-    //printf("%d\r\n", (int)(0xFFF*((double)preOutputVol/0xFFFF)));
-
-    return (unsigned int)(0xFFF*((double)preOutputVol/0xFFFF));  //DACの分解能に適応(16bit->12bit)
+    return (unsigned int)(0xFFF*((double)outputVoltage/0xFFFF));  //DACの分解能に適応(16bit->12bit)
 }
 
 int calcRequestTorque(void)
@@ -403,14 +399,14 @@
 {
     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;
-    
+
     return limitRate;
 }
 
@@ -425,6 +421,8 @@
     int distributionTrq=0;  //分配トルク
     int torqueHigh, torqueLow;    //トルクの大きい方小さい方
 
+    static unsigned int preMcpA=0, preMcpB=0;
+
     loadSensors();      //APS,BRAKE更新
     loadSteerAngle();   //舵角更新
 
@@ -443,8 +441,7 @@
     }
 
     indicateSystem(readyToDriveFlag | (errCounter.brakeOverRide > ERRCOUNTER_DECISION));
-//    LED[0] = readyToDriveFlag | (errCounter.brakeOverRide > ERRCOUNTER_DECISION)
-    LED[0] = isPressedRTD();
+    LED[0] = readyToDriveFlag | (errCounter.brakeOverRide > ERRCOUNTER_DECISION);
 
     requestTorque=calcRequestTorque();  //ドライバー要求トルク取得
 
@@ -454,15 +451,16 @@
 
     //デバッグ中
     //distributionTrq = 0;
-    
+
     if(requestTorque < MIN_INNERWHEEL_MOTOR_TORQUE) {
         torqueHigh = torqueLow = requestTorque;     //内輪側モーター最低トルクより小さい要求トルクなら等配分
     } else {
-        if(requestTorque + distributionTrq > MAX_OUTPUT_TORQUE)  //片モーター上限時最大値にクリップ
+        if(requestTorque + distributionTrq > MAX_OUTPUT_TORQUE) { //片モーター上限時最大値にクリップ
             torqueHigh = MAX_OUTPUT_TORQUE;
-        else
+        } else {
             torqueHigh = requestTorque + distributionTrq;
-            
+        }
+
         if(requestTorque - distributionTrq < MIN_INNERWHEEL_MOTOR_TORQUE) {
             torqueLow = MIN_INNERWHEEL_MOTOR_TORQUE;      //内輪最低トルクにクリップ
             torqueHigh = (int)((requestTorque-MIN_INNERWHEEL_MOTOR_TORQUE)*2.0) + MIN_INNERWHEEL_MOTOR_TORQUE;   //片モーター下限値時，トルク高側のモーターも出力クリップ
@@ -471,7 +469,7 @@
     }
 
     //printf("%d %d\r\n", torqueLow, torqueHigh);
-    
+
     if(getSteerDirection()==1) {
         //steer left
         McpData.valA = calcTorqueToVoltage(torqueHigh, RIGHT_MOTOR);
@@ -482,10 +480,13 @@
         McpData.valB = calcTorqueToVoltage(torqueHigh, LEFT_MOTOR);
     }
 
-    //pc.printf("%u %u\r\n", McpData.valA, McpData.valB);
+    //pc.printf("%u %u\r\n\r\n", McpData.valA, McpData.valB);
 
-    mcp.writeA(McpData.valA);   //右モーター
-    mcp.writeB(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);   //左モーター
 }
 
 void initTVD(void)
@@ -505,7 +506,7 @@
 
     mcp.writeA(0);   //右モーター
     mcp.writeB(0);   //左モーター
-    
+
     printf("MAX OUTPUT TORQUE:\t\t%1.2f[Nm]\r\n", 45.0/0xFFFF * MAX_OUTPUT_TORQUE);
     printf("MAX DISTRIBUTION TORQUE:\t%1.2f[Nm]\r\n", 45.0/0xFFFF * MAX_DISTRIBUTION_TORQUE);
     printf("MIN INNERWHEEL-MOTOR TORQUE:\t%1.2f[Nm]\r\n", 45.0/0xFFFF * MIN_INNERWHEEL_MOTOR_TORQUE);