File content as of revision 0:c7d5470d50e9:

#include "mbed.h"
#include "ADXL345_I2C.h"
#include "INA226.hpp"
    
#define BUFFER 30

Serial rs485(p13,p14);
DigitalOut Receiver(p5);
ADXL345_I2C accelerometer(p9, p10);
PwmOut servo1(p21);
PwmOut servo2(p22);
I2C i2c_Ina(p28,p27);
INA226 Ina226(i2c_Ina);
 
//ADXL345 accelerometer(p5, p6, p7, p8);
ADXL345_I2C accelerometer(p9, p10);
Serial pc(USBTX, USBRX);
 
void ADXL345(int *x)
{
    x[3] = {0, 0, 0};
     
   // pc.printf("Starting ADXL345 test...\n");
    //pc.printf("Device ID is: 0x%02x\n", accelerometer.getDevId());
 
    //Go into standby mode to configure the device.
    accelerometer.setPowerControl(0x00);
 
    //Full resolution, +/-16g, 4mg/LSB.
    accelerometer.setDataFormatControl(0x0B);
     
    //3.2kHz data rate.
    accelerometer.setDataRate(ADXL345_3200HZ);
 
    //Measurement mode.
    accelerometer.setPowerControl(0x08);

        accelerometer.getOutput(x);
         
        //13-bit, sign extended values.
        //pc.printf("%6i, %6i, %6i\n\r", (int16_t)readings[0], (int16_t)readings[1], (int16_t)readings[2]);
     
}

int INA226(double *Voltage,double *Current){
    while(1){
      if(Ina226.isExist() == 0){
        Ina226.getVoltage(Voltage);
        Ina226.getCurrent(Current);
        }
    //pc.printf("V : %lf, C : %lf \n\r",Voltage,Current);
    
    }
    return 0;
    }



int counter = 0;
int servo_deg1 = 0;
int servo_deg2 = 0;

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

void rs485_rx()
{
    int acc[3];
    signed char rec_data = rs485.getc();
    switch(rec_data) {
        case 'A':
            servo_deg1 = rs485.getc();
           // pc.printf("counter1:%d\n\r",servo_deg1);
            break;
        case 'B':
            servo_deg2 = rs485.getc();
            //pc.printf("counter2:%d\n\r",servo_deg2);
            break;
        case 'P':
        INA226_check(Voltage,Current);
        ADXL345(acc);
    //counter++;
   // if(counter > 1000){
   // counter = 0;
   // }
            Receiver = 1;
            wait_ms(3);
           // if(counter % 2 == 0){
           rs485.putc('X');
          // pc.printf("X");
            rs485.putc((signed char)acc[0]);
        //pc.printf("%5d",(signed char)acc[0]);
            rs485.putc('Y');
            //pc.printf("Y");
            rs485.putc((signed char)acc[1]);
           // pc.printf("%5d",(signed char)acc[1]);
            rs485.putc('Z');
            //pc.printf("Z");
            rs485.putc((signed char)acc[2]);
            //}
           // else{
           // pc.printf("%5d",(signed char)acc[2]);
            rs485.putc('V');
            pc.printf("V");
            rs485.putc((signed char)Voltage);
            pc.printf("%5d",(signed char)Voltage);
            rs485.putc('C');
            pc.printf("C");
            rs485.putc((signed char)Current);
            pc.printf("%5d\n\r",(signed char)Current);
            //}
            //pc.printf("\n\r");
//            rs485.printf("X%iY%iZ%i\n\r",(short)acc[0],(short)acc[1],(short)acc[2]);
            wait_ms(15);
            Receiver = 0;
            //pc.printf("x:%d,y:%d,z:%d\n\r",(signed char)acc[0],(signed char)acc[1],(signed char)acc[2]);
            break;
        default:
            wait_us(5);
          //  pc.printf("%d\n\r",rec_data);
    }
}

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


int main()
{
    init();
    while(1) {
        servo1.pulsewidth(0.00093 + (servo_deg1 / 180.0) * (0.00235 - 0.00077));
        servo2.pulsewidth(0.00093 + (servo_deg2 / 180.0) * (0.00235 - 0.00077));
        wait_ms(10);
    }
}