File content as of revision 0:5af6fad57e1a:

#include "mbed.h"
#include <stdio.h>
#include "BufferSerial.h"

Serial pc(USBTX,USBRX);
I2C i2c(p28,p27);
BufferSerial kirim(USBTX,USBRX,1);

#define ACCEL 0xA6
#define MAGNET 0x3C
#define ID_GATHOTKACA 106

//initialize accel
#define ADXL345_AXIS_X_SCALE 4

#define adxl345_address (0xA6>>1)
#define adxl345_reg_data_format (0x31)
#define adxl345_reg_pwr_ctl (0x2D)
#define adxl345_reg_xlsb (0x32)

//initialize gyro
#define ITG3200_R 0x69
#define ITG3200_W 0x68
#define WHO 0x00
#define SMPL 0x15
#define INT_C 0x17
#define TMP_H 0x1B
#define GX_H 0x1D
#define GY_H 0x1F
#define GZ_H 0x21
#define PWR_M 0x3E
#define DLPF 0x16
#define INT_S 0x1A
#define TMP_L 0x1C
#define GX_L 0x1E
#define GY_L 0x20
#define GZ_L 0x22

#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)


//initialize magneto
#define HMC5843_W 0x3C
#define HMC5843_R 0x3D
#define PI 3.14159265

#define GRAVITY 256.0f // "1G reference" used for DCM filter and accelerometer calibration

    #define MAGN_X_MAX 465
    #define MAGN_Y_MAX 475
    #define MAGN_Z_MAX 600

    #define MAGN_X_MIN -561
    #define MAGN_Y_MIN -547
    #define MAGN_Z_MIN -379
    #define MAGN_X_OFFSET ((MAGN_X_MIN + MAGN_X_MAX) / 2.0f)
    #define MAGN_Y_OFFSET ((MAGN_Y_MIN + MAGN_Y_MAX) / 2.0f)
    #define MAGN_Z_OFFSET ((MAGN_Z_MIN + MAGN_Z_MAX) / 2.0f)

    #define MAGN_X_SCALE (100.0f / (MAGN_X_MAX - MAGN_X_OFFSET))
    #define MAGN_Y_SCALE (100.0f / (MAGN_Y_MAX - MAGN_Y_OFFSET))
    #define MAGN_Z_SCALE (100.0f / (MAGN_Z_MAX - MAGN_Z_OFFSET))

short rawAccX, rawAccY, rawAccZ, rawGyroX, rawGyroY, rawGyroZ, rawMagX, rawMagY, rawMagZ;
unsigned short sendAccX, sendAccY, sendAccZ, sendGyroX, sendGyroY, sendGyroZ, sendMagX, sendMagY, sendMagZ;
char baca;

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 tulis_i2c(unsigned char devadd, unsigned char regadd, unsigned char data)
{
    i2c.start();
    i2c.write(devadd);
    i2c.write(regadd);
    i2c.write(data);
    i2c.stop();
    
}
unsigned char baca_i2c(unsigned char devadd, unsigned char regadd)
{
    unsigned char data;
    i2c.start();
    i2c.write(devadd);
    i2c.write(regadd);
    i2c.start();
    i2c.write(devadd|1);
    data=i2c.read(0);
    i2c.stop();
    return data;
}

int baca_adxl()
{
    unsigned char acc_x_msb, acc_x_lsb;
    unsigned char acc_y_msb, acc_y_lsb;
    unsigned char acc_z_msb, acc_z_lsb;
    
    tulis_i2c(ACCEL, adxl345_reg_pwr_ctl, 0);
    tulis_i2c(ACCEL, adxl345_reg_pwr_ctl, 16);
    tulis_i2c(ACCEL, adxl345_reg_pwr_ctl, 8);
    
    tulis_i2c(ACCEL, adxl345_reg_data_format, 0x03);
    
     acc_x_lsb = baca_i2c(ACCEL,0x32);
     acc_x_msb = baca_i2c(ACCEL,0x33);
     acc_y_lsb = baca_i2c(ACCEL,0x34);
     acc_y_msb = baca_i2c(ACCEL,0x35);
     acc_z_lsb = baca_i2c(ACCEL,0x36);
     acc_z_msb = baca_i2c(ACCEL,0x37);
    
        float acc_x = (signed short)((signed short)(acc_x_msb<<8) | acc_x_lsb);//*ADXL345_AXIS_X_SCALE/1000.0f;
        float acc_y = 1*(signed short)((signed short)(acc_y_msb<<8) | acc_y_lsb);//*ADXL345_AXIS_X_SCALE/1000.0f;
        float acc_z = (signed short)((signed short)(acc_z_msb<<8) | acc_z_lsb);//*ADXL345_AXIS_X_SCALE/1000.0f
        
        rawAccX=-1*acc_y;
        rawAccY=acc_x;
        rawAccZ=acc_z;
}

void baca_itg()
{
    tulis_i2c(itg3200_address, itg3200_reg_dlpf_fs, (DLPF_FS_SEL_0|DLPF_FS_SEL_1|DLPF_CFG_0));
    tulis_i2c(itg3200_address, itg3200_reg_smplrt_div, 9);
    char xh,xl,yh,yl,zh,zl;
    short x,y,z;
        xl = baca_i2c(itg3200_address,0x1E);
        xh = baca_i2c(itg3200_address,0x1D);
        yh = baca_i2c(itg3200_address,0x1F);
        yl = baca_i2c(itg3200_address,0x20);
        zh = baca_i2c(itg3200_address,0x21);
        zl = baca_i2c(itg3200_address,0x22);
        
        x=1*(signed short)((signed short)(xl | (xh<<8)));//*0.0695652174;
        y=1*(signed short)((signed short)(yl | (yh<<8)));//*0.0695652174;
        z=1*(signed short)((signed short)(zl | (zh<<8)));
        
        rawGyroX=-1*y;
        rawGyroY=x;
        rawGyroZ=z;
}

void i2c_tulis_hmc(unsigned char address, unsigned char data)
{
i2c.start();
i2c.write(HMC5843_W); // write 0x
i2c.write(address); // write register address
i2c.write(data); // write register address
i2c.stop();
//__delay_ms(10);
}

unsigned char i2c_baca_hmc(unsigned char address)
{
unsigned char data;
i2c.start();
i2c.write(HMC5843_W); // write 0x
i2c.write(address); // write register address
i2c.start();
i2c.write(HMC5843_R); // write 0x
data = i2c.read(0); // Get MSB result
i2c.stop();
//__delay_ms(10);
return data;
}

int baca_hmc() // baca_itg(raw) untuk ambil data raw : baca_itg(scaled) untuk ambil data terskala
{
//    char xh, xl, yh, yl, zh, zl;
//    short x, y, z;
  int xh, xl, yh, yl, zh, zl;
    float x, y, z;
    
      i2c_tulis_hmc(0x02,0x01);
        xh=i2c_baca_hmc(0x03);
        xl=i2c_baca_hmc(0x04);
        zh=i2c_baca_hmc(0x05);
        zl=i2c_baca_hmc(0x06);
        yh=i2c_baca_hmc(0x07);
        yl=i2c_baca_hmc(0x08);
        x=(signed short)((signed short)(xl | (xh<<8)));
        y=(signed short)((signed short)(yl | (yh<<8)));
        z=(signed short)((signed short)(zl | (zh<<8)));
//        rawMagX=-1*y;
//        rawMagY=-1*x;
//        rawMagZ=-1*z;
        rawMagX=x;
        rawMagY=y;
        rawMagZ=z;
}

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.baud(57600);
    while(1)
    {
        if(kirim.readable())
          {
              baca=kirim.getc();
              if(baca=='s')
              baca='0';
              while(1)
              {
                  //pc.printf("ivan\n");
                  baca_adxl();
                  baca_itg();
                  baca_hmc();
                  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;
                  }
              }
          }
    }
}