File content as of revision 44:d433bb5f77c0:

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

extern AnalogIn apsP;
extern AnalogIn apsS;
extern AnalogIn brake;
extern DigitalOut LED[];
extern DigitalOut brakeSignal;
extern DigitalOut indicatorLed;
extern DigitalOut shutDown;
extern DigitalIn sdState;
extern InterruptIn rightMotorPulse;
extern InterruptIn leftMotorPulse;
extern InterruptIn rightWheelPulse1;
extern InterruptIn rightWheelPulse2;
extern InterruptIn leftWheelPulse1;
extern InterruptIn leftWheelPulse2;
extern MCP4922 mcp;
extern Serial pc;
extern AnalogOut STR2AN;
extern CAN can;

#define indicateSystem(x)       (indicatorLed.write(x))

Timer wheelPulseTimer[4];
Ticker ticker1;
Ticker ticker2;

#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;  //現在の補正無しのセンサ値

int gRightMotorTorque=0, gLeftMotorTorque=0;

int getMotorTorque(select_t rl)
{
    return ((rl==RL_MOTOR) ? gLeftMotorTorque : gRightMotorTorque);
}

//エラーカウンタ外部参照用関数
//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;
    }
}

volatile 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の一時的なエラーカウンタ
        int tmpRawApsP, tmpRawApsS, tmpRawBrake;

        tmpRawApsP = (int)apsP.read_u16();
        tmpRawApsS = (int)apsS.read_u16();
        tmpRawBrake = (int)brake.read_u16();
        
        //Low Pass Filter
        tmpApsP = (int)(tmpRawApsP * ratioLPF + preApsP * (1.0-ratioLPF));
        tmpApsS = (int)(tmpRawApsS * ratioLPF + preApsS * (1.0-ratioLPF));
        tmpBrake = (int)(tmpRawBrake * ratioLPF + preBrake * (1.0-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_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;
        }

        //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((isBrakeOn() == 1) && (tmpApsP >= APS_OVERRIDE25))   //Brake-ON and APS > 25%
            errCounter.brakeOverRide++;
        if(tmpApsP < APS_OVERRIDE05)   //APS < 5%
            errCounter.brakeOverRide=0;

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

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

//*******************************************************
//車輪速計測は”【空転再粘着制御】山下道寛”を参照のこと(一部改修)
//*******************************************************
//wheelNumbler:ホイール識別番号
//FR:0 FL:1 RR:2 RL:3
typedef struct {
    int counter;
    int dT1;
    int dT2;
    bool preInputState;
    int stopCounter;
    float preRps;
} rps_t;

//パルス数カウンタ
volatile int gWheelPulseCounter[4] = {0};
//パルス入力までの時間
volatile int gWheelPulse_dT2[4] = {0};
//現在の回転数[rps]
float gRps[4] = {0};
//車輪速計測周期フラグ
volatile bool gloadWheelRpsFlag = false;

//***********************************
//モータパルスカウント
void countRightMotorPulseISR(void)
{
    gWheelPulseCounter[RR_MOTOR]++;
    gWheelPulse_dT2[RR_MOTOR] = wheelPulseTimer[RR_MOTOR].read_us();      //現在の時間いただきます
}
void countLeftMotorPulseISR(void)
{
    gWheelPulseCounter[RL_MOTOR]++;
    gWheelPulse_dT2[RL_MOTOR] = wheelPulseTimer[RL_MOTOR].read_us();      //現在の時間いただきます
}
//***********************************
//ホイールパルスカウント
void countRightWheelPulseISR(void)
{
    gWheelPulseCounter[FR_WHEEL]++;
    gWheelPulse_dT2[FR_WHEEL] = wheelPulseTimer[FR_WHEEL].read_us();      //現在の時間いただきます
}
void countLeftWheelPulseISR(void)
{
    gWheelPulseCounter[FL_WHEEL]++;
    gWheelPulse_dT2[FL_WHEEL] = wheelPulseTimer[FL_WHEEL].read_us();      //現在の時間いただきます
}
//***********************************

//回転数構造体初期化関数
rps_t initRps(void)
{
    rps_t initResult = {0, MAX_WHEEL_PULSE_TIME_US, 0, false, 0, 0.0f};
    return initResult;
}

//RPS読み込み許可設定関数
void loadRpsISR(void)
{
    gloadWheelRpsFlag = true;
}

//RPS読み込み関数
void loadRps(void)
{
    static rps_t rps[4] = {initRps(), initRps(), initRps(), initRps()};
    static int currentTime[4] = {0};
    float pulseNumPerRev;

    if(false == gloadWheelRpsFlag)
        return;
    else
        gloadWheelRpsFlag = false;

    for(int i=0; i<4; i++) {
        rps[i].counter  = gWheelPulseCounter[i];
        rps[i].dT2      = gWheelPulse_dT2[i];

        //前回パルス入力がない場合
        if(rps[i].preInputState == false) {
            //以前のdT1に前回の計測周期の時間を積算
            rps[i].dT1 = rps[i].dT1 + currentTime[i];
            //overflow防止処理
            if(rps[i].dT1 > MAX_WHEEL_PULSE_TIME_US)
                rps[i].dT1 = MAX_WHEEL_PULSE_TIME_US;
        }

        //現在の時間取得
        currentTime[i] = wheelPulseTimer[i].read_us();

        //次回計測周期までのパルス時間計測開始
        wheelPulseTimer[i].reset();
        //パルス数クリア
        gWheelPulseCounter[i] = 0;
        //dT2の初期値はパルス入力ない状態 => 計測時間=0
        gWheelPulse_dT2[i]    = 0;

        //一回転当たりのパルス数設定
        if(i <= 1)
            pulseNumPerRev = WHEEL_PULSE_NUM;   //Front車輪パルス数*割込み回数
        else
            pulseNumPerRev = MOTOR_PULSE_NUM;   //モータパルス数*割込み回数

        //パルス入力あれば直前のパルス入力からの経過時間取得
        if(rps[i].counter != 0) {
            rps[i].dT2 = currentTime[i] - rps[i].dT2;
        }

        //パルス入力ない場合---(設定回数未満)前回値保持/(設定回数以上)疑似パルス入力判定 (ピーク値を保存したい)
        if(rps[i].counter == 0) {
            if(rps[i].stopCounter < 50) //低回転数時、急に0rpsと演算しないように前回値保持(設定値はだいたい)
                rps[i].stopCounter++;
            else
                gRps[i] = 0.0f;
        } else {
            //RPS計算[rps](1sec当たりパルス数/タイヤパルス数)
            gRps[i] = ((float)rps[i].counter / ((currentTime[i] + rps[i].dT1 - rps[i].dT2) / 1000000.0f)) / pulseNumPerRev;
            gRps[i] = gRps[i] * ratioLPF_RPS + (1.0f-ratioLPF_RPS)*rps[i].preRps;
            rps[i].stopCounter = 0;
        }

        rps[i].preRps = gRps[i];

        //パルス入力あれば次回のdT1はdT2を採用(パルス入力なければ現在値保持)
        if(rps[i].counter != 0)
            rps[i].dT1 = rps[i].dT2;

        if(rps[i].counter == 0)
            rps[i].preInputState = false;
        else
            rps[i].preInputState = true;
    }
}

//車輪RPS取得関数
float getWheelRps(select_t position)
{
    float deltaN;   //左右モータ回転数差
    float aveN;     //左右モータ回転数平均値

    if(position < 2)
        return gRps[position];
    else {
        //右車輪回転数が大きいと仮定
        aveN    = ((gRps[RR_MOTOR] + gRps[RL_MOTOR]) / GEAR_RATIO) / 2.0f;    //平均回転数計算
        deltaN  = ((gRps[RR_MOTOR] - gRps[RL_MOTOR]) / GEAR_RATIO) / ALPHA;   //速度線図上の車輪回転数差計算

        if(position == RR_MOTOR)
            return aveN + deltaN / 2.0f;  //右車輪回転数
        else
            return aveN - deltaN / 2.0f;  //左車輪回転数
    }
}

float getRps(select_t position)
{
    return gRps[position];
}

//***********************************
//スリップ率取得関数
//select    :0---RIGHT WHEEL
//           1---LEFT WHEEL
float getSlipRatio(bool rl_select)
{
    float slipRatio;

    if(rl_select == 0) {
        slipRatio = 1.0f - getWheelRps(FR_WHEEL) / getWheelRps(RR_MOTOR);
    } else {
        slipRatio = 1.0f - getWheelRps(FL_WHEEL) / getWheelRps(RL_MOTOR);
    }

    if(slipRatio < 0.0f)
        slipRatio = 0.0f;    //減速時は考慮しない

    return slipRatio;
}

//車速取得関数[m/s]
//左右従動輪回転数の平均値から車速を演算
float getVelocity(void)
{
    return (0.5f*TIRE_DIAMETER*2.0f*M_PI*(getWheelRps(FR_WHEEL) + getWheelRps(FL_WHEEL))*0.5f);
}

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

    steeringWheelAngle = ratioLPF * steeringWheelAngle + (1.0f - ratioLPF) * lastSteering;

    omega = steeringWheelAngle - lastSteering;    //舵角の差分算出
    omega /= 0.01f;  //制御周期で角速度演算

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

    lastSteering = steeringWheelAngle;

    if(omega < 0)
        disTrq = -disTrq;

    return disTrq;
}

