File content as of revision 5:7459a428e16e:

//翼端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 0.2
#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(p22);
PwmOut servo2(p23);// eruron
DigitalOut led1(LED1);
AnalogIn a(p20);
AnalogIn b(p19);
DigialIn switch1(p15);

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 neutral_a = 0;
int neutral_b = 0;

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];
        //pc.printf("%d\t",acc[i]);
    }
    //pc.printf("\n");
    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];
         //   pc.printf("%d:%d    ",i,recmsg.data[i]);
        }
        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, ",controlValues[i]);
//        
//    }
    //pc.printf("\n\r");
//    pc.printf("%f",a.read());
    //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(int a,int b){
    if(a >= 45 && a <= 90 && b >= 45 && b <= 90){
    if(controlValues[0] == (char)1){
      servo1.Pulsewidth(calcPulse(90));
    else{
        servo1.pulsewidth(calcPulse(a));
    }
    if(controlValues[1] == (char)1){
        servo2.pulsewidth(calcPulse(90));
    } 
    else{
        servo2.pulsewidth(calcPulse(b));
    }
    }
    //servo1.pulsewidth(calcPulse(a.read()*170));
//    servo2.pulsewidth(calcPulse(b.read()*170));
    //pc.printf("%f", a.read());
}


//初期迎角を変える
void Debug(){
    if(switch1 ==  1){
    neutral_a = (int)(a * 180);
    neutral_b = (int)(b * 180);
    WriteServo(neutral_a);
    WriteServo(neutral_b);
    }
    }
    
int main(){
    init();
    while(1){
        receiveDatas();
        WriteServo(neutral_a,neutral_b);
           updateDatas();
        toString();
        sendDatas();
        Debug();
        wait(WAIT_LOOP_TIME);
    }
}