File content as of revision 0:e052602db102:

//翼端can program
#include "mbed.h"
#include "ADXL345_I2C.h"
#include "INA226.hpp"
#define TO_SEND_DATAS_NUM 5
#define INIT_SERVO_PERIOD_MS 20000
#define WAIT_LOOP_TIME_MS 10
#define CONTROL_VALUES_NUM 2
#define TO_SEND_CAN_ID 100
CAN can(p30,p29);
CANMessage recmsg;
Serial pc(USBTX,USBRX);
ADXL345_I2C accelerometer(p9, p10);
I2C ina226_i2c(p28,p27);
INA226 VCmonitor(ina226_i2c);
PwmOut servo1(p21);
PwmOut servo2(p22);

char toSendDatas[TO_SEND_DATAS_NUM];
char controlValues[CONTROL_VALUES_NUM];

int counter = 0;
int eruron_deg = 0;
int drug_deg = 0;
unsigned short ina_val;
double V,C;
bool SERVO_FLAG;
bool ADXL_FLAG;
bool INA_FLAG;

int acc[3] = {0,0,0};

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

bool adxlInit(){
    accelerometer.setPowerControl(0x00);
    accelerometer.setDataFormatControl(0x0B);
    accelerometer.setDataRate(ADXL345_3200HZ);
    accelerometer.setPowerControl(0x08);
    return true;
}

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

void init(){
    pc.printf("Receiver\n\r");
    SERVO_FLAG = servoInit();
    ADXL_FLAG = adxlInit();
    INA_FLAG = inaInit();
}

void updateDatas(){
    if(ADXL_FLAG){
        accelerometer.getOutput(acc);
    }
    if(INA_FLAG){
        int tmp = VCmonitor.getVoltage(&V);
        tmp = VCmonitor.getCurrent(&C);
    }
    for(int i = 0; i < 3; i++){
        toSendDatas[i] = acc[i];
    }
    toSendDatas[3] = (char)(V/100);
    toSendDatas[4] = (char)(C/0100);
}

void receiveDatas(){
    if(can.read(recmsg)){
        for(int i = 0; i < recmsg.len; i++){
            controlValues[i] = recmsg.data[i];
        }
    }
}

void toString(){
    //for(int i = 0; i < TO_SEND_DATAS_NUM; i++){
        pc.printf("%i:",toSendDatas[3]);
        pc.printf("%i:",toSendDatas[4]);
        pc.printf("%f:",V);
        pc.printf("%f:",C);
    //}
    //for(int i = 0; i <CONTROL_VALUES_NUM; i++){
//        pc.printf("%i:",controlValues[i]);
//    }
//    pc.printf("%d",INA_FLAG);
    pc.printf("\n\r");
}

void sendDatas(){
    if(can.write(CANMessage(TO_SEND_CAN_ID, toSendDatas, TO_SEND_DATAS_NUM)))
        pc.printf("resend suc\n\r");   
}

void 

int main(){
    init();
    while(1){
        receiveDatas();
        updateDatas();
        //WriteServo();
        toString();
        sendDatas();
        wait_ms(WAIT_LOOP_TIME_MS);
    }
}