File content as of revision 37:ba10cf09c151:

#include "TVDCTRL.h"
#include "MCP4922.h"
#include "Steering.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 rightTirePulse1;
extern InterruptIn rightTirePulse2;
extern InterruptIn leftTirePulse1;
extern InterruptIn leftTirePulse2;
extern MCP4922 mcp;
extern Serial pc;
extern AnalogOut STR2AN;
extern CAN can;

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

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

int gRightMotorTorque=0, gLeftMotorTorque=0;

int getMotorTorque(Select rl)
{
    return ((rl==LEFT) ? 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;
    }
}

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_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)   //Brake-ON and APS < 5%
            errCounter.brakeOverRide=0;

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

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

volatile int gRightMotorPulseCounter = 0, gLeftMotorPulseCounter = 0;
volatile bool pulseTimeISRFlag = false;

void countRightMotorPulseISR(void)
{
    gRightMotorPulseCounter++;
}

void countLeftMotorPulseISR(void)
{
    gLeftMotorPulseCounter++;
}

void getPulseCounterISR(void)
{
    pulseTimeISRFlag = true;
}

//default argument  : switchWheel=false
int getRPS(Select rl, bool switchWheel)
{
    static int rightMotorPulse[100]= {0}, leftMotorPulse[100]= {0};     //過去1.0秒間のパルス数格納
    static int sumRightMotorPulse, sumLeftMotorPulse;
    float rps;

    if(pulseTimeISRFlag == true) {
        for(int i=99; i>0; i--) {
            rightMotorPulse[i] = rightMotorPulse[i-1];
            leftMotorPulse[i] = leftMotorPulse[i-1];
        }

        rightMotorPulse[0] = gRightMotorPulseCounter;
        leftMotorPulse[0] = gLeftMotorPulseCounter;

        gRightMotorPulseCounter = 0;
        gLeftMotorPulseCounter = 0;

        sumRightMotorPulse = 0;
        sumLeftMotorPulse = 0;

        for(int i=0; i<100; i++) {
            sumRightMotorPulse += rightMotorPulse[i];
            sumLeftMotorPulse += leftMotorPulse[i];
        }
        pulseTimeISRFlag = false;
    }

    if(switchWheel == false) {
        if(rl == RIGHT)
            rps = (float)1.0f*sumRightMotorPulse / MOTOR_PULSE_NUM / GEAR_RATIO;    //過去0.5秒間のモータパルス数を使ってRPS算出
        else
            rps = (float)1.0f*sumLeftMotorPulse / MOTOR_PULSE_NUM / GEAR_RATIO;     //過去0.5秒間のモータパルス数を使ってRPS算出
    } else {
        //こっちはタイヤ回転数
        //そのうち対応
        if(rl == RIGHT)
            rps = (float)sumRightMotorPulse / MOTOR_PULSE_NUM;    //過去1秒間のモータパルス数を使ってRPS算出
        else
            rps = (float)sumLeftMotorPulse / MOTOR_PULSE_NUM;     //過去1秒間のモータパルス数を使ってRPS算出
    }
    return (int)(rps / LSB_MOTORSPEED);     //LSB変換
}

float getVelocity(void)
{
    return (0.5f*TIRE_DIAMETER*2*M_PI*(getRPS(RIGHT) + getRPS(LEFT))*0.5f)*LSB_MOTORSPEED;
}

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

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

    if(currentVelocity < 15.0f)
        limitRate = 0.0f;
    else if(currentVelocity < 30.0f)
        limitRate = (currentVelocity - 15.0f) / (30.0f - 10.0f);
    else
        limitRate = 1.0f;

    //printf("rate:%1.3f\r\n", limitRate);

    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();   //舵角更新

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

    //+++++++++++++++++++
    //後で削除すること！
    //readyToDriveFlag = 0;
    //+++++++++++++++++++

    indicateSystem(readyToDriveFlag | (errCounter.brakeOverRide > ERRCOUNTER_DECISION));
    LED[0] = readyToDriveFlag | (errCounter.brakeOverRide > ERRCOUNTER_DECISION);

    requestTorque=calcRequestTorque();  //ドライバー要求トルク取得

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

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

    //distributionTrq = 0;
    disTrq_omega = 0;

    torqueRight = requestTorque + distributionTrq;
    torqueLeft = requestTorque - distributionTrq;

    torqueRight += disTrq_omega;
    torqueLeft -= disTrq_omega;

    //アクセルべた踏みでトルクMAX、旋回より駆動を優先(加速番長モード)
    if(torqueLeft > MAX_OUTPUT_TORQUE_POWER) { //片モーター上限時最大値にクリップ
        torqueLeft = MAX_OUTPUT_TORQUE_POWER;
        torqueRight = requestTorque - (MAX_OUTPUT_TORQUE_POWER-requestTorque);
    }
    if(torqueRight > MAX_OUTPUT_TORQUE_POWER) { //片モーター上限時最大値にクリップ
        torqueRight = MAX_OUTPUT_TORQUE_POWER;
        torqueLeft = requestTorque - (MAX_OUTPUT_TORQUE_POWER-requestTorque);
    }

    gRightMotorTorque = torqueRight;
    gLeftMotorTorque = torqueLeft;

    McpData.valA = calcTorqueToVoltage(torqueRight, getRPS(RIGHT));
    McpData.valB = calcTorqueToVoltage(torqueLeft, getRPS(LEFT));

    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)
{
    RightPulseTimer.reset();
    LeftPulseTimer.reset();
    RightPulseTimer.start();
    LeftPulseTimer.start();

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

    ticker1.attach(&loadSensorsISR, CONTROL_CYCLE_S);   //制御周期毎にデータ読み込み(LPF演算のため)
    ticker2.attach(&getPulseCounterISR, CONTROL_CYCLE_S);  //

    mcp.writeA(0);   //右モーター
    mcp.writeB(0);   //左モーター

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