gmat juara
Dependencies: BufferSerial mbed BMP085_lib
Diff: main.cpp
- Revision:
- 0:5af6fad57e1a
- Child:
- 1:ea4efc600a1e
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Wed Mar 12 13:42:11 2014 +0000 @@ -0,0 +1,289 @@ +#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; + } + } + } + } +}