File content as of revision 47:949e6c2e69fc:

#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 rightWheelPulse1(p24);
InterruptIn rightWheelPulse2(p25);
InterruptIn leftWheelPulse1(p26);
InterruptIn leftWheelPulse2(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_RIGHT_TRQ     0xF3
#define CANID_LEFT_TRQ      0xF4
#define CANID_STEERING      0xF5
#define CANID_WHEELSPEED    0xF6
#define CANID_MOTORSPEED    0xF7
#define CANID_VEHICLESPEED  0xF8
#define CANID_DISTRQ        0xF9
#define CANID_RR_RPS        0xFA
#define CANID_RL_RPS        0xFB
#define CANID_FR_RPS        0xFC
#define CANID_FL_RPS        0xFD
#define CANID_APS_P         0xFE
#define CANID_APS_S         0xFF
//++++++++++++++++++++++++++

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

void writeCanBuff(long long 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);
    rightWheelPulse1.mode(PullNone);
    rightWheelPulse2.mode(PullNone);
    leftWheelPulse1.mode(PullNone);
    leftWheelPulse2.mode(PullNone);

    can.frequency(500000);      //500kHz

    indicateSystem(1);
    bootSystem();
}

int main(void)
{
//    char canBrake[CANDATANUM_BRAKE]= {0};
    char canRequestTorque[CANDATANUM_REQTRQ]= {0};
    char canMotorTorqueR[CANDATANUM_MOTORTRQ]= {0};
    char canMotorTorqueL[CANDATANUM_MOTORTRQ]= {0};
    char canSteer[CANDATANUM_STEERING]= {0};
//    char canMotorSpeed[CANDATANUM_MOTORSPEED]= {0};
//    char canWheelSpeed[CANDATANUM_WHEELSPEED]= {0};
    char canVehicleSpeed[CANDATANUM_VEHICLESPEED]= {0};
    char canTorqueDistribution[CANDATANUM_DISTRQ]= {0};
    char canRR_RPS[CANDATANUM_RPS]= {0};
    char canRL_RPS[CANDATANUM_RPS]= {0};
    char canFR_RPS[CANDATANUM_RPS]= {0};
    char canFL_RPS[CANDATANUM_RPS]= {0};
    char canAPS_P[CANDATANUM_APS]= {0};
    char canAPS_S[CANDATANUM_APS]= {0};

    unsigned int canWriteSucceeded = 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();

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

    wait(3);

    while(1) {

        canWriteSucceeded = 0;

        getCurrentErrCount(&eCounter);

        timer.reset();

        driveTVD(0, isPressedRTD());

        brakeSignal.write(isBrakeOn());

        writeCanBuff((int)(getVelocity() / LSB_MOTORSPEED),canVehicleSpeed,CANDATANUM_VEHICLESPEED);
        writeCanBuff(getSteerAngle()+127, canSteer, CANDATANUM_STEERING);
        writeCanBuff(calcRequestTorque(), canRequestTorque, CANDATANUM_REQTRQ);
        writeCanBuff(getMotorTorque(RR_MOTOR), canMotorTorqueR, CANDATANUM_MOTORTRQ);
        writeCanBuff(getMotorTorque(RL_MOTOR), canMotorTorqueL, CANDATANUM_MOTORTRQ);

        writeCanBuff(getWheelRps(FR_WHEEL) / LSB_RPS, canFR_RPS, CANDATANUM_RPS);
        writeCanBuff(getWheelRps(FL_WHEEL) / LSB_RPS, canFL_RPS, CANDATANUM_RPS);
        writeCanBuff(getWheelRps(RR_MOTOR) / LSB_RPS, canRR_RPS, CANDATANUM_RPS);
        writeCanBuff(getWheelRps(RL_MOTOR) / LSB_RPS, canRL_RPS, CANDATANUM_RPS);

        writeCanBuff(getRawSensor(APS_PRIMARY), canAPS_P, CANDATANUM_APS);
        writeCanBuff(getRawSensor(APS_SECONDARY), canAPS_S, CANDATANUM_APS);
        
//        printf("%d %d %d %d %d\r\n", getMotorTorque(RR_MOTOR), getMotorTorque(RL_MOTOR), calcRequestTorque(), 0, 4000);

//        writeCanBuff((int)((distributeTorque(M_PI * getSteerAngle() / 127.0f, getVelocity())*limitTorqueDistribution()) / 2.0f), canTorqueDistribution, CANDATANUM_DISTRQ);

        do {
            if(0 == (canWriteSucceeded & 0b1))
                canWriteSucceeded |= (can.write(CANMessage(CANID_VEHICLESPEED, canVehicleSpeed, CANDATANUM_VEHICLESPEED)) << 0);
            if(0 == (canWriteSucceeded & 0b10))
                canWriteSucceeded |= (can.write(CANMessage(CANID_STEERING, canSteer, CANDATANUM_STEERING)) << 1);
            if(0 == (canWriteSucceeded & 0b100))
                canWriteSucceeded |= (can.write(CANMessage(CANID_REQTRQ, canRequestTorque, CANDATANUM_REQTRQ)) << 2);
            if(0 == (canWriteSucceeded & 0b1000))
                canWriteSucceeded |= (can.write(CANMessage(CANID_RIGHT_TRQ, canMotorTorqueR, CANDATANUM_MOTORTRQ)) << 3);
            if(0 == (canWriteSucceeded & 0b10000))
                canWriteSucceeded |= (can.write(CANMessage(CANID_LEFT_TRQ, canMotorTorqueL, CANDATANUM_MOTORTRQ)) << 4);

            if(0 == (canWriteSucceeded & 0b100000))
                canWriteSucceeded |= (can.write(CANMessage(CANID_FR_RPS, canFR_RPS, CANDATANUM_RPS)) << 5);
            if(0 == (canWriteSucceeded & 0b1000000))
                canWriteSucceeded |= (can.write(CANMessage(CANID_FL_RPS, canFL_RPS, CANDATANUM_RPS)) << 6);
            if(0 == (canWriteSucceeded & 0b10000000))
                canWriteSucceeded |= (can.write(CANMessage(CANID_RR_RPS, canRR_RPS, CANDATANUM_RPS)) << 7);
            if(0 == (canWriteSucceeded & 0b100000000))
                canWriteSucceeded |= (can.write(CANMessage(CANID_RL_RPS, canRL_RPS, CANDATANUM_RPS)) << 8);
            if(0 == (canWriteSucceeded & 0b1000000000))
                canWriteSucceeded |= (can.write(CANMessage(CANID_DISTRQ, canTorqueDistribution, CANDATANUM_DISTRQ)) << 9);

            if(0 == (canWriteSucceeded & 0b10000000000))
                canWriteSucceeded |= (can.write(CANMessage(CANID_APS_P, canAPS_P, CANDATANUM_APS)) << 10);
            if(0 == (canWriteSucceeded & 0b100000000000))
                canWriteSucceeded |= (can.write(CANMessage(CANID_APS_S, canAPS_S, CANDATANUM_APS)) << 11);
            //printf("%d\r\n", canWriteSucceeded);
        } while(timer.read() < CONTROL_CYCLE_S);      //制御周期管理 関数内処理時間より短い時間の制御周期の設定は禁止
        //printf("\r\n");

        //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,RPS_R:%1.2f, RPS_L%1.2f\r", 3.3f/65535 * getRawSensor(APS_PRIMARY), 3.3f/65535 * getRawSensor(APS_SECONDARY), 3.3f/65535 * getRawSensor(BRAKE), getRPS(RIGHT)*LSB_MOTORSPEED*GEAR_RATIO*60.0f, getRPS(LEFT)*LSB_MOTORSPEED*GEAR_RATIO*60.0f);
    }
}