Diff: TVDCTRL.cpp

Revision:

12:ae291fa7239c

Parent:

11:88701a5e7eae

Child:

13:6dc51981f391

--- a/TVDCTRL.cpp	Sat Aug 06 05:56:18 2016 +0000
+++ b/TVDCTRL.cpp	Sun Aug 07 15:23:01 2016 +0000
@@ -28,21 +28,37 @@
 
 //各変数が一定値を超えた時点でエラー検出とする
 //2つのAPSの区別はつけないことにする
-volatile struct errCounter_t errCounter= {0,0,0,0,0,0,0};
+struct errCounter_t errCounter= {0,0,0,0,0,0,0};
 
-volatile int gApsP=0, gApsS=0, gBrake=0;        //現在のセンサ値
-volatile int rawApsP=0, rawApsS=0, rawBrake=0;  //現在の補正無しのセンサ値
+int readyToDriveFlag = 1;
+
+int gApsP=0, gApsS=0, gBrake=0;        //現在のセンサ値
+int rawApsP=0, rawApsS=0, rawBrake=0;  //現在の補正無しのセンサ値
 
 void getCurrentErrCount(struct errCounter_t *ptr)
 {
-    ptr->apsUnderVolt = errCounter.apsUnderVolt;
-    ptr->apsExceedVolt = errCounter.apsExceedVolt;
-    ptr->apsErrorTolerance = errCounter.apsErrorTolerance;
-    ptr->apsStick = errCounter.apsStick;
-    ptr->brakeUnderVolt = errCounter.brakeUnderVolt;
-    ptr->brakeExceedVolt = errCounter.brakeExceedVolt;
-    ptr->brakeFuzzyVolt = errCounter.brakeFuzzyVolt;
-    ptr->brakeOverRide = errCounter.brakeOverRide;
+    ptr->apsUnderVolt       = errCounter.apsUnderVolt;
+    ptr->apsExceedVolt      = errCounter.apsExceedVolt;
+    ptr->apsErrorTolerance  = errCounter.apsErrorTolerance;
+    ptr->apsStick           = errCounter.apsStick;
+    ptr->brakeUnderVolt     = errCounter.brakeUnderVolt;
+    ptr->brakeExceedVolt    = errCounter.brakeExceedVolt;
+    ptr->brakeFuzzyVolt     = errCounter.brakeFuzzyVolt;
+    ptr->brakeOverRide      = errCounter.brakeOverRide;
+}
+
+//Brake-ON:1 Brake-OFF:0
+int isBrakeOn(void)
+{
+    int brake = gBrake;
+    int brakeOnOff = 0;
+
+    if(brake > (BRK_ON_VOLTAGE - ERROR_TOLERANCE))
+        brakeOnOff = 1;
+    if(brake < (BRK_OFF_VOLTAGE + ERROR_TOLERANCE))
+        brakeOnOff = 0;
+
+    return brakeOnOff;
 }
 
 int getCurrentSensor(int sensor)
@@ -151,23 +167,23 @@
         }
 
         //Brake上限値チェック
