File content as of revision 49:c97740265324:

#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_ACC_BRK + preApsP * (1.0-ratioLPF_ACC_BRK));
        tmpApsS = (int)(tmpRawApsS * ratioLPF_ACC_BRK + preApsS * (1.0-ratioLPF_ACC_BRK));
        tmpBrake = (int)(tmpRawBrake * ratioLPF_ACC_BRK + preBrake * (1.0-ratioLPF_ACC_BRK));

        //生のセンサ値取得
        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;
    double 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読み込み許可設定関数
void loadRpsISR(void)
{
    gloadWheelRpsFlag = true;
}

//RPS読み込み関数
void loadRps(void)
{
    const rps_t INITRPS = {0, MAX_WHEEL_PULSE_TIME_US, 0, false, 0, 0.0};  //構造体初期化用定数
    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 <= FL_WHEEL)
            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.0;
        } else {
            //RPS計算[rps](1sec当たりパルス数/タイヤパルス数)
            gRps[i] = ((double)rps[i].counter / ((currentTime[i] + rps[i].dT1 - rps[i].dT2) / 1000000.0)) / pulseNumPerRev;
            gRps[i] = gRps[i] * ratioLPF_RPS + (1.0-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取得関数
double getWheelRps(select_t position)
{
    double deltaN;   //左右モータ回転数差
    double aveN;     //左右モータ回転数平均値

    if(position <= FL_WHEEL)                                                //前輪の場合
        return gRps[position];                                              //演算結果をそのまま適用
    else {                                                                  //後輪の場合,モータ回転数から速度線図に従い演算
        //右車輪回転数が大きいと仮定
        aveN    = ((gRps[RR_MOTOR] + gRps[RL_MOTOR]) / GEAR_RATIO) / 2.0;  //平均回転数計算
        deltaN  = ((gRps[RR_MOTOR] - gRps[RL_MOTOR]) / GEAR_RATIO) / ALPHA; //速度線図上の車輪回転数差計算

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

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

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

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 - intercept) / slope);

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

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

//------------------------------
//参考文献：デジタルPI制御と離散化
//------------------------------
//速度アルゴリズム型PID制御器
//lastOutput    : 前回の制御出力
//*e            : 過去3つ分の偏差
double calcPID(double lastOutput, double* e)
{
    //------------------------------
    //Constant variables
    const double KP = 40.0;
    const double KI = 0.0;
    const double KD = 0.0;
    //------------------------------

    return lastOutput + KP*(e[0] - e[1]) + KI*(e[1] + e[0]) + KD*(e[0] - 2.0*e[1] + e[2]);    //PID controller
}

//--------------------------------------------------
//参考文献：モデルを用いたトラクションコントロールの基礎研究
//--------------------------------------------------
void getTractionCtrl(int* i_motorTrq)
{
    //------------------------------
    //Constant variables
    const double TGT_SLIP_RATIO = 0.1;
    const double TGT_VEHICLE_SPEED = 1.0 / 3.6;                 //トラコンONとなる車速[m/s](これ未満は空転を抑える制御をする)
    //------------------------------

    double reqMotorTrq[2] = {i_motorTrq[0] * LSB_MOTOR_TORQUE, i_motorTrq[1] * LSB_MOTOR_TORQUE};         //実数値へ変換
    double outputTrq[2] = {0.0};
    double steeringAngle, R, V, Vb, Vw, wheelRpsRR, wheelRpsRL;
    static double lastMotorTrq[2] = {0.0};                          //前回の出力トルク
    double motorTrq_wTCS[2] = {0.0};                                //TCSトルクベクタリングを含めたトルク
    static double e[2][3] = {0.0};                                  //3つ前の偏差まで保持

    V = getVelocity();                                              //前輪回転方向における車速換算値
    
//    V = 5.6;
    
    wheelRpsRR = getWheelRps(RR_MOTOR);
    wheelRpsRL = getWheelRps(RL_MOTOR);

    steeringAngle   = getSteerAngle()/127.0 * M_PI*STEER_RATIO; //実舵角取得
    Vb              = V * cos(steeringAngle);                   //2輪モデルにおける車体進行方向速度取得
    R               = mySign(steeringAngle) * (1.0 + A*Vb*Vb) * WHEEL_BASE/myMax(myAbs(steeringAngle), 0.001);  //理論旋回半径取得

    if(myAbs(R) < 1.0)
        R = mySign(steeringAngle) * 1.0;

    for (int rlFlag = 0; rlFlag < 2; rlFlag++) {
        if(rlFlag == 0) {
            Vb = Vb*(1.0 + TREAD/(2.0*R));                          //トレッドを考慮した従動輪速度[m/s]
            Vw = 0.5*TIRE_DIAMETER*2.0*M_PI*wheelRpsRR;             //駆動輪速度[m/s]
        } else {
            Vb = Vb*(1.0 - TREAD/(2.0*R));                          //トレッドを考慮した従動輪速度[m/s
            Vw = 0.5*TIRE_DIAMETER*2.0*M_PI*wheelRpsRL;             //駆動輪速度[m/s]
        }

        if(Vb < TGT_VEHICLE_SPEED) {                                //対地車速が一定値より小さい場合(停車状態)
            e[rlFlag][0] = TGT_VEHICLE_SPEED/(1.0 - TGT_SLIP_RATIO) - Vw;   //Rev limit制御(一定回転数以上回転させない)
        } else {
            e[rlFlag][0] = Vb/(1.0 - TGT_SLIP_RATIO) - Vw;          //現在の偏差取得(目標スリップ率における車輪速と駆動輪速度の差分)
        }

        lastMotorTrq[rlFlag] = calcPID(lastMotorTrq[rlFlag], e[rlFlag]);    //PIDコントローラへどーーーん
        e[rlFlag][2] = e[rlFlag][1];
        e[rlFlag][1] = e[rlFlag][0];
    }

    if((lastMotorTrq[0] < reqMotorTrq[0]) && (lastMotorTrq[1] > reqMotorTrq[1])) {          //TCS R:L => ON:OFF
        motorTrq_wTCS[1] = reqMotorTrq[1] + (reqMotorTrq[0] - lastMotorTrq[0]);                               //TCSで制限された分を左へベクタリング
        motorTrq_wTCS[0] = reqMotorTrq[0];
    } else if((lastMotorTrq[1] < reqMotorTrq[1]) && (lastMotorTrq[0] > reqMotorTrq[0])) {   //TCS R:L => OFF:ON
        motorTrq_wTCS[0] = reqMotorTrq[0] + (reqMotorTrq[1] - lastMotorTrq[1]);                               //TCSで制限された分を右へベクタリング
        motorTrq_wTCS[1] = reqMotorTrq[1];
    } else {
        motorTrq_wTCS[0] = reqMotorTrq[0];
        motorTrq_wTCS[1] = reqMotorTrq[1];
    }
    
    printf("%f %f\r\n", e[0][0], e[1][0]);

    for (int rlFlag = 0; rlFlag < 2; rlFlag++) {
        outputTrq[rlFlag] = myMin(reqMotorTrq[rlFlag], myMin(lastMotorTrq[rlFlag], motorTrq_wTCS[rlFlag]));    //ちっさい方を採用

//        if(outputTrq[rlFlag] < 0.0)                              //現状、マイナストルクは無しで
//            outputTrq[rlFlag] = 0.0;

        if(outputTrq[rlFlag] > MAX_MOTOR_TORQUE_POWER*LSB_MOTOR_TORQUE)
            outputTrq[rlFlag] = MAX_MOTOR_TORQUE_POWER*LSB_MOTOR_TORQUE;

        if(outputTrq[rlFlag] < MAX_MOTOR_TORQUE_REGENERATIVE*LSB_MOTOR_TORQUE)
            outputTrq[rlFlag] = MAX_MOTOR_TORQUE_REGENERATIVE*LSB_MOTOR_TORQUE;

        i_motorTrq[rlFlag] = (int)(outputTrq[rlFlag] / LSB_MOTOR_TORQUE + 0.5);
    }
}

void driveTVD(int TVDmode, bool isRedyToDrive)
{
    int requestTorque = 0;    //ドライバー要求トルク
    int distributionTrq = 0;  //分配トルク
    int disTrq_omega = 0;
    int motorTrq[2] = {0};        //左右モータトルク
    static unsigned int preMcpA = 0, preMcpB = 0;

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

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

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

    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;

    motorTrq[0] = requestTorque + distributionTrq;
    motorTrq[1] = requestTorque - distributionTrq;

    motorTrq[0] += disTrq_omega;
    motorTrq[1] -= disTrq_omega;

    getTractionCtrl(motorTrq);

    //現在バグあり
    //アクセル全開で旋回後、舵を中立に戻していくと加速する。旋回を優先するモード
//    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 = motorTrq[0];
    gLeftMotorTorque = motorTrq[1];

    McpData.valA = calcTorqueToVoltage(motorTrq[0], getRps(RR_MOTOR) * 60.0f);
    McpData.valB = calcTorqueToVoltage(motorTrq[1], 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);
}