//int distributeTorque(float steeringWheelAngle, float velocity)
//{
//    double V2 = velocity * velocity;
//    double R = 0.0;     //旋回半径
//    double Gy = 0.0;    //横G
//    double deadband = 0.0;
//    double steeringAngle = (double)steeringWheelAngle * STEER_RATIO;
//    double steeringSign = 1.0;
//    int disTrq = 0;
//
//    if(steeringAngle > 0)
//        steeringSign = 1.0;
//    else
//        steeringSign = -1.0;
//
//    steeringAngle = myAbs(steeringAngle);
//
//    if(steeringAngle <= 0.0001)
//        steeringAngle = 0.0001;
//
//    R = (1.0 + A*V2)*WHEEL_BASE / steeringAngle;   //理論旋回半径 計算
//    Gy = V2 / R / 9.81;                                    //理論横G
//
//    if(Gy <= deadband)
//        disTrq = 0;
//    else if(Gy <= 1.5) {
//        Gy -= deadband;
//        disTrq = (int)(MAX_DISTRIBUTION_TORQUE / (1.5 - deadband) * Gy);
//    } else {
//        disTrq = MAX_DISTRIBUTION_TORQUE;
//    }
//
//    return (int)(disTrq * steeringSign);
//}

int distributeTorque(float steeringWheelAngle)
{
    float deadband = 0.0;
    double steeringSign = 1.0;
    int disTrq = 0;

    if(steeringWheelAngle > 0)
        steeringSign = 1.0;
    else
        steeringSign = -1.0;

    if(myAbs(steeringWheelAngle) <= deadband)
        disTrq = 0;
    else if(myAbs(steeringWheelAngle) <= (0.5f*M_PI)) {
        disTrq = (int)(MAX_DISTRIBUTION_TORQUE / (0.5f*M_PI - deadband) * (myAbs(steeringWheelAngle) - deadband));
    } else {
        disTrq = MAX_DISTRIBUTION_TORQUE;
    }

    return (int)(disTrq * steeringSign);
}


