File content as of revision 1:4d86ec2fe4b1:

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

extern AnalogIn apsP;
extern AnalogIn apsS;
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)

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

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;

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

inline float convertVoltage(unsigned int count)
{
    return (count*(3.3f/0xFFFF));
}

int checkSensorPlausibility(void)
{
    int plausibility = 1;

    if(g_apsP < APS_MIN_POSITION) {
        plausibility = 0;
        LED[0] = 1;
    }
    if(g_apsP > APS_MAX_POSITION) {
        plausibility = 0;
        LED[0] = 1;
    }
    if(g_apsS < APS_MIN_POSITION) {
        plausibility = 0;
        LED[1] = 1;
    }
    if(g_apsS > APS_MAX_POSITION) {
        plausibility = 0;
        LED[1] = 1;
    }

    return plausibility;
}

inline void calcRequestTorque(void)
{
    unsigned int currentAPS;

    currentAPS = ((g_apsP>g_apsS) ? g_apsS : g_apsP);   //センサ値は小さい方を採用
    currentAPS -= APS_MIN_POSITION;     //オフセット修正

    if(currentAPS < APS_DEADBAND)   //デッドバンド内であれば要求トルク->0
        currentRequestTorque = 0;
    else
        currentRequestTorque = (unsigned int)(((float)MAX_MOTOR_TORQUE / APS_VALID_RANGE) * (currentAPS - APS_DEADBAND));
}

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 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)
{
    //Do not use "printf" in interrupt!!!
    static int preTime=0;
    int currentTime = timer.read_ms();
    rightPulseTime = currentTime - preTime;
    preTime = currentTime;
}

void countPulseL(void)
{
    //Do not use "printf" in interrupt!!!
    static int preTime=0;
    int currentTime = timer.read_ms();
    leftPulseTime = currentTime - preTime;
    preTime = currentTime;
}

void generatePulse(void)
{
    static bool flag = false;
    flag = !flag;
    MotorPulse[0] = MotorPulse[1] = LED[0] = flag;
}

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;

    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;

    aps = ((aps<APS_MIN_POSITION) ? 0.0f : (aps-APS_MIN_POSITION)); //オフセット修正

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