File content as of revision 2:7fcb4f970a02:

//翼端can program
#include "mbed.h"
#include "ADXL345_I2C.h"
#include "INA226.hpp"
#define TO_SEND_DATAS_NUM 5
#define INIT_SERVO_PERIOD_MS 20
#define WAIT_LOOP_TIME 1
#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);
DigitalOut led1(LED1);
AnalogIn a(p20);
AnalogIn b(p19);

char toSendDatas[TO_SEND_DATAS_NUM];
char controlValues[CONTROL_VALUES_NUM];//0:eruruon,1:drug

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 < CONTROL_VALUES_NUM; i++){
            //controlValues[i] = recmsg.data[i];
            controlValues[i] = 1 - controlValues[i];
            //pc.printf("%d:%d    ",i,recmsg.data[i]);
        }
        pc.printf("\n\r");
        led1 = !led1;
    }
}

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("%d:%d, ",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");   
}

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

void WriteServo(){
    //if(controlValues[0] == 1){
//        servo1.pulsewidth(calcPulse(90));
//    } 
//    else{
//        servo1.pulsewidth(calcPulse(45));
//    }
//    if(controlValues[1] == 1){
//        servo1.pulsewidth(calcPulse(90));
//    } 
//    else{
//        servo1.pulsewidth(calcPulse(45));
//    }
    servo1.pulsewidth(calcPulse(a.read()*170));
    servo2.pulsewidth(calcPulse(b.read()*170));
    pc.printf("%f", a.read());
}

int main(){
    init();
    while(1){
        receiveDatas();
        updateDatas();
        WriteServo();
        toString();
        sendDatas();
        wait(WAIT_LOOP_TIME);
    }
}