Diff: TVDCTRL.cpp

Revision:

2:9d69f27a3d3b

Parent:

1:4d86ec2fe4b1

Child:

3:821e2f07a260

--- a/TVDCTRL.cpp	Sat Jul 09 12:04:47 2016 +0000
+++ b/TVDCTRL.cpp	Sun Jul 24 02:48:39 2016 +0000
@@ -4,130 +4,287 @@
 
 extern AnalogIn apsP;
 extern AnalogIn apsS;
+extern AnalogIn brake;
 extern DigitalOut LED[];
 extern InterruptIn rightMotorPulse;
 extern InterruptIn leftMotorPulse;
 extern DigitalOut MotorPulse[];
 extern MCP4922 mcp;
 
-static Ticker ticker;   //static -> このファイル内でのみ有効
-static Timer timer;
-
-#define apsPVol() (apsP.read() * 3.3f)
-#define apsSVol() (apsS.read() * 3.3f)
+Timer RightPulseTimer;
+Timer LeftPulseTimer;
+Ticker ticker1;
+Ticker ticker2;
+Ticker ticker3;
 
-const unsigned int APS_MIN_POSITION       =(unsigned int)(0xFFFF/3.3f * 1.0f);     //"正常時"最小入力電圧
-const unsigned int APS_MAX_POSITION       =(unsigned int)(0xFFFF/3.3f * 1.27f);    //"正常時"最大入力電圧
-const unsigned int APS_LOW_VOLTAGE        =(unsigned int)(0xFFFF/3.3f * 0.35f);    //"異常時"最低入力電圧
-const unsigned int APS_HIGH_VOLTAGE       =(unsigned int)(0xFFFF/3.3f * 3.0f);     //"異常時"最高入力電圧
-const unsigned int APS_DEADBAND           =(unsigned int)((APS_MAX_POSITION - APS_MIN_POSITION) * 0.1f);    //APS信号不感帯
-const unsigned int APS_VALID_RANGE        =(APS_MAX_POSITION - APS_MIN_POSITION) - APS_DEADBAND;    //APS信号有効範囲
-
-const unsigned int MCP_REFERENCE          =(unsigned int)(0xFFFF/3.3f * 3.3f);
-const unsigned int DACOUTPUT_MIN          =(unsigned int)(0xFFFF/3.3f * 0.49f);
-const unsigned int DACOUTPUT_MAX          =(unsigned int)(0xFFFF/3.3f * 2.45f);
-const unsigned int DACOUTPUT_VALID_RANGE  =DACOUTPUT_MAX - DACOUTPUT_MIN;
-const unsigned int DACOUTPUT_LIMIT        =(unsigned int)(0xFFFF/3.3f * 0.4f);
+#define apsPVol() (apsP.read() * 3.3)
+#define apsSVol() (apsS.read() * 3.3)
 
 struct {
     unsigned int valA:12;
     unsigned int valB:12;
-} mcpData;
-
-unsigned int g_apsP=0, g_apsS=0, g_brake=0;   //現在のセンサ値
-unsigned int rightTire=0, leftTire=0;         //出力タイヤトルク
-unsigned int currentAPS=0;
-unsigned int currentRequestTorque=0;
-unsigned int rightPulseTime=0, leftPulseTime=0;
+} McpData;
 
-struct {
-    unsigned int apsUnderVolt;      //aps電圧不足
-    unsigned int apsExceedVolt;     //aps電圧超過
-    unsigned int brakeUnderVolt;    //brake電圧不足
-    unsigned int brakeExceedVolt;   //brake電圧超過
-    unsigned int apsStick;          //aps固着
-} errCounter={0,0,0,0,0};
+//各変数が一定値を超えた時点でエラー検出とする
+//2つのAPSの区別はつけないことにする
+volatile struct errCounter_t errCounter= {0,0,0,0,0,0,0};
 
-inline float convertVoltage(unsigned int count)
+volatile int gApsP=0, gApsS=0, gBrake=0;        //現在のセンサ値
+volatile int rawApsP=0, rawApsS=0, rawBrake=0;  //現在の補正無しのセンサ値
+
+float convertVoltage(int count)
 {
     return (count*(3.3f/0xFFFF));
 }
 
-int checkSensorPlausibility(void)
+void getCurrentErrCount(struct errCounter_t *ptr)
 {
-    int plausibility = 1;
+    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;
+}
 
-    if(g_apsP < APS_MIN_POSITION) {
-        plausibility = 0;
-        LED[0] = 1;
+int loadCurrentSensor(int sensor)
+{
+    switch (sensor) {
+        case APS_PRIMARY:
+            return gApsP;
+        case APS_SECONDARY:
+            return gApsS;
+        case BRAKE:
+            return gBrake;
+        default:
+            return -1;
     }
-    if(g_apsP > APS_MAX_POSITION) {
-        plausibility = 0;
-        LED[0] = 1;
+}
+
+int loadRawSensor(int sensor)
+{
+    switch (sensor) {
+        case APS_PRIMARY:
+            return rawApsP;
+        case APS_SECONDARY:
+            return rawApsS;
+        case BRAKE:
+            return rawBrake;
+        default:
+            return -1;
     }
-    if(g_apsS < APS_MIN_POSITION) {
-        plausibility = 0;
-        LED[1] = 1;
-    }
-    if(g_apsS > APS_MAX_POSITION) {
-        plausibility = 0;
-        LED[1] = 1;
-    }
+}
+
+int myAbs(int x)
+{
+    return (x<0)?-x:x;
+}
 
-    return plausibility;
+bool loadSensorFlag = false;
+
+//タイマー割り込みでコールされる
+void loadSensorsISR(void)
+{
+    loadSensorFlag = true;
 }
 
-inline void calcRequestTorque(void)
+//関数内処理時間より短い時間のタイマーのセットは禁止
+void loadSensors(void)
 {
-    unsigned int currentAPS;
+    if(true == loadSensorFlag) {
+        loadSensorFlag = false;
+        static int preApsP=0, preApsS=0;            //過去のセンサ値
+        static int preBrake=0;
+        int tmpApsP=0, tmpApsS=0, tmpBrake=0;       //補正後のセンサ値
+        int tmpApsErrCountU=0, tmpApsErrCountE=0;   //APSの一時的なエラーカウンタ
+
+        //Low Pass Filter
+        tmpApsP = (int)(apsP.read_u16()*ratioLPF + preApsP*(1.0f-ratioLPF));
+        tmpApsS = (int)(apsS.read_u16()*ratioLPF + preApsS*(1.0f-ratioLPF));
+        tmpBrake = (int)(brake.read_u16()*ratioLPF + preBrake*(1.0f-ratioLPF));
+
+        //生のセンサ値取得
+        rawApsP = tmpApsP;
+        rawApsS = tmpApsS;
+        rawBrake = tmpBrake;
+
+        //センサーチェック
+        //APS上限値チェック
+        if(tmpApsP > APS_MAX_POSITION + ERROR_TOLERANCE) {
+            tmpApsP = APS_MAX_POSITION;         //異常時，上限値にクリップ
+            tmpApsErrCountE++;
+        }
+        if(tmpApsS > APS_MAX_POSITION + ERROR_TOLERANCE) {
+            tmpApsS = APS_MAX_POSITION;         //異常時，上限値にクリップ
+            tmpApsErrCountE++;
+        }
+        if(0 == tmpApsErrCountE)
+            errCounter.apsExceedVolt = 0;       //どちらも正常時エラーカウンタクリア
+        else
+            errCounter.apsExceedVolt += tmpApsErrCountE;
+
+        //APS下限値チェック
+        if(tmpApsP < APS_MIN_POSITION - ERROR_TOLERANCE) {
+            tmpApsP = APS_MIN_POSITION;         //下限値にクリップ
+            tmpApsErrCountU++;
+        }
+        if(tmpApsS < APS_MIN_POSITION - ERROR_TOLERANCE) {
+            tmpApsS = APS_MIN_POSITION;         //下限値にクリップ
+            tmpApsErrCountU++;
+        }
+        if(0 == tmpApsErrCountU)
+            errCounter.apsUnderVolt = 0;        //どちらも正常時エラーカウンタクリア
+        else
+            errCounter.apsUnderVolt += tmpApsErrCountU;
+
+        //センサー偏差チェック
+        if(myAbs(tmpApsP - tmpApsS) > APS_DEVIATION_TOLERANCE) {    //偏差チェックには補正後の値(tmp)を使用
+            errCounter.apsErrorTolerance++;
+        } else {
+            errCounter.apsErrorTolerance = 0;
+        }
 
-    currentAPS = ((g_apsP>g_apsS) ? g_apsS : g_apsP);   //センサ値は小さい方を採用
-    currentAPS -= APS_MIN_POSITION;     //オフセット修正
+        //小さい方にクリップ
+        //APS値は好きな方を使いな
+        if(tmpApsP > tmpApsS) {
+            tmpApsP = tmpApsS;
+        } else {
+            tmpApsS = tmpApsP;
+        }
+
+        //Brake上限値チェック
+        if(tmpBrake > BRK_OFF_VOLTAGE + ERROR_TOLERANCE) {
+            errCounter.brakeExceedVolt++;
+            tmpBrake = BRK_OFF_VOLTAGE;
+        } else {
+            errCounter.brakeExceedVolt = 0;
+        }
+
+        //Brake下限値チェック
+        if(tmpBrake < BRK_ON_VOLTAGE - ERROR_TOLERANCE) {
+            errCounter.brakeUnderVolt++;
+            tmpBrake = BRK_ON_VOLTAGE;
+        } else {
+            errCounter.brakeUnderVolt = 0;
+        }
 
-    if(currentAPS < APS_DEADBAND)   //デッドバンド内であれば要求トルク->0
-        currentRequestTorque = 0;
-    else
-        currentRequestTorque = (unsigned int)(((float)MAX_MOTOR_TORQUE / APS_VALID_RANGE) * (currentAPS - APS_DEADBAND));
+        //brake範囲外電圧チェック
+        if((tmpBrake < BRK_OFF_VOLTAGE - ERROR_TOLERANCE) && (tmpBrake > BRK_ON_VOLTAGE + ERROR_TOLERANCE)) {
+            errCounter.brakeFuzzyVolt++;
+            tmpBrake = BRK_OFF_VOLTAGE;
+        } else {
+            errCounter.brakeFuzzyVolt=0;
+        }
+
+        //APS固着チェック
+        if((preApsP == tmpApsP) && (tmpApsP == APS_MAX_POSITION))
+            errCounter.apsStick++;
+        else
+            errCounter.apsStick=0;
+
+        //ブレーキオーバーライドチェック
+        if((tmpApsP >= APS_OVERRIDE+APS_MIN_POSITION) && (tmpBrake > BRK_ON_VOLTAGE)) {
+            errCounter.brakeOverRide++;
+        } else {
+            errCounter.brakeOverRide=0;
+        }
+
+        //センサ値取得
+        gApsP = tmpApsP;
+        gApsS = tmpApsS;
+        gBrake = tmpBrake;
+
+        //未来の自分に期待
+        preApsP = rawApsP;
+        preApsS = rawApsS;
+        preBrake = rawBrake;
+    }
 }
 
-void loadSensorsLPF(void)
-{
-    static unsigned int preApsP=0, preApsS=0;
-
-    preApsP = (unsigned int)(apsP.read_u16()*ratioLPF + preApsP*(1.0f-ratioLPF));
-    preApsS = (unsigned int)(apsS.read_u16()*ratioLPF + preApsS*(1.0f-ratioLPF));
-
-    g_apsP = preApsP;
-    g_apsS = preApsS;
-}
+int gRightPulseTime=100000, gLeftPulseTime=100000;
+bool loadVelocityFlag = false;
 
-int getVelocity(void)
-{
-    if(rightPulseTime > 100)
-        rightPulseTime = 100;
-    if(leftPulseTime > 100)
-        leftPulseTime = 100;
-
-    //return (int)convPToV_533[(int)((rightPulseTime+leftPulseTime)/2.0f)];
-    return (int)convPToV_533[rightPulseTime];
-}
-
-void countPulseR(void)
+void countRightPulseISR(void)
 {
     //Do not use "printf" in interrupt!!!
     static int preTime=0;
-    int currentTime = timer.read_ms();
-    rightPulseTime = currentTime - preTime;
-    preTime = currentTime;
+    int currentTime = RightPulseTimer.read_us();
+
+    gRightPulseTime = currentTime - preTime;
+
+    if(gRightPulseTime < 1)  //最低パルス時間にクリップ
+        gRightPulseTime = 1;
+
+    if(currentTime < 1800000000) {
+        preTime = currentTime;
+    } else {    //30分経過後
+        RightPulseTimer.reset();
+        preTime = 0;
+    }
 }
 
-void countPulseL(void)
+void countLeftPulseISR(void)
 {
     //Do not use "printf" in interrupt!!!
     static int preTime=0;
-    int currentTime = timer.read_ms();
-    leftPulseTime = currentTime - preTime;
-    preTime = currentTime;
+    int currentTime = LeftPulseTimer.read_us();
+
+    gLeftPulseTime = currentTime - preTime;
+
+    if(gLeftPulseTime < 1)  //最低パルス時間にクリップ
+        gLeftPulseTime = 1;
+
+    if(currentTime < 1800000000) {
+        preTime = currentTime;
+    } else {    //30分経過後
+        LeftPulseTimer.reset();
+        preTime = 0;
+    }
+}
+
+void loadVelocityISR(void)
+{
+    loadVelocityFlag = true;
+}
+
+int getPulseTime(SelectMotor rl)
+{
+    static int preRightPulse, preLeftPulse;
+
+    if(loadVelocityFlag == true) {
+        loadVelocityFlag = false;
+
+        preRightPulse = (int)(gRightPulseTime*ratioLPF + preRightPulse*(1.0f-ratioLPF));
+        preLeftPulse = (int)(gLeftPulseTime*ratioLPF + preLeftPulse*(1.0f-ratioLPF));
+    }
+
+    if(rl == RIGHT_MOTOR)
+        return preRightPulse;
+    else
+        return preLeftPulse;
+}
+
+float getVelocity(void)
+{
+    int rightPulse=0, leftPulse=0;
+    int avePulseTime;
+
+    rightPulse = getPulseTime(RIGHT_MOTOR);
+    leftPulse = getPulseTime(LEFT_MOTOR);
+
+    if(rightPulse > 100000)
+        rightPulse = 100000;
+    if(leftPulse > 100000)
+        leftPulse = 100000;
+
+    avePulseTime = (int)((rightPulse+leftPulse)/2.0f);
+
+    if(avePulseTime < 1)    //最低パルス時間にクリップ
+        avePulseTime = 1;
+
+    return (M_PI*TIRE_DIAMETER / ((avePulseTime/1000000.0f)*TVD_GEAR_RATIO));
 }
 
 void generatePulse(void)
@@ -137,55 +294,152 @@
     MotorPulse[0] = MotorPulse[1] = LED[0] = flag;
 }
 
+int distributeTorque(float velocity, float steering)
+{
+    int disTor = 0;
+    float sqrtVelocity = velocity*velocity;
+    float Gy=0;
+
+    Gy = (sqrtVelocity*steering) / ((1.0f+STABIRITY_FACTOR*sqrtVelocity)*WHEEL_BASE);
+
+    if(Gy > 9.8f)
+        Gy = 9.8f;
+
+    if(Gy < 1.0f) {
+        disTor = 0;
+    } else if(Gy < 4.9f) {
+        disTor = ((float)MAX_DISTRIBUTION_TORQUE / (9.8f-4.9f) * Gy);
+    } else {    //0.5G以上は配分一定
+        disTor = MAX_DISTRIBUTION_TORQUE;
+    }
+
+    return disTor;
+}
+
+//トルク値線形補間関数
+int interpolateLinear(int torque, int currentMaxTorque)
+{
+    return (int)( ((double)(DACOUTPUT_MAX-LINEAR_REGION_VOLTAGE)/(currentMaxTorque-LINEAR_REGION_TORQUE)) * torque ) + LINEAR_REGION_VOLTAGE;
+}
+
+unsigned int calcTorqueToVoltage(int torque, SelectMotor rl)
+{
+    int outputVoltage=0;
+    int pulseTime=0;
+    int rpm=0;
+    int index=0;
+
+    if(torque <= LINEAR_REGION_TORQUE) {         //要求トルク<=2.5Nmの時
+        outputVoltage = (int)((double)LINEAR_REGION_VOLTAGE/LINEAR_REGION_TORQUE * torque);
+    } else {
+        pulseTime = getPulseTime(rl);
+        rpm = (int)((1.0/pulseTime) * 1000.0 * 60.0);  //pulseTime:[ms]
+
+        if(rpm < 3000) {        //3000rpm未満は回転数による出力制限がない領域
+            outputVoltage = interpolateLinear(torque, MAX_MOTOR_TORQUE);
+            //outputVoltage = (int)((DACOUTPUT_MAX-LINEAR_REGION_VOLTAGE)/(MAX_MOTOR_TORQUE-LINEAR_REGION_TORQUE) * torque) + LINEAR_REGION_VOLTAGE;
+        } else if(rpm <=11000) {
+            index = (int)((rpm - 3000)/10.0);    //マップは10rpm刻みに作成
+
+            if(calcMaxTorque[index] < torque) {   //要求トルクが現在の回転数での最大値を超えている時
+                outputVoltage = DACOUTPUT_VALID_RANGE;  //現在の回転数での最大トルクにクリップ
+            } else {
+                outputVoltage = interpolateLinear(torque, calcMaxTorque[index]);
+                //outputVoltage = (int)((DACOUTPUT_MAX-LINEAR_REGION_VOLTAGE)/(calcMaxTorque[index]-LINEAR_REGION_TORQUE) * torque) + LINEAR_REGION_VOLTAGE;
+            }
+        } else if(rpm < 12000) {
+            if(MAX_REVOLUTION_TORQUE < torque) {    //要求トルクが現在の回転数での最大値を超えている時
+                outputVoltage = DACOUTPUT_VALID_RANGE;
+            } else {
+                outputVoltage = interpolateLinear(torque, MAX_REVOLUTION_TORQUE);
+                //outputVoltage = (int)((float)(DACOUTPUT_MAX-LINEAR_REGION_VOLTAGE)/(MAX_REVOLUTION_TORQUE-LINEAR_REGION_TORQUE) * torque) + LINEAR_REGION_VOLTAGE;
+            }
+        } else {
+            outputVoltage = 0;  //回転上限のため出力停止
+        }
+    }
+
+    outputVoltage += DACOUTPUT_MIN;   //最低入力電圧でかさ上げ
+
+    return (unsigned int)(FIX_DACOUTFORM * outputVoltage);  //DACの分解能に適応(16bit->12bit)
+}
+
+int calcRequestTorque(void)
+{
+    int currentAPS;
+    int requestTorque;
+
+    currentAPS = ((gApsP>gApsS) ? gApsS : gApsP);   //センサ値は小さい方を採用
+    if(currentAPS < APS_MIN_POSITION)
+        currentAPS = 0;
+    else
+        currentAPS -= APS_MIN_POSITION;     //オフセット修正
+
+    if(currentAPS < APS_DEADBAND)   //デッドバンド内であれば要求トルク->0
+        requestTorque = 0;
+    else
+        requestTorque = (int)(((double)MAX_OUTPUT_TORQUE / APS_VALID_RANGE) * (currentAPS - APS_DEADBAND));
+
+    if(requestTorque > MAX_MOTOR_TORQUE)
+        requestTorque = MAX_MOTOR_TORQUE;
+    else if(requestTorque < 0)
+        requestTorque = 0;
+
+    return requestTorque;
+}
+
 void driveTVD(void)
 {
-    //仮に等配分のみ
-    mcpData.valA = (unsigned int)(((float)DACOUTPUT_VALID_RANGE / MAX_MOTOR_TORQUE) * currentRequestTorque) + DACOUTPUT_MIN;
-    mcpData.valB = (unsigned int)(((float)DACOUTPUT_VALID_RANGE / MAX_MOTOR_TORQUE) * currentRequestTorque) + DACOUTPUT_MIN;
+    int requestTorque=calcRequestTorque();  //ドライバーリクエストトルク
+    int distributionTor=0;  //分配トルク
+    float torqueHigh, torqueLow;
+
+    loadSensors();      //APS,BRAKE更新
+    loadSteerAngle();   //舵角更新
+    getPulseTime(RIGHT_MOTOR);  //車速更新(更新時は片方指定コールでOK)
+
+    //distributionTor = distributeTorque(getVelocity(), MAX_STEER_ANGLE / M_PI * getSteerAngle());  //トルク分配量計算
+    distributionTor = distributeTorque(0, MAX_STEER_ANGLE / M_PI * getSteerAngle());  //トルク分配量計算
+    distributionTor /= 2.0f;
+
+    if(requestTorque + distributionTor > MAX_OUTPUT_TORQUE)  //片モーター上限時最大値にクリップ
+        torqueHigh = MAX_OUTPUT_TORQUE;
+    else
+        torqueHigh = requestTorque + distributionTor;
 
-    mcp.writeA(mcpData.valA);
-    mcp.writeB(mcpData.valB);
+    if(requestTorque - distributionTor < 0) {
+        torqueLow = 0;
+        torqueHigh = (int)(requestTorque*2.0);   //片モーター下限値時，反対のモーターも出力クリップ
+    } else
+        torqueLow = requestTorque - distributionTor;
+
+    if(getSteerDirection()) {
+        //steer left
+        McpData.valA = calcTorqueToVoltage(torqueHigh, RIGHT_MOTOR);
+        McpData.valB = calcTorqueToVoltage(torqueLow, LEFT_MOTOR);
+    } else {
+        //steer right
+        McpData.valA = calcTorqueToVoltage(torqueLow, RIGHT_MOTOR);
+        McpData.valB = calcTorqueToVoltage(torqueHigh, LEFT_MOTOR);
+    }
+
+    mcp.writeA(McpData.valA);   //右モーター
+    mcp.writeB(McpData.valB);   //左モーター
 }
 
 void initTVD(void)
 {
     rightMotorPulse.mode(PullUp);
     leftMotorPulse.mode(PullUp);
-    rightMotorPulse.fall(&countPulseR);
-    leftMotorPulse.fall(&countPulseL);
-
-    timer.reset();
-    timer.start();
-
-    ticker.attach(&loadSensorsLPF, 0.001);    //サンプリング周期1msec
-    ticker.attach(&generatePulse, 0.025);
-}
-
-
-
-
-//APS信号をモーターコントローラの入力電圧に変換
-//fixVoltageを通してから使うこと
-inline float fixSpecifiedOutputData(float aps)
-{
-    float temp;
+    rightMotorPulse.fall(&countRightPulseISR);
+    leftMotorPulse.fall(&countLeftPulseISR);
 
-    aps = ((aps<APS_MIN_POSITION) ? 0.0f : (aps-APS_MIN_POSITION)); //オフセット修正
+    RightPulseTimer.reset();
+    LeftPulseTimer.reset();
+    RightPulseTimer.start();
+    LeftPulseTimer.start();
 
-    /*
-    if(aps < APS_DEADBAND)
-        temp = 0.0f;
-    else {
-        aps -= APS_DEADBAND;
-        temp = (aps * (DACOUTPUT_VALID_RANGE/(APS_VALID_RANGE)));
-        temp *= LIMIT;
-    }
-
-    temp += DACOUTPUT_MIN;
-    */
-
-    //temp = (aps * (1.1f/(APS_MAX_VOLTAGE-APS_MIN_VOLTAGE)));
-
-    return temp/3.3f;
+    ticker1.attach(&loadSensorsISR, 0.01f);    //サンプリング周期10msec
+    //ticker2.attach(&generatePulse, 0.03f);
+    ticker3.attach(&loadVelocityISR, 0.01f);
 }
-