File content as of revision 0:149c2a43ef70:

#include "mbed.h"
#include <stdio.h>
#include "ADXL345.h"
#include "ITG3200.h"
#include "HMC5883L.h"
#include "BufferSerial.h"

#define ID_GATHOTKACA 106
#define itg3200_address (0xD0)
#define itg3200_reg_xmsb (0x1D)
#define itg3200_reg_who_am_I (0x00)
#define itg3200_reg_smplrt_div (0x15)
#define itg3200_reg_dlpf_fs (0x16)
#define DLPF_CFG_0 (1<<0)
#define DLPF_CFG_1 (1<<1)
#define DLPF_CFG_2 (1<<2)
#define DLPF_FS_SEL_0 (1<<3)
#define DLPF_FS_SEL_1 (1<<4)

Serial pc(USBTX,USBRX);
BufferSerial kirim(USBTX,USBRX,1);
ADXL345 acc(p28,p27);
ITG3200 gyro(p28,p27);
HMC5883L magneto(p28,p27);
I2C i2c(p28,p27);

int bacaXYZ[3]={0,0,0};
char gyroX,gyroY,gyroZ;
char accX,accY,accZ;
char mx,my,mz;
char baca;
short rawAccX, rawAccY, rawAccZ, rawGyroX, rawGyroY, rawGyroZ, rawMagX, rawMagY, rawMagZ;
unsigned short sendAccX, sendAccY, sendAccZ, sendGyroX, sendGyroY, sendGyroZ, sendMagX, sendMagY, sendMagZ;

void bin_dec_conv(unsigned short data)
{
    unsigned short hasil;
//    unsigned char kirim[16];

    hasil = data%100;
    pc.printf("%d%d%d",(data/100),(hasil/10),(hasil%10));

}

void baca_sensor()
{
        acc.getOutput(bacaXYZ);
        accX=(signed short)bacaXYZ[0];
        accY=(signed short)bacaXYZ[1];
        accZ=(signed short)bacaXYZ[2];
        gyroX=gyro.getGyroX();
        gyroY=gyro.getGyroY();
        gyroZ=gyro.getGyroZ();
        mx=magneto.getMx();
        my=magneto.getMy();
        mz=magneto.getMz();
        rawAccX=-1*accY;
        rawAccY=accX;
        rawAccZ=accZ;
        rawGyroX=-1*gyroY;
        rawGyroY=gyroX;
        rawGyroZ=gyroZ;
        rawMagX=mx;
        rawMagY=my;
        rawMagZ=mz;
//        rawAccX=accX;
//        rawAccY=accY;
//        rawAccZ=accZ;
//        rawMagX=mx;
//        rawMagY=my;
//        rawMagZ=mz;
//        rawGyroX=gyroY;
//        rawGyroY=gyroX;
//        rawGyroZ=gyroZ;
}

void raw_to_send()
{
    sendAccX = (unsigned short) (rawAccX + 512);//if(sendAccX>999) sendAccX=999;
    sendAccY = (unsigned short) (rawAccY + 512);//if(sendAccY>999) sendAccY=999;
    sendAccZ = (unsigned short) (rawAccZ + 512);//if(sendAccZ>999) sendAccZ=999;
    sendGyroX = (unsigned short)((rawGyroX*0.06956+2400)*0.2083);//if(sendGyroX>999) sendGyroX=999;
    sendGyroY = (unsigned short)((rawGyroY*0.06956+2400)*0.2083);//if(sendGyroY>999) sendGyroY=999;
    sendGyroZ = (unsigned short)((rawGyroZ*0.06956+2400)*0.2083);//if(sendGyroZ>999) sendGyroZ=999;
    sendMagX = (unsigned short)((rawMagX+2048)*0.25);//if(sendMagX>999) sendMagX=999;
    sendMagY = (unsigned short)((rawMagY+2048)*0.25);//if(sendMagY>999) sendMagY=999;
    sendMagZ = (unsigned short)((rawMagZ+2048)*0.25);//if(sendMagZ>999) sendMagZ=999;
}

int main()
{
//    pc.printf("start");
    pc.baud(57600);
    acc.setPowerControl(0);
    acc.setPowerControl(16);
    acc.setPowerControl(8);
    acc.setDataFormatControl(0x03);
    //acc.setDataRate(ADXL345_3200HZ);
    gyro.setLpBandwidth(DLPF_FS_SEL_0|DLPF_FS_SEL_1|DLPF_CFG_0);
    gyro.setSampleRateDivider(9);
    magneto.setDefault();
//    pc.printf("mulai");
    while(1)
    {
//        acc.getOutput(bacaXYZ);
//        accX=(signed short)bacaXYZ[0];
//        accY=(signed short)bacaXYZ[1];
//        accZ=(signed short)bacaXYZ[2];
//        gyroX=gyro.getGyroX();
//        gyroY=gyro.getGyroY();
//        gyroZ=gyro.getGyroZ();
//        mx=magneto.getMx();
//        my=magneto.getMy();
//        mz=magneto.getMz();
       // pc.printf("%d %d %d|%d %d %d|%d %d %d\n",accX,accY,accZ,gyroX,gyroY,gyroZ,mx,my,mz);
        wait_ms(200);
          if(kirim.readable())
          {
              baca=kirim.getc();
              if(baca=='s')
              baca='0';
              while(1)
              {
                  //pc.printf("ivan\n");
                  baca_sensor();
                  raw_to_send();

                  pc.putc(0x0D);
                  bin_dec_conv(ID_GATHOTKACA);
                  pc.putc(0x20);
                  
                  
                  bin_dec_conv(sendAccX);
                  pc.putc(0x20);
                  
                  
                  bin_dec_conv(sendAccY);
                  pc.putc(0x20);
                  
                  
                  bin_dec_conv(sendAccZ);
                  pc.putc(0x20);
                  
                  
                  bin_dec_conv(sendGyroX);
                  pc.putc(0x20);
                  
                  
                  bin_dec_conv(sendGyroY);
                  pc.putc(0x20);
                  
                  
                  bin_dec_conv(sendGyroZ);
                  pc.putc(0x20);
                  
                  
                  bin_dec_conv(sendMagX);
                  pc.putc(0x20);
                  
                  
                  bin_dec_conv(sendMagY);
                  pc.putc(0x20);
                  
                  
                  bin_dec_conv(sendMagZ);
                  
                  wait_ms(75); 
                  
                  baca=kirim.getc();
                  if(baca=='x')
                  {
                      baca='0';
                      break;
                  }
              }
          }

        
    }
}