-        if(tmpBrake > BRK_OFF_VOLTAGE + ERROR_TOLERANCE) {
+        if(tmpBrake > BRK_ON_VOLTAGE + ERROR_TOLERANCE) {
             errCounter.brakeExceedVolt++;
-            tmpBrake = BRK_OFF_VOLTAGE;
+            tmpBrake = BRK_ON_VOLTAGE;
         } else {
             errCounter.brakeExceedVolt = 0;
         }
 
         //Brake下限値チェック
-        if(tmpBrake < BRK_ON_VOLTAGE - ERROR_TOLERANCE) {
+        if(tmpBrake < BRK_OFF_VOLTAGE - ERROR_TOLERANCE) {
             errCounter.brakeUnderVolt++;
-            tmpBrake = BRK_ON_VOLTAGE;
+            tmpBrake = BRK_OFF_VOLTAGE;
         } else {
             errCounter.brakeUnderVolt = 0;
         }
 
         //brake範囲外電圧チェック
-        if((tmpBrake < BRK_OFF_VOLTAGE - ERROR_TOLERANCE) && (tmpBrake > BRK_ON_VOLTAGE + ERROR_TOLERANCE)) {
+        if((tmpBrake < BRK_ON_VOLTAGE - ERROR_TOLERANCE) && (tmpBrake > BRK_OFF_VOLTAGE + ERROR_TOLERANCE)) {
             errCounter.brakeFuzzyVolt++;
             tmpBrake = BRK_OFF_VOLTAGE;
         } else {
@@ -181,11 +197,10 @@
             errCounter.apsStick=0;
 
         //ブレーキオーバーライドチェック
-        if((tmpApsP >= APS_OVERRIDE+APS_MIN_POSITION) && (tmpBrake > BRK_ON_VOLTAGE)) {
+        if((isBrakeOn() == 1) && (tmpApsP >= APS_OVERRIDE25))   //Brake-ON and APS > 25%
             errCounter.brakeOverRide++;
-        } else {
+        if(tmpApsP < APS_OVERRIDE05)   //Brake-ON and APS < 5%
             errCounter.brakeOverRide=0;
-        }
 
         //センサ値取得
         gApsP = tmpApsP;
@@ -290,23 +305,16 @@
     MotorPulse[0] = MotorPulse[1] = LED[0] = flag;
 }
 
-int distributeTorque(float velocity, float steering)
+int distributeTorque(float steering)
 {
     int disTrq = 0;
-    double sqrtVelocity = (double)velocity*velocity;
-    double Gy=0;
-
-    Gy = (sqrtVelocity*steering) / ((1.0+STABIRITY_FACTOR*sqrtVelocity)*WHEEL_BASE);
+    const double deadband = (M_PI/180.0)*5.0;
 
-    if(Gy > 9.8)
-        Gy = 9.8;
-
-    if(Gy < 0.98) {
+    if(steering < deadband)
         disTrq = 0;
-    } else if(Gy < 4.9) {
-        disTrq = (int)(MAX_DISTRIBUTION_TORQUE / (4.9-0.98) * (Gy-0.98));
-    } else {    //0.5G以上は配分一定
-        disTrq = MAX_DISTRIBUTION_TORQUE;
+    else {
+        steering -= deadband;
+        disTrq = (MAX_DISTRIBUTION_TORQUE / (M_PI - deadband)) * steering;
     }
 
     return disTrq;
@@ -323,16 +331,14 @@
     int outputVoltage=0;
     int rpm=0;
     int currentMaxTorque=0;
-    
-    int preOutputVol=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 {
         rpm = (int)(1.0/getPulseTime(rl)*1000000.0 * 60.0);  //pulseTime:[us]
 
-        //rpm = 12000;
-
         if(rpm < 3000) {        //3000rpm未満は回転数による出力制限がないフラットな領域
             outputVoltage = interpolateLinear(torque, MAX_MOTOR_TORQUE);
         } else {
@@ -352,8 +358,8 @@
     }
 
     outputVoltage += DACOUTPUT_MIN;   //最低入力電圧でかさ上げ
-    
-    preOutputVol = (int)(outputVoltage*0.05 + preOutputVol*0.95);
+
+    preOutputVol = (int)(outputVoltage*0.003 + preOutputVol*0.997);
 
     return (unsigned int)(0xFFF*((double)preOutputVol/0xFFFF));  //DACの分解能に適応(16bit->12bit)
 }
@@ -364,6 +370,7 @@
     int requestTorque;
 
     currentAPS = ((gApsP>gApsS) ? gApsS : gApsP);   //センサ値は小さい方を採用
+
     if(currentAPS < APS_MIN_POSITION)
         currentAPS = 0;
     else
@@ -379,25 +386,47 @@
     else if(requestTorque < 0)
         requestTorque = 0;
 
+    if((errCounter.brakeOverRide > ERRCOUNTER_DECISION) || (readyToDriveFlag == 1))
+        requestTorque = 0;
+
     return requestTorque;
 }
 
+extern DigitalIn RTDSW;
+#define isPressedRTD(void)      (!RTDSW.read())
+extern DigitalOut indicatorLed;
+#define indicateSystem(x)       (indicatorLed.write(x))
+
 void driveTVD(void)
 {
     int requestTorque=0;    //ドライバー要求トルク
     int distributionTrq=0;  //分配トルク
     int torqueHigh, torqueLow;    //トルクの大きい方小さい方
-    
+
     loadSensors();      //APS,BRAKE更新
     loadSteerAngle();   //舵角更新
-    //getPulseTime(RIGHT_MOTOR);  //車速更新(更新時は片方指定コールでOK)
+
+    if(isPressedRTD() && isBrakeOn())
+        readyToDriveFlag = 0;
 
+    if((errCounter.apsUnderVolt > ERRCOUNTER_DECISION)
+            || (errCounter.apsExceedVolt > ERRCOUNTER_DECISION)
+            || (errCounter.apsErrorTolerance > ERRCOUNTER_DECISION)
+//            || (errCounter.apsStick > ERRCOUNTER_DECISION)
+            || (errCounter.brakeUnderVolt > ERRCOUNTER_DECISION)
+            || (errCounter.brakeExceedVolt > ERRCOUNTER_DECISION)
+            || (errCounter.brakeFuzzyVolt > ERRCOUNTER_DECISION)
+      ) {
+          readyToDriveFlag = 1;
+    }
+    
+    indicateSystem(readyToDriveFlag | (errCounter.brakeOverRide > ERRCOUNTER_DECISION));
+    
     requestTorque=calcRequestTorque();  //ドライバー要求トルク取得
 
-    distributionTrq = distributeTorque(getVelocity(), MAX_STEER_ANGLE / M_PI * getSteerAngle());  //トルク分配量計算
-    //distributionTrq = distributeTorque(50.0f, MAX_STEER_ANGLE / M_PI * getSteerAngle());  //トルク分配量計算
+    distributionTrq = distributeTorque(getSteerAngle());  //トルク分配量計算
     distributionTrq /= 2.0;
-    
+
     //デバッグ中
     distributionTrq = 0;