File content as of revision 29:a51cb2cf22ae:

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

Serial pc(USBTX, USBRX); // tx, rx

////////////////////////////////////////
//IO宣言
SPI spi(p5,p6,p7);
MCP4922 mcp(p5,p7,p8);      // MOSI, SCLK, CS
DigitalOut ioExpCs(p9);
DigitalOut indicatorLed(p10);
DigitalOut shutDown(p11);
DigitalOut brakeSignal(p12);
DigitalIn sdState(p13);
DigitalIn RTDSW(p14);
AnalogIn apsS(p15);         //"S"econdary
AnalogIn apsP(p16);         //"P"rimary
AnalogIn brake(p17);
AnalogOut STR2AN(p18);
DigitalIn SLCTSW[3] = {p19, p20, p21};
InterruptIn rightMotorPulse(p22);
InterruptIn leftMotorPulse(p23);
InterruptIn rightTirePulse1(p24);
InterruptIn rightTirePulse2(p25);
InterruptIn leftTirePulse1(p26);
InterruptIn leftTirePulse2(p27);
DigitalOut WDT(p28);
CAN can(p30, p29);
DigitalOut LED[] = {LED1, LED2, LED3, LED4};

Timer timer;

#define indicateSystem(x)       (indicatorLed.write(x))
#define shutdownSystem(void)    (shutDown.write(0))
#define bootSystem(void)        (shutDown.write(1))
#define isPressedRTD(void)      (!RTDSW.read())
#define isShutdownSystem(void)  (SDState.read())

//++++++++++++++++++++++++++
//CAN ID
#define CANID_BRAKE         0xF1
#define CANID_REQTRQ        0xF2
#define CANID_MOTORTRQ      0xF3
#define CANID_STEERING      0xF4
#define CANID_WHEELSPEED    0xF5
#define CANID_MOTORSPEED    0xF6
#define CANID_VEHICLESPEED  0xF7
//++++++++++++++++++++++++++

//++++++++++++++++++++++++++
//CANデータのバイト数
#define CANDATANUM_BRAKE        1
#define CANDATANUM_REQTRQ       4
#define CANDATANUM_MOTORTRQ     8
#define CANDATANUM_STEERING     1
#define CANDATANUM_WHEELSPEED   4
#define CANDATANUM_MOTORSPEED   4
#define CANDATANUM_VEHICLESPEED 2
//++++++++++++++++++++++++++

void writeCanBuff(int data, char* buff, int byteNum)
{
    for(int i=0; i<byteNum; i++) {
        buff[i] = 0xff & (data >> (i*8));
    }
}

void init(void)
{
    indicatorLed = 0;
    shutDown = 0;
    brakeSignal = 0;
    LED[0] = LED[1] = LED[2] = LED[3] = 0;

    sdState.mode(PullNone);
    RTDSW.mode(PullNone);
    SLCTSW[0].mode(PullNone);
    SLCTSW[1].mode(PullNone);
    SLCTSW[2].mode(PullNone);

    rightMotorPulse.mode(PullNone);
    leftMotorPulse.mode(PullNone);
    rightTirePulse1.mode(PullNone);
    rightTirePulse2.mode(PullNone);
    leftTirePulse1.mode(PullNone);
    leftTirePulse2.mode(PullNone);

    can.frequency(500000);      //500kHz

    indicateSystem(1);
    bootSystem();
}

int main(void)
{
    char canBrake[CANDATANUM_BRAKE]= {0};
    char canRequestTorque[CANDATANUM_REQTRQ]= {0};
    char canMotorTorque[CANDATANUM_MOTORTRQ]= {0};
    char canSteer[CANDATANUM_STEERING]= {0};
    char canMotorSpeed[CANDATANUM_MOTORSPEED]= {0};
    char canWheelSpeed[CANDATANUM_WHEELSPEED]= {0};
    char canVehicleSpeed[CANDATANUM_VEHICLESPEED]= {0};

    //wait(1);  //断線検出に引っかかるためMotorControllerの起動よりも早くTVDCは起動する必要あり 
    
    pc.baud(115200);
    printf("\033[2J");          //teratermの画面クリア
    printf("\033[1;1H");        //teratermの画面クリア
    printf("\r\nVersion:TVDctrller2017_brdRev1...start!!!!!\r\n");

    init();       //IOポート初期化

    initTVD();

    initSteering();

    timer.start();

    float time;

    struct errCounter_t eCounter= {0,0,0,0,0,0,0,0};

    wait(3);

    while(1) {
        //getCurrentErrCount(&eCounter);

        timer.reset();

        driveTVD(0, isPressedRTD());

        printf("%f\n\r", M_PI * getSteerAngle()/127.0f);

        /*
        writeCanBuff(isBrakeOn(),                                           canBrake,           CANDATANUM_BRAKE);
        writeCanBuff(calcRequestTorque(),                                   canRequestTorque,   CANDATANUM_REQTRQ);
        writeCanBuff(getMotorTorque(RIGHT) + (getMotorTorque(LEFT) << 16),  canMotorTorque,     CANDATANUM_MOTORTRQ);
        writeCanBuff(getSteerAngle(),                                       canSteer,           CANDATANUM_STEERING);
        writeCanBuff(getRPS(RIGHT, 0) + (getRPS(LEFT, 0)<<16),              canMotorSpeed,      CANDATANUM_MOTORSPEED);
        writeCanBuff(getRPS(RIGHT, 1) + (getRPS(LEFT, 1)<<16),              canWheelSpeed,      CANDATANUM_WHEELSPEED);
        writeCanBuff(getVelocity() / 0.1f,                                  canVehicleSpeed,    CANDATANUM_VEHICLESPEED);

        can.write(CANMessage(CANID_BRAKE, canBrake, CANDATANUM_BRAKE));
        can.write(CANMessage(CANID_REQTRQ, canRequestTorque, CANDATANUM_REQTRQ));
        can.write(CANMessage(CANID_MOTORTRQ, canMotorTorque, CANDATANUM_MOTORTRQ));
        can.write(CANMessage(CANID_STEERING, canSteer, CANDATANUM_STEERING));
        can.write(CANMessage(CANID_WHEELSPEED, canMotorSpeed, CANDATANUM_MOTORSPEED));
        can.write(CANMessage(CANID_MOTORSPEED, canWheelSpeed, CANDATANUM_WHEELSPEED));
        can.write(CANMessage(CANID_VEHICLESPEED, canVehicleSpeed, CANDATANUM_VEHICLESPEED));
        */
        
        //printf("%1.6f\r\n", time);
        //printf("%d\r\n", eCounter.brakeOverRide);
        time = timer.read();
        while(timer.read_ms() < CONTROL_CYCLE_MS);      //制御周期管理 関数内処理時間より短い時間の制御周期の設定は禁止

        //printf("%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t\r\n", eCounter.apsUnderVolt, eCounter.apsExceedVolt, eCounter.apsErrorTolerance, eCounter.apsStick, eCounter.brakeUnderVolt, eCounter.brakeExceedVolt, eCounter.brakeFuzzyVolt, eCounter.brakeOverRide);
        //printf("apsP:%1.2f, apsS:%1.2f, brake:%1.2f\r", 3.3f/65535 * getRawSensor(APS_PRIMARY), 3.3f/65535 * getRawSensor(APS_SECONDARY), 3.3f/65535 * getRawSensor(BRAKE));
    }
}