File content as of revision 30:00041540e23c:

//翼端can program
#include "mbed.h"
#include "MPU6050.h"
#include "INA226.hpp"
#include "XBusServo.h"

#define TO_SEND_DATAS_NUM 7
#define INIT_SERVO_PERIOD_MS 20
#define WAIT_LOOP_TIME 0.02
#define CONTROL_VALUES_NUM sizeof(float) + 1
#define TO_SEND_CAN_ID 100
#define SEND_DATAS_LOOP_TIME 0.1
#define RECEIVE_DATAS_LOOP_TIME 0.05

#define ERURON_MOVE_DEG_INI_R 30
#define DRUG_MOVE_DEG_INI_R 76
#define ERURON_TRIM_INI_R 97
#define DRUG_TRIM_INI_R 33

#define ERURON_MOVE_DEG_INI_L -30
#define DRUG_MOVE_DEG_INI_L -80
#define ERURON_TRIM_INI_L 113  
#define DRUG_TRIM_INI_L 110

#define kMaxServoNum        1       // 1 - 50
#define kMaxServoPause     (sizeof(motionData) / sizeof(pauseRec))
#define kMotionInterval     10      // flame / sec
#define kMotionMinMark      0x1249
#define kMotionEndMark      0xED86

CAN can(p30,p29);
CANMessage recmsg;
Serial pc(USBTX,USBRX);
MPU6050 mpu(p9,p10);
I2C ina226_i2c(p28,p27);
INA226 VCmonitor(ina226_i2c);
PwmOut drugServo(p22);
DigitalOut led1(LED1);
AnalogIn drugAna(p20);
AnalogIn eruronAna(p19);
DigitalIn LRstatePin(p11);
DigitalIn setTrimPin(p12);
DigitalIn EDstatePin(p14);
DigitalIn setMaxDegPin(p15);
DigitalOut debugLED(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);
Ticker sendDatasTicker;
//Ticker toStringTicker;
Ticker receiveDatasTicker;

char toSendDatas[TO_SEND_DATAS_NUM];
char controlValues[CONTROL_VALUES_NUM];//0~3:eruruon,4:drug
char floatvalues[sizeof(float)];
float eruronTrim;
float drugTrim;
float eruronMoveDeg;
float drugMoveDeg;
float eruronfloat;
unsigned short ina_val;
double V,C;
bool SERVO_FLAG;
bool INA_FLAG;
bool MPU_FLAG;

char gyro_c[6] = {0,0,0,0,0,0};

void toString();
void receiveDatas();
void WriteServo();

static const uint8_t        servoChannel = 0x01;

XBusServo gXBus(p13, NC, NC, kMaxServoNum);
Ticker gTimer;

bool servoInit()
{
    drugServo.period_ms(INIT_SERVO_PERIOD_MS);
    return true;
}

void sendDatas()
{
    if(can.write(CANMessage(TO_SEND_CAN_ID, toSendDatas, TO_SEND_DATAS_NUM))) {
    }
}

bool inaInit()
{
    if(!VCmonitor.isExist()) {
        pc.printf("VCmonitor NOT FOUND\n");
        return false;
    }
    ina_val = 0;
    if(VCmonitor.rawRead(0x00,&ina_val) != 0) {
        pc.printf("VCmonitor READ ERROR\n");
        return false;
    }
    VCmonitor.setCurrentCalibration();
    return true;
}

XBusError initXBus()
{
    XBusError result;
    result = gXBus.start();
    if (result != kXBusError_NoError) {
        gXBus.stop();
        return result;
    }
    result = gXBus.addServo(servoChannel, kXbusServoNeutral);
    if (result != kXBusError_NoError) {
        gXBus.stop();
        return result;
    }
    return kXBusError_NoError;
}

void init()
{
    if(!LRstatePin) {
        eruronTrim = ERURON_TRIM_INI_L;
        drugTrim = DRUG_TRIM_INI_L;
        eruronMoveDeg = ERURON_MOVE_DEG_INI_L;
        drugMoveDeg = DRUG_MOVE_DEG_INI_L;
    } else {
        eruronTrim = ERURON_TRIM_INI_R;
        drugTrim = DRUG_TRIM_INI_R;
        eruronMoveDeg = ERURON_MOVE_DEG_INI_R;
        drugMoveDeg =DRUG_MOVE_DEG_INI_R;
    }
    SERVO_FLAG = servoInit();
    MPU_FLAG = mpu.testConnection();
    INA_FLAG = inaInit();
    sendDatasTicker.attach(&sendDatas,SEND_DATAS_LOOP_TIME);
    // toStringTicker.attach(&toString,0.5);
    receiveDatasTicker.attach(&receiveDatas,RECEIVE_DATAS_LOOP_TIME);

    initXBus();
}