//rpm +++++ モータ回転数
//regSwitch +++++ 回生=1 力行=0
inline int calcMaxTorque(int rpm, bool regSwitch)
{
    int maxTrq=0;

    //後で削除
    rpm = 2000;
    //++++++++++++++++++++

    if(regSwitch == 0) {
        if(rpm <3000)
            maxTrq = MAX_MOTOR_TORQUE_POWER;
        else if(rpm <= 11000)
            maxTrq = maxTorqueMap[(int)(rpm / 10.0)];
        else
            maxTrq = MAX_REVOLUTION_TORQUE_POWER;
    } else {
        if(rpm < 600) {
            maxTrq = 0;
        } else if(rpm < 1250) {
            //+++++++++++++++++++++++++++++++++++++
            //暫定処理 今後回生トルクマップも要作成
            maxTrq = 0;
            //+++++++++++++++++++++++++++++++++++++
        } else if(rpm <= 6000) {
            maxTrq = MAX_MOTOR_TORQUE_REGENERATIVE;
        } else if(rpm <= 11000) {
            //+++++++++++++++++++++++++++++++++++++
            //暫定処理 今後回生トルクマップも要作成
            maxTrq = MAX_REVOLUTION_TORQUE_REGENERATIVE;
            //+++++++++++++++++++++++++++++++++++++
        } else {
            maxTrq = MAX_REVOLUTION_TORQUE_REGENERATIVE;
        }
    }
    return maxTrq;
}

//演算方法
//y = a(x - b) + c
//x = 1/a * (y + ab - c)
unsigned int calcTorqueToVoltage(int reqTorque, int rpm)
{
    float slope = 0;        //a
    int startPoint = 0;     //b
    int intercept = 0;      //c

    int outputVoltage=0;

    if(reqTorque > LINEAR_REGION_TORQUE_POWER) {        //力行トルクがrpmに対して非線形となる領域
        slope = (float)(calcMaxTorque(rpm, 0) - LINEAR_REGION_TORQUE_POWER)/(DACOUTPUT_MAX - LINEAR_REGION_VOLTAGE_POWER);
        startPoint = LINEAR_REGION_VOLTAGE_POWER;
        intercept = LINEAR_REGION_TORQUE_POWER;

        outputVoltage = (int)((reqTorque + slope*startPoint - intercept) / slope);

    } else if(reqTorque > 0) {                          //力行トルクがrpmに対して線形となる領域
        slope = (float)LINEAR_REGION_TORQUE_POWER/(LINEAR_REGION_VOLTAGE_POWER - ZERO_TORQUE_VOLTAGE_P);
        startPoint = ZERO_TORQUE_VOLTAGE_P;
        intercept = 0;

        outputVoltage = (int)(reqTorque/slope + startPoint);

    } else if(0 == reqTorque) {

        outputVoltage = ZERO_TORQUE_VOLTAGE_NEUTRAL;    //ニュートラル信号

    } else if(reqTorque > LINEAR_REGION_TORQUE_REGENERATIVE) {  //回生トルクがrpmに対して線形となる領域
        slope = (float)(0 - LINEAR_REGION_TORQUE_REGENERATIVE)/(ZERO_TORQUE_VOLTAGE_REG - LINEAR_REGION_VOLTAGE_REGENERATIVE);
        startPoint = LINEAR_REGION_VOLTAGE_REGENERATIVE;
        intercept = LINEAR_REGION_TORQUE_REGENERATIVE;

        outputVoltage = (int)(reqTorque/slope + startPoint);

    } else {                                            //回生トルクがrpmに対して非線形となる領域
        slope = (float)(LINEAR_REGION_TORQUE_REGENERATIVE - calcMaxTorque(rpm, 1))/(LINEAR_REGION_VOLTAGE_REGENERATIVE - DACOUTPUT_MIN);
        startPoint = DACOUTPUT_MIN;
        intercept = calcMaxTorque(rpm, 1);

        outputVoltage = (int)((reqTorque + slope*startPoint - intercept) / slope);
    }

    if(outputVoltage > DACOUTPUT_MAX)
        outputVoltage = DACOUTPUT_MAX;
    if(outputVoltage < DACOUTPUT_MIN)
        outputVoltage = DACOUTPUT_MIN;

    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_REG_RANGE)   //デッドバンド内であれば要求トルク->0
        requestTorque = (int)((float)(-MAX_OUTPUT_TORQUE_REGENERATIVE) / APS_REG_RANGE * currentAPS + MAX_OUTPUT_TORQUE_REGENERATIVE);
    else
        requestTorque = (int)((float)MAX_OUTPUT_TORQUE_POWER / APS_PWR_RANGE * (currentAPS - APS_REG_RANGE));

    if(requestTorque > MAX_OUTPUT_TORQUE_POWER)
        requestTorque = MAX_OUTPUT_TORQUE_POWER;
    else if(requestTorque < MAX_OUTPUT_TORQUE_REGENERATIVE)
        requestTorque = MAX_OUTPUT_TORQUE_REGENERATIVE;

    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 < 15.0f)
        limitRate = 0.0f;
    else if(currentVelocity < 30.0f)
        limitRate = (currentVelocity - 15.0f) / (30.0f - 15.0f);
    else
        limitRate = 1.0f;

    return limitRate;
}

void driveTVD(int TVDmode, bool isRedyToDrive)
{
    int requestTorque=0;    //ドライバー要求トルク
    int distributionTrq=0;  //分配トルク
    int disTrq_omega=0;
    int torqueRight, torqueLeft;    //トルクの右左
    static unsigned int preMcpA=0, preMcpB=0;

    loadSensors();      //APS,BRAKE更新
    loadSteerAngle();   //舵角更新
    loadRps();          //従動輪・モータ回転数更新

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

    if(isRedyToDrive && 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();  //ドライバー要求トルク取得

    distributionTrq = (int)(distributeTorque(M_PI * getSteerAngle() / 127.0f) / 2.0f);  //片モーターのトルク分配量計算
    disTrq_omega = (int)((distributeTorque_omega(M_PI * getSteerAngle() / 127.0f)*limitTorqueDistribution()) / 2.0f);      //微分制御

//    distributionTrq = 0;
    disTrq_omega = 0;

    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_POWER) { //片モーター上限時最大値にクリップ
            torqueLeft = MAX_OUTPUT_TORQUE_POWER;

            if(((torqueRight + torqueLeft)/2.0f) > requestTorque) {
                torqueRight = requestTorque - (MAX_OUTPUT_TORQUE_POWER-requestTorque);
            }
        }
        if(torqueRight > MAX_OUTPUT_TORQUE_POWER) { //片モーター上限時最大値にクリップ
            torqueRight = MAX_OUTPUT_TORQUE_POWER;
            if(((torqueRight + torqueLeft)/2.0f) > requestTorque) {
                torqueLeft = requestTorque - (MAX_OUTPUT_TORQUE_POWER-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;   //片モーター下限値時，トルク高側のモーターも出力クリップ
        }
    }

    gRightMotorTorque = torqueRight;
    gLeftMotorTorque = torqueLeft;

    McpData.valA = calcTorqueToVoltage(torqueRight, getRps(RR_MOTOR) * 60.0f);
    McpData.valB = calcTorqueToVoltage(torqueLeft, getRps(RL_MOTOR) * 60.0f);

    preMcpA = McpData.valA;
    preMcpB = McpData.valB;

    mcp.writeA(preMcpA);   //右モーター
    mcp.writeB(preMcpB);   //左モーター
}

void initTVD(void)
{
    mcp.writeA(0);   //右モーター初期値
    mcp.writeB(0);   //左モーター初期値

    wheelPulseTimer[FR_WHEEL].reset();
    wheelPulseTimer[FL_WHEEL].reset();
    wheelPulseTimer[RR_MOTOR].reset();
    wheelPulseTimer[RL_MOTOR].reset();

    wheelPulseTimer[FR_WHEEL].start();
    wheelPulseTimer[FL_WHEEL].start();
    wheelPulseTimer[RR_MOTOR].start();
    wheelPulseTimer[RL_MOTOR].start();

    rightMotorPulse.rise(&countRightMotorPulseISR);
    leftMotorPulse.rise(&countLeftMotorPulseISR);

//    rightWheelPulse1.fall(&countRightWheelPulseISR);  //パルス測定は立ち上がりor立下りのどちらかを計測するのが吉
//    rightWheelPulse2.fall(&countRightWheelPulseISR);  //立下り特性悪すぎなので測定誤差が増える
    rightWheelPulse1.rise(&countRightWheelPulseISR);
//    rightWheelPulse2.rise(&countRightWheelPulseISR);

//    leftWheelPulse1.fall(&countLeftWheelPulseISR);
//    leftWheelPulse2.fall(&countLeftWheelPulseISR);
    leftWheelPulse1.rise(&countLeftWheelPulseISR);
//    leftWheelPulse2.rise(&countLeftWheelPulseISR);    //AB相の位相差が90度から離れすぎなので測定誤差が増える(スリットが一定間隔でないことになる)

    ticker1.attach(&loadSensorsISR, CONTROL_CYCLE_S);               //制御周期毎にデータ読み込み(LPF演算のため)
    ticker2.attach(&loadRpsISR, RPS_MEAS_CYCLE_S);   //RPS計測周期設定

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