ivan fajar
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;
+ }
+ }
+ }
+ }
+}