void updateDatas()
{
    if(INA_FLAG) {
        int tmp = VCmonitor.getVoltage(&V);
        tmp = VCmonitor.getCurrent(&C);
    }
    if(MPU_FLAG) {
        mpu.read(MPU6050_GYRO_XOUT_H_REG, gyro_c, 6);
    }
    for(int i = 0; i < TO_SEND_DATAS_NUM - 1; i++) {
        toSendDatas[i] = gyro_c[i];
    }
//    toSendDatas[TO_SEND_DATAS_NUM - 1] = (char)(V/100);
    toSendDatas[TO_SEND_DATAS_NUM - 1] = (char)77;
}

void receiveDatas()
{
    if(can.read(recmsg)) {
        for(int i = 0; i < CONTROL_VALUES_NUM; i++) {
            controlValues[i] = recmsg.data[i];
            if(i<CONTROL_VALUES_NUM-1) floatvalues[i] = controlValues[i];
        }
        eruronfloat = *(const float *)floatvalues;
        led1 = !led1;
    }
}

double calcPulse(int deg)
{
    return (0.0006+(deg/180.0)*(0.00235-0.00045));
}

void XbusIntervalHandler()
{
    uint16_t value;
    uint16_t diff  = kMotionEndMark - kMotionMinMark;
    value = (uint16_t)(diff * (eruronTrim  + eruronMoveDeg * (eruronfloat-1)) + kMotionMinMark);
    gXBus.setServo(servoChannel, value);
    gXBus.sendChannelDataPacket();
}

void WriteServo()
{
    pc.printf("%d",controlValues[4]);
    drugServo.pulsewidth(calcPulse( drugTrim + drugMoveDeg * controlValues[4]));
}

void setTrim()
{
    debugLED =  1;
    if(EDstatePin) {
        eruronTrim = eruronAna.read();
    } else {
        drugTrim = drugAna.read()*180;
        drugServo.pulsewidth(calcPulse(drugTrim));
    }
    //pc.printf("eruronTrim:%f    drugTrim:%f\n\r",eruronTrim,drugTrim);
    pc.printf("eruronTrim:%f    drugTrim:%f    ",eruronTrim,drugTrim);
    pc.printf("eMD:%f   dMD:%f    ef:%f\n\r",eruronMoveDeg,drugMoveDeg,eruronfloat);
}

void setMaxDeg()
{
    led4 = 1;
    float eruronTemp = eruronAna.read();
    float drugTemp = drugAna.read()*180;
    if(EDstatePin) {
        eruronMoveDeg = eruronTemp-eruronTrim;
    } else {
        drugServo.pulsewidth(calcPulse(drugTemp));
        drugMoveDeg = drugTemp-drugTrim;
    }
    // pc.printf("eMD:%f   dMD:%f\n\r",eruronMoveDeg,drugMoveDeg);
    pc.printf("eruronTrim:%f    drugTrim:%f    ",eruronTrim,drugTrim);
    pc.printf("eMD:%f   dMD:%f    ef:%f\n\r",eruronMoveDeg,drugMoveDeg,eruronfloat);
    wait_us(10);
}

int main()
{
    init();
    XBusError result;
    
    setTrimPin.mode(PullDown);
    setMaxDegPin.mode(PullDown);
    EDstatePin.mode(PullDown);
    LRstatePin.mode(PullDown);

    // start motion
    gTimer.attach_us(&XbusIntervalHandler, 1000000 / kMotionInterval);

    while(1) {
        while(setTrimPin) {
            setTrim();
        }
        while (setMaxDegPin) {
            setMaxDeg();
        }
        //  pc.printf("eT:%f\n\r",eruronTrim);
        pc.printf("eruronTrim:%f    drugTrim:%f    ",eruronTrim,drugTrim);
        pc.printf("eMD:%f   dMD:%f    ef:%f\n\r",eruronMoveDeg,drugMoveDeg,eruronfloat);
        led4 = 0;

        debugLED = 0;
        //receiveDatas();
//        sendDatas();
        WriteServo();
        updateDatas();
//      pc.printf("%6d,%6d,%6d\n\r",gyro[0],gyro[1],gyro[2]);
        led3 = !led3;
        wait(WAIT_LOOP_TIME);
    }
}