File content as of revision 3:2bdb60bbc665:

//翼端操舵プログラム
#include "mbed.h"
#include "ADXL345_I2C.h"
#include "INA226.hpp"
#define BUFFER 30

Serial rs485(p13,p14);
Serial pc(USBTX,USBRX);
DigitalOut Receiver(p5);
ADXL345_I2C accelerometer(p9, p10);
I2C ina226_i2c(p28,p27);
INA226 VCmonitor(ina226_i2c);
PwmOut servo1(p21);
PwmOut servo2(p22);

int counter = 0;
int eruron_deg = 0;
int drug_deg = 0;
unsigned short ina_val;
double V,C;

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

void rs485_rx()
{
    signed char rec_data = rs485.getc();
    switch(rec_data) {
        case 'A':
            eruron_deg = rs485.getc();
            break;
        case 'B':
            drug_deg = rs485.getc();
            break;
        case 'C':
            Receiver = 1;
            wait_ms(1);
            rs485.putc('X');
            rs485.putc((signed char)acc[0]);
            rs485.putc('Y');
            rs485.putc((signed char)acc[1]);
            rs485.putc('Z');
            rs485.putc((signed char)acc[2]);
            rs485.putc('V');
            rs485.putc((signed char)V);
            rs485.putc('I');
            rs485.putc((signed char)C);
            wait_ms(2);
            Receiver = 0;
            break;
        default:
            wait_us(5);
    }
}

void init()
{
    Receiver = 0;
    pc.printf("Receiver\n\r");

    rs485.baud(38400);
    rs485.attach(rs485_rx, Serial::RxIrq);

    servo1.period_ms(20);
    servo2.period_ms(20);

    accelerometer.setPowerControl(0x00);
    accelerometer.setDataFormatControl(0x0B);
    accelerometer.setDataRate(ADXL345_3200HZ);
    accelerometer.setPowerControl(0x08);

    if(!VCmonitor.isExist()){
        pc.printf("VCmonitor NOT FOUND\n");
    }
    ina_val = 0;
    if(VCmonitor.rawRead(0x00,&ina_val) != 0){
        pc.printf("VCmonitor READ ERROR\n");
        while(1){}
    }
    VCmonitor.setCurrentCalibration();
}

void updateDatas(){
    accelerometer.getOutput(acc);
    int tmp = VCmonitor.getVoltage(&V);
    tmp = VCmonitor.getCurrent(&C);
}

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

void WriteServo(){
    servo1.pulsewidth(calcPulse(eruron_deg));
    servo2.pulsewidth(calcPulse(drug_deg));    
}

void toStringDatas(){
    pc.printf("acc[0]:%i   acc[1]:%i   acc[2]:%i\r\n",acc[0],acc[1],acc[2]);
    pc.printf("V:%i   C:%i\r\n",V,C);
    pc.printf("\r\n");
}

int main()
{
    init();
    while(1) {
        updateDatas();
        WriteServo();
        toStringDatas();
        wait_ms(10);
    }
}