Diff: TVDCTRL.cpp

Revision:

30:c596a0f5d685

Parent:

29:a51cb2cf22ae

Child:

31:042c08a7434f

--- a/TVDCTRL.cpp	Sat Jul 08 06:20:29 2017 +0000
+++ b/TVDCTRL.cpp	Fri Jul 21 10:20:57 2017 +0000
@@ -75,9 +75,9 @@
     int brake = gBrake;
     int brakeOnOff = 0;
 
-    if(brake > (BRK_ON_VOLTAGE - ERROR_TOLERANCE))
+    if(brake < (BRK_ON_VOLTAGE + ERROR_TOLERANCE))
         brakeOnOff = 1;
-    if(brake < (BRK_OFF_VOLTAGE + ERROR_TOLERANCE))
+    if(brake > (BRK_OFF_VOLTAGE - ERROR_TOLERANCE))
         brakeOnOff = 0;
 
     return brakeOnOff;
@@ -186,23 +186,23 @@
         }
 
         //Brake上限値チェック
-        if(tmpBrake > BRK_ON_VOLTAGE + ERROR_TOLERANCE) {
+        if(tmpBrake > BRK_OFF_VOLTAGE + ERROR_TOLERANCE) {
             errCounter.brakeExceedVolt++;
-            tmpBrake = BRK_ON_VOLTAGE;
+            tmpBrake = BRK_OFF_VOLTAGE;
         } else {
             errCounter.brakeExceedVolt = 0;
         }
 
         //Brake下限値チェック
-        if(tmpBrake < BRK_OFF_VOLTAGE - ERROR_TOLERANCE) {
+        if(tmpBrake < BRK_ON_VOLTAGE - ERROR_TOLERANCE) {
             errCounter.brakeUnderVolt++;
-            tmpBrake = BRK_OFF_VOLTAGE;
+            tmpBrake = BRK_ON_VOLTAGE;
         } else {
             errCounter.brakeUnderVolt = 0;
         }
 
         //brake範囲外電圧チェック
-        if((tmpBrake < BRK_ON_VOLTAGE - ERROR_TOLERANCE) && (tmpBrake > BRK_OFF_VOLTAGE + ERROR_TOLERANCE)) {
+        if((tmpBrake < BRK_OFF_VOLTAGE - ERROR_TOLERANCE) && (tmpBrake > BRK_ON_VOLTAGE + ERROR_TOLERANCE)) {
             errCounter.brakeFuzzyVolt++;
             tmpBrake = BRK_OFF_VOLTAGE;
         } else {
@@ -306,7 +306,7 @@
     static float lastSteering=0.0f;
     float omega=0;
     int disTrq=0;
-    
+
     steering = ratioLPF * steering + (1.0f - ratioLPF) * lastSteering;
 
     omega = lastSteering - steering;    //舵角の差分算出
@@ -506,25 +506,27 @@
             || (errCounter.apsExceedVolt > ERRCOUNTER_DECISION)
             || (errCounter.apsErrorTolerance > ERRCOUNTER_DECISION)
 //            || (errCounter.apsStick > ERRCOUNTER_DECISION)
-//            || (errCounter.brakeUnderVolt > ERRCOUNTER_DECISION)
-//            || (errCounter.brakeExceedVolt > ERRCOUNTER_DECISION)
-//            || (errCounter.brakeFuzzyVolt > ERRCOUNTER_DECISION)
+            || (errCounter.brakeUnderVolt > ERRCOUNTER_DECISION)
+            || (errCounter.brakeExceedVolt > ERRCOUNTER_DECISION)
+            || (errCounter.brakeFuzzyVolt > ERRCOUNTER_DECISION)
       ) {
         readyToDriveFlag = 1;
     }
 
     //+++++++++++++++++++
     //後で削除すること！
-    readyToDriveFlag = 0;
+    //readyToDriveFlag = 0;
     //+++++++++++++++++++
 
     indicateSystem(readyToDriveFlag | (errCounter.brakeOverRide > ERRCOUNTER_DECISION));
     LED[0] = readyToDriveFlag | (errCounter.brakeOverRide > ERRCOUNTER_DECISION);
 
     requestTorque=calcRequestTorque();  //ドライバー要求トルク取得
+    
+    distributionTrq = 0;
 
-    distributionTrq = (int)(distributeTorque(M_PI * getSteerAngle() / 127.0f) / 2.0);  //片モーターのトルク分配量計算
-    disTrq_omega = (int)(distributeTorque_omega(M_PI * getSteerAngle() / 127.0f) / 2.0);      //微分制御
+    //distributionTrq = (int)(distributeTorque(M_PI * getSteerAngle() / 127.0f) / 2.0);  //片モーターのトルク分配量計算
+    //disTrq_omega = (int)(distributeTorque_omega(M_PI * getSteerAngle() / 127.0f) / 2.0);      //微分制御
     //distributionTrq = (int)(distributionTrq * limitTorqueDistribution());     //トルク配分の最低車速制限
 
     torqueRight = requestTorque + distributionTrq;
@@ -569,8 +571,8 @@
     mcp.writeA(0);   //右モーター
     mcp.writeB(0);   //左モーター
 
-    printf("MAX OUTPUT TORQUE:\t\t%1.2f[Nm]\r\n", 45.0/0xFFFF * MAX_OUTPUT_TORQUE_POWER);
-    printf("MAX OUTPUT TORQUE:\t\t%1.2f[Nm]\r\n", 45.0/0xFFFF * MAX_OUTPUT_TORQUE_REGENERATIVE);
-    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);
+    printf("MAX OUTPUT TORQUE:\t\t%1.2f[Nm]\r\n", LSB_MOTOR_TORQUE * MAX_OUTPUT_TORQUE_POWER);
+    printf("MAX OUTPUT REG-TORQUE:\t\t%1.2f[Nm]\r\n", LSB_MOTOR_TORQUE * MAX_OUTPUT_TORQUE_REGENERATIVE);
+    printf("MAX DISTRIBUTION TORQUE:\t%1.2f[Nm]\r\n", LSB_MOTOR_TORQUE * MAX_DISTRIBUTION_TORQUE);
+    printf("MIN INNERWHEEL-MOTOR TORQUE:\t%1.2f[Nm]\r\n", LSB_MOTOR_TORQUE * MIN_INNERWHEEL_MOTOR_TORQUE);
 }