File content as of revision 23:ee53d2658801:

#include "TVDCTRL.h"
#include "MCP4922.h"
#include "Steering.h"

extern AnalogIn apsP;
extern AnalogIn apsS;
extern AnalogIn brake;
extern DigitalOut LED[];
extern InterruptIn rightMotorPulse;
extern InterruptIn leftMotorPulse;
extern MCP4922 mcp;
extern Serial pc;
extern AnalogOut STR2AN;

Timer RightPulseTimer;
Timer LeftPulseTimer;
Ticker ticker1;
Ticker ticker2;

#define myAbs(x)    ((x>0)?(x):(-(x)))

#define apsPVol() (apsP.read() * 3.3)
#define apsSVol() (apsS.read() * 3.3)

struct {
    unsigned int valA:12;
    unsigned int valB:12;
} McpData;

//各変数が一定値を超えた時点でエラー検出とする
//2つのAPSの区別はつけないことにする
struct errCounter_t errCounter= {0,0,0,0,0,0,0};

int readyToDriveFlag = 1;

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;
    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;
}

//ブレーキONOFF判定関数
//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)
{
    switch (sensor) {
        case APS_PRIMARY:
            return gApsP;
        case APS_SECONDARY:
            return gApsS;
        case BRAKE:
            return gBrake;
        default:
            return -1;
    }
}

//補正前センサ現在値外部参照関数
int getRawSensor(int sensor)
{
    switch (sensor) {
        case APS_PRIMARY:
            return rawApsP;
        case APS_SECONDARY:
            return rawApsS;
        case BRAKE:
            return rawBrake;
        default:
            return -1;
    }
}

bool loadSensorFlag = false;

//タイマー割り込みでコールされる
void loadSensorsISR(void)
{
    loadSensorFlag = true;
}

//関数内処理時間より短い時間のタイマーのセットは禁止
void loadSensors(void)
{
    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;
        }

        //小さい方にクリップ
        //APS値は好きな方を使いな
        if(tmpApsP > tmpApsS) {
            tmpApsP = tmpApsS;
        } else {
            tmpApsS = tmpApsP;
        }

        //Brake上限値チェック
        if(tmpBrake > BRK_ON_VOLTAGE + ERROR_TOLERANCE) {
            errCounter.brakeExceedVolt++;
            tmpBrake = BRK_ON_VOLTAGE;
        } else {
            errCounter.brakeExceedVolt = 0;
        }

        //Brake下限値チェック
        if(tmpBrake < BRK_OFF_VOLTAGE - ERROR_TOLERANCE) {
            errCounter.brakeUnderVolt++;
            tmpBrake = BRK_OFF_VOLTAGE;
        } else {
            errCounter.brakeUnderVolt = 0;
        }

        //brake範囲外電圧チェック
        if((tmpBrake < BRK_ON_VOLTAGE - ERROR_TOLERANCE) && (tmpBrake > BRK_OFF_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((isBrakeOn() == 1) && (tmpApsP >= APS_OVERRIDE25))   //Brake-ON and APS > 25%
            errCounter.brakeOverRide++;
        if(tmpApsP < APS_OVERRIDE05)   //Brake-ON and APS < 5%
            errCounter.brakeOverRide=0;

        //センサ値取得
        gApsP = tmpApsP;
        gApsS = tmpApsS;
        gBrake = tmpBrake;

        //未来の自分に期待
        preApsP = rawApsP;
        preApsS = rawApsS;
        preBrake = rawBrake;
    }
}

volatile int gRightPulseTime=100000, gLeftPulseTime=100000;
volatile bool pulseTimeISRFlag = false;

void countRightPulseISR(void)
{
    //Do not use "printf" in interrupt!!!
    static int preTime=0;
    int currentTime = RightPulseTimer.read_us();

    gRightPulseTime = currentTime - preTime;

    if(gRightPulseTime < MIN_PULSE_TIME)  //12000rpm上限より早い場合
        gRightPulseTime = MIN_PULSE_TIME;

    if(currentTime < 1800000000) {
        preTime = currentTime;
    } else {    //30分経過後
        RightPulseTimer.reset();
        preTime = 0;
    }
}

void countLeftPulseISR(void)
{
    //Do not use "printf" in interrupt!!!
    static int preTime=0;
    int currentTime = LeftPulseTimer.read_us();

    gLeftPulseTime = currentTime - preTime;

    if(gLeftPulseTime < MIN_PULSE_TIME)  //12000rpm上限より早い場合
        gLeftPulseTime = MIN_PULSE_TIME;

    if(currentTime < 1800000000) {
        preTime = currentTime;
    } else {    //30分経過後
        LeftPulseTimer.reset();
        preTime = 0;
    }
}

void getPulseTimeISR(void)
{
    pulseTimeISRFlag = true;
}

int getPulseTime(SelectMotor rl)
{
    static int preRightPulse, preLeftPulse;

    if(pulseTimeISRFlag == true) {
        pulseTimeISRFlag = false;

        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));
    }

    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);

    avePulseTime = (int)((rightPulse+leftPulse)/2.0);

    if(avePulseTime < MIN_PULSE_TIME)    //最低パルス時間にクリップ
        avePulseTime = MIN_PULSE_TIME;

    return (float)(M_PI*TIRE_DIAMETER / ((avePulseTime/1000000.0)*TVD_GEAR_RATIO));
}

int distributeTorque_omega(float steering)
{
    static float lastSteering=0.0f;
    float omega=0;
    int disTrq=0;

    omega = lastSteering - steering;    //舵角の差分算出

    omega /= 0.01f;  //制御周期で角速度演算

    if(myAbs(omega) < 0.349f) { //20deg/s
        disTrq = 0;
    } else if(myAbs(omega) <= 8.727f) { //500deg/s
        disTrq = (int)((0xFFFF/45.0f * 10.0f) / (8.727f-0.349f) * (myAbs(omega) - 0.349f));
    } else
        disTrq = (int)(0xFFFF/45.0f * 10.0f);

    lastSteering = steering;

    if(omega >= 0)
        disTrq = -disTrq;

    return disTrq;
}

int distributeTorque(float steering)
{
    int disTrq = 0;
    const float deadband = (M_PI/180.0f)*5.0f;

    if(steering < deadband)
        disTrq = 0;
    else if(steering < M_PI*0.5) {
        steering -= deadband;
        disTrq = (int)(MAX_DISTRIBUTION_TORQUE / (M_PI*0.5 - deadband) * steering);
    } else
        disTrq = MAX_DISTRIBUTION_TORQUE;
    /*else {
            steering -= deadband;
            disTrq = (int)(MAX_DISTRIBUTION_TORQUE / (M_PI - deadband) * steering);
        }
    */
    //pc.printf("%1.2f\r\n", 45.0/0xffff*disTrq);

    return disTrq;
}

//トルク値線形補間関数
inline int interpolateLinear(int torque, int currentMaxTorque)
{
    return (int)(((double)(DACOUTPUT_MAX-LINEAR_REGION_VOLTAGE)/(currentMaxTorque-LINEAR_REGION_TORQUE)) * (torque-LINEAR_REGION_TORQUE)) + LINEAR_REGION_VOLTAGE-DACOUTPUT_MIN;
}

unsigned int calcTorqueToVoltage(int torque, SelectMotor rl)
{
    int outputVoltage=0;
    int rpm=0;
    int currentMaxTorque=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 = 0;

        if(rpm < 3000) {        //3000rpm未満は回転数による出力制限がないフラットな領域
            outputVoltage = interpolateLinear(torque, MAX_MOTOR_TORQUE);
        } else {
            if(rpm <= 11000) {
                int index = (int)((rpm - 3000)/10.0);    //マップは10rpm刻みに作成
                currentMaxTorque = calcMaxTorque[index];
            } else {
                currentMaxTorque = MAX_REVOLUTION_TORQUE;      //回転数上限時の最大トルク
            }

            if(currentMaxTorque < torque) {     //要求トルクが現在の回転数での最大値を超えている時
                outputVoltage = DACOUTPUT_VALID_RANGE;  //現在の回転数での最大トルクにクリップ
            } else {
                outputVoltage = interpolateLinear(torque, currentMaxTorque);
            }
        }
    }

    outputVoltage += DACOUTPUT_MIN;   //最低入力電圧でかさ上げ

    //printf("%d\r\n", (int)(0xFFF*((double)outputVoltage/0xFFFF)));

    return (unsigned int)(0xFFF*((double)outputVoltage/0xFFFF));  //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_OUTPUT_TORQUE)
        requestTorque = MAX_OUTPUT_TORQUE;
    else if(requestTorque < 0)
        requestTorque = 0;

    if((errCounter.brakeOverRide > ERRCOUNTER_DECISION) || (readyToDriveFlag == 1))
        requestTorque = 0;

    return requestTorque;
}

//トルク配分車速制限関数
//車速が低速域の場合，トルク配分0
float limitTorqueDistribution(void)
{
    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;
}

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 disTrq_omega=0;
    int torqueRight, torqueLeft;    //トルクの右左

    static unsigned int preMcpA=0, preMcpB=0;

    loadSensors();      //APS,BRAKE更新
    loadSteerAngle();   //舵角更新

    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));
    LED[0] = readyToDriveFlag | (errCounter.brakeOverRide > ERRCOUNTER_DECISION);

    requestTorque=calcRequestTorque();  //ドライバー要求トルク取得
    //デバッグ中!!!
    //requestTorque = (int)(MAX_OUTPUT_TORQUE/2.0f);

    distributionTrq = (int)(distributeTorque(myAbs(getSteerAngle())) / 2.0);  //片モーターのトルク分配量計算
    disTrq_omega = (int)(distributeTorque_omega(getSteerAngle()) / 2.0);
    
    //printf("%d\r\n", disTrq_omega);

    //distributionTrq = (int)(distributionTrq * limitTorqueDistribution());   //トルク配分の最低車速制限

    //デバッグ中
    //distributionTrq = 0;

    if(getSteerDirection()) { //steer left
        torqueRight = requestTorque + distributionTrq;
        torqueLeft = requestTorque - distributionTrq;
    } else {    //steer right
        torqueRight = requestTorque - distributionTrq;
        torqueLeft = requestTorque + distributionTrq;
    }

    torqueRight += disTrq_omega;
    torqueLeft -= disTrq_omega;

    if(requestTorque < MIN_INNERWHEEL_MOTOR_TORQUE) {
        torqueRight = torqueLeft = requestTorque;     //内輪側モーター最低トルクより小さい要求トルクなら等配分
    } else {
        if(torqueLeft > MAX_OUTPUT_TORQUE) { //片モーター上限時最大値にクリップ
            torqueLeft = MAX_OUTPUT_TORQUE;

            if(((torqueRight + torqueLeft)/2.0) > requestTorque) {
                torqueRight = requestTorque - (MAX_OUTPUT_TORQUE-requestTorque);
            }
        }
        if(torqueRight > MAX_OUTPUT_TORQUE) { //片モーター上限時最大値にクリップ
            torqueRight = MAX_OUTPUT_TORQUE;
            if(((torqueRight + torqueLeft)/2.0) > requestTorque) {
                torqueLeft = requestTorque - (MAX_OUTPUT_TORQUE-requestTorque);
            }
        }
        if(torqueLeft < MIN_INNERWHEEL_MOTOR_TORQUE) {  //内輪最低トルク時
            torqueLeft = MIN_INNERWHEEL_MOTOR_TORQUE;      //内輪最低トルクにクリップ
            torqueRight = (int)((requestTorque-MIN_INNERWHEEL_MOTOR_TORQUE)*2.0) + MIN_INNERWHEEL_MOTOR_TORQUE;   //片モーター下限値時，トルク高側のモーターも出力クリップ
        }
        if(torqueRight < MIN_INNERWHEEL_MOTOR_TORQUE) {  //内輪最低トルク時
            torqueRight = MIN_INNERWHEEL_MOTOR_TORQUE;      //内輪最低トルクにクリップ
            torqueLeft = (int)((requestTorque-MIN_INNERWHEEL_MOTOR_TORQUE)*2.0) + MIN_INNERWHEEL_MOTOR_TORQUE;   //片モーター下限値時，トルク高側のモーターも出力クリップ
        }
    }

    //printf("%d %d %f\r\n", torqueRight, torqueLeft, getSteerAngle());

    McpData.valA = calcTorqueToVoltage(torqueRight, RIGHT_MOTOR);
    McpData.valB = calcTorqueToVoltage(torqueLeft, LEFT_MOTOR);

    //pc.printf("%u %u\r\n", McpData.valA, 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)
{
    rightMotorPulse.mode(PullUp);
    leftMotorPulse.mode(PullUp);
    rightMotorPulse.fall(&countRightPulseISR);
    leftMotorPulse.fall(&countLeftPulseISR);

    RightPulseTimer.reset();
    LeftPulseTimer.reset();
    RightPulseTimer.start();
    LeftPulseTimer.start();

    ticker1.attach(&loadSensorsISR, 0.01f);    //サンプリング周期10msec
    ticker2.attach(&getPulseTimeISR, 0.01f);

    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);
}