Diff: TVDCTRL.cpp

Revision:

18:b7c362c8f0fd

Parent:

17:a2246ce3333f

Child:

19:571a4d00b89c

diff -r a2246ce3333f -r b7c362c8f0fd TVDCTRL.cpp
--- a/TVDCTRL.cpp	Tue Aug 09 20:59:59 2016 +0000
+++ b/TVDCTRL.cpp	Wed Aug 10 11:21:47 2016 +0000
@@ -18,6 +18,8 @@
 Ticker ticker2;
 Ticker ticker3;
 
+#define myAbs(x)    ((x>0)?(x):(-(x)))
+
 #define apsPVol() (apsP.read() * 3.3)
 #define apsSVol() (apsS.read() * 3.3)
 
@@ -35,6 +37,8 @@
 int gApsP=0, gApsS=0, gBrake=0;        //現在のセンサ値
 int rawApsP=0, rawApsS=0, rawBrake=0;  //現在の補正無しのセンサ値
 
+//エラーカウンタ外部参照用関数
+//errCounter_t型変数のポインタを引数に取る
 void getCurrentErrCount(struct errCounter_t *ptr)
 {
     ptr->apsUnderVolt       = errCounter.apsUnderVolt;
@@ -47,6 +51,7 @@
     ptr->brakeOverRide      = errCounter.brakeOverRide;
 }
 
+//ブレーキONOFF判定関数
 //Brake-ON:1 Brake-OFF:0
 int isBrakeOn(void)
 {
@@ -61,6 +66,7 @@
     return brakeOnOff;
 }
 
+//センサ現在値外部参照関数
 int getCurrentSensor(int sensor)
 {
     switch (sensor) {
@@ -75,6 +81,7 @@
     }
 }
 
+//補正前センサ現在値外部参照関数
 int getRawSensor(int sensor)
 {
     switch (sensor) {
@@ -89,11 +96,6 @@
     }
 }
 
-int myAbs(int x)
-{
-    return (x<0)?-x:x;
-}
-
 bool loadSensorFlag = false;
 
 //タイマー割り込みでコールされる
@@ -225,8 +227,8 @@
 
     gRightPulseTime = currentTime - preTime;
 
-    if(gRightPulseTime < MAX_PULSE_TIME)  //12000rpm上限より早い場合
-        gRightPulseTime = MAX_PULSE_TIME;
+    if(gRightPulseTime < MIN_PULSE_TIME)  //12000rpm上限より早い場合
+        gRightPulseTime = MIN_PULSE_TIME;
 
     if(currentTime < 1800000000) {
         preTime = currentTime;
@@ -244,8 +246,8 @@
 
     gLeftPulseTime = currentTime - preTime;
 
-    if(gLeftPulseTime < MAX_PULSE_TIME)  //12000rpm上限より早い場合
-        gLeftPulseTime = MAX_PULSE_TIME;
+    if(gLeftPulseTime < MIN_PULSE_TIME)  //12000rpm上限より早い場合
+        gLeftPulseTime = MIN_PULSE_TIME;
 
     if(currentTime < 1800000000) {
         preTime = currentTime;
@@ -267,10 +269,10 @@
     if(pulseTimeISRFlag == true) {
         pulseTimeISRFlag = false;
 
-        if(gRightPulseTime > 150000)    //最長パルス制限(about:4.02km/h)
-            gRightPulseTime = 150000;
-        if(gLeftPulseTime > 150000)
-            gLeftPulseTime = 150000;
+        if(gRightPulseTime > MAX_PULSE_TIME)    //最大パルス時間にクリップ
+            gRightPulseTime = MAX_PULSE_TIME;
+        if(gLeftPulseTime > MAX_PULSE_TIME)
+            gLeftPulseTime = MAX_PULSE_TIME;
 
         preRightPulse = (int)(gRightPulseTime*ratioLPF_V + preRightPulse*(1.0f-ratioLPF_V));
         preLeftPulse = (int)(gLeftPulseTime*ratioLPF_V + preLeftPulse*(1.0f-ratioLPF_V));
@@ -292,10 +294,10 @@
 
     avePulseTime = (int)((rightPulse+leftPulse)/2.0);
 
-    if(avePulseTime < MAX_PULSE_TIME)    //最低パルス時間にクリップ
-        avePulseTime = MAX_PULSE_TIME;
+    if(avePulseTime < MIN_PULSE_TIME)    //最低パルス時間にクリップ
+        avePulseTime = MIN_PULSE_TIME;
 
-    return (M_PI*TIRE_DIAMETER / ((avePulseTime/1000000.0)*TVD_GEAR_RATIO));
+    return (float)(M_PI*TIRE_DIAMETER / ((avePulseTime/1000000.0)*TVD_GEAR_RATIO));
 }
 
 void generatePulse(void)
@@ -362,8 +364,7 @@
 
     outputVoltage += DACOUTPUT_MIN;   //最低入力電圧でかさ上げ
 
-    //preOutputVol = (int)(outputVoltage*0.1 + preOutputVol*0.9);
-    preOutputVol = outputVoltage;
+    preOutputVol = (int)(outputVoltage*0.1 + preOutputVol*0.9);
 
     //printf("%d\r\n", (int)(0xFFF*((double)preOutputVol/0xFFFF)));
 
@@ -407,8 +408,8 @@
     
     if(currentVelocity < 5.0f)
         limitRate = 0.0f;
-    else if(currentVelocity < 20.0f)
-        limitRate = (currentVelocity - 5.0f) / (20.0f - 5.0f);
+    else if(currentVelocity < 15.0f)
+        limitRate = (currentVelocity - 5.0f) / (15.0f - 5.0f);
     else
         limitRate = 1.0f;
     
@@ -449,13 +450,13 @@
 
     distributionTrq = (int)(distributeTorque(getSteerAngle()) / 2.0);  //片モーターのトルク分配量計算
 
-    distributionTrq = (int)(distributionTrq * limitTorqueDistribution());   //トルク配分の最低車速制限
+    //distributionTrq = (int)(distributionTrq * limitTorqueDistribution());   //トルク配分の最低車速制限
 
     //デバッグ中
     //distributionTrq = 0;
     
     if(requestTorque < MIN_INNERWHEEL_TORQUE) {
-        torqueHigh = torqueLow = requestTorque;     //内輪最低トルクより小さい要求トルクなら等配分
+        torqueHigh = torqueLow = requestTorque;     //内輪側モーター最低トルクより小さい要求トルクなら等配分
     } else {
         if(requestTorque + distributionTrq > MAX_OUTPUT_TORQUE)  //片モーター上限時最大値にクリップ
             torqueHigh = MAX_OUTPUT_TORQUE;
@@ -464,7 +465,7 @@
             
         if(requestTorque - distributionTrq < MIN_INNERWHEEL_TORQUE) {
             torqueLow = MIN_INNERWHEEL_TORQUE;      //内輪最低トルクにクリップ
-            torqueHigh = (int)((requestTorque-MIN_INNERWHEEL_TORQUE)*2.0) + MIN_INNERWHEEL_TORQUE;   //片モーター下限値時，反対のモーターも出力クリップ
+            torqueHigh = (int)((requestTorque-MIN_INNERWHEEL_TORQUE)*2.0) + MIN_INNERWHEEL_TORQUE;   //片モーター下限値時，トルク高側のモーターも出力クリップ
         } else
             torqueLow = requestTorque - distributionTrq;
     }
@@ -481,7 +482,7 @@
         McpData.valB = calcTorqueToVoltage(torqueHigh, LEFT_MOTOR);
     }
 
-    pc.printf("%u %u\r\n", McpData.valA, McpData.valB);
+    //pc.printf("%u %u\r\n", McpData.valA, McpData.valB);
 
     mcp.writeA(McpData.valA);   //右モーター
     mcp.writeB(McpData.valB);   //左モーター
@@ -505,5 +506,8 @@
 
     mcp.writeA(0);   //右モーター
     mcp.writeB(0);   //左モーター
-
+    
+    printf("MAX OUTPUT TORQUE:\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_TORQUE);
 }