ivan fajar / Mbed 2 deprecated 10dof2

gmat juara

Dependencies:   BufferSerial mbed BMP085_lib

main.cpp@0:5af6fad57e1a, 2014-03-12 (annotated)

Committer:
ivanff15
Date:
Wed Mar 12 13:42:11 2014 +0000
Revision:
0:5af6fad57e1a
Child:
1:ea4efc600a1e
gmat juara

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ivanff15 0:5af6fad57e1a 1 #include "mbed.h"
ivanff15 0:5af6fad57e1a 2 #include <stdio.h>
ivanff15 0:5af6fad57e1a 3 #include "BufferSerial.h"
ivanff15 0:5af6fad57e1a 4
ivanff15 0:5af6fad57e1a 5 Serial pc(USBTX,USBRX);
ivanff15 0:5af6fad57e1a 6 I2C i2c(p28,p27);
ivanff15 0:5af6fad57e1a 7 BufferSerial kirim(USBTX,USBRX,1);
ivanff15 0:5af6fad57e1a 8
ivanff15 0:5af6fad57e1a 9 #define ACCEL 0xA6
ivanff15 0:5af6fad57e1a 10 #define MAGNET 0x3C
ivanff15 0:5af6fad57e1a 11 #define ID_GATHOTKACA 106
ivanff15 0:5af6fad57e1a 12
ivanff15 0:5af6fad57e1a 13 //initialize accel
ivanff15 0:5af6fad57e1a 14 #define ADXL345_AXIS_X_SCALE 4
ivanff15 0:5af6fad57e1a 15
ivanff15 0:5af6fad57e1a 16 #define adxl345_address (0xA6>>1)
ivanff15 0:5af6fad57e1a 17 #define adxl345_reg_data_format (0x31)
ivanff15 0:5af6fad57e1a 18 #define adxl345_reg_pwr_ctl (0x2D)
ivanff15 0:5af6fad57e1a 19 #define adxl345_reg_xlsb (0x32)
ivanff15 0:5af6fad57e1a 20
ivanff15 0:5af6fad57e1a 21 //initialize gyro
ivanff15 0:5af6fad57e1a 22 #define ITG3200_R 0x69
ivanff15 0:5af6fad57e1a 23 #define ITG3200_W 0x68
ivanff15 0:5af6fad57e1a 24 #define WHO 0x00
ivanff15 0:5af6fad57e1a 25 #define SMPL 0x15
ivanff15 0:5af6fad57e1a 26 #define INT_C 0x17
ivanff15 0:5af6fad57e1a 27 #define TMP_H 0x1B
ivanff15 0:5af6fad57e1a 28 #define GX_H 0x1D
ivanff15 0:5af6fad57e1a 29 #define GY_H 0x1F
ivanff15 0:5af6fad57e1a 30 #define GZ_H 0x21
ivanff15 0:5af6fad57e1a 31 #define PWR_M 0x3E
ivanff15 0:5af6fad57e1a 32 #define DLPF 0x16
ivanff15 0:5af6fad57e1a 33 #define INT_S 0x1A
ivanff15 0:5af6fad57e1a 34 #define TMP_L 0x1C
ivanff15 0:5af6fad57e1a 35 #define GX_L 0x1E
ivanff15 0:5af6fad57e1a 36 #define GY_L 0x20
ivanff15 0:5af6fad57e1a 37 #define GZ_L 0x22
ivanff15 0:5af6fad57e1a 38
ivanff15 0:5af6fad57e1a 39 #define itg3200_address (0xD0)
ivanff15 0:5af6fad57e1a 40 #define itg3200_reg_xmsb (0x1D)
ivanff15 0:5af6fad57e1a 41 #define itg3200_reg_who_am_I (0x00)
ivanff15 0:5af6fad57e1a 42 #define itg3200_reg_smplrt_div (0x15)
ivanff15 0:5af6fad57e1a 43 #define itg3200_reg_dlpf_fs (0x16)
ivanff15 0:5af6fad57e1a 44 #define DLPF_CFG_0 (1<<0)
ivanff15 0:5af6fad57e1a 45 #define DLPF_CFG_1 (1<<1)
ivanff15 0:5af6fad57e1a 46 #define DLPF_CFG_2 (1<<2)
ivanff15 0:5af6fad57e1a 47 #define DLPF_FS_SEL_0 (1<<3)
ivanff15 0:5af6fad57e1a 48 #define DLPF_FS_SEL_1 (1<<4)
ivanff15 0:5af6fad57e1a 49
ivanff15 0:5af6fad57e1a 50
ivanff15 0:5af6fad57e1a 51 //initialize magneto
ivanff15 0:5af6fad57e1a 52 #define HMC5843_W 0x3C
ivanff15 0:5af6fad57e1a 53 #define HMC5843_R 0x3D
ivanff15 0:5af6fad57e1a 54 #define PI 3.14159265
ivanff15 0:5af6fad57e1a 55
ivanff15 0:5af6fad57e1a 56 #define GRAVITY 256.0f // "1G reference" used for DCM filter and accelerometer calibration
ivanff15 0:5af6fad57e1a 57
ivanff15 0:5af6fad57e1a 58 #define MAGN_X_MAX 465
ivanff15 0:5af6fad57e1a 59 #define MAGN_Y_MAX 475
ivanff15 0:5af6fad57e1a 60 #define MAGN_Z_MAX 600
ivanff15 0:5af6fad57e1a 61
ivanff15 0:5af6fad57e1a 62 #define MAGN_X_MIN -561
ivanff15 0:5af6fad57e1a 63 #define MAGN_Y_MIN -547
ivanff15 0:5af6fad57e1a 64 #define MAGN_Z_MIN -379
ivanff15 0:5af6fad57e1a 65 #define MAGN_X_OFFSET ((MAGN_X_MIN + MAGN_X_MAX) / 2.0f)
ivanff15 0:5af6fad57e1a 66 #define MAGN_Y_OFFSET ((MAGN_Y_MIN + MAGN_Y_MAX) / 2.0f)
ivanff15 0:5af6fad57e1a 67 #define MAGN_Z_OFFSET ((MAGN_Z_MIN + MAGN_Z_MAX) / 2.0f)
ivanff15 0:5af6fad57e1a 68
ivanff15 0:5af6fad57e1a 69 #define MAGN_X_SCALE (100.0f / (MAGN_X_MAX - MAGN_X_OFFSET))
ivanff15 0:5af6fad57e1a 70 #define MAGN_Y_SCALE (100.0f / (MAGN_Y_MAX - MAGN_Y_OFFSET))
ivanff15 0:5af6fad57e1a 71 #define MAGN_Z_SCALE (100.0f / (MAGN_Z_MAX - MAGN_Z_OFFSET))
ivanff15 0:5af6fad57e1a 72
ivanff15 0:5af6fad57e1a 73 short rawAccX, rawAccY, rawAccZ, rawGyroX, rawGyroY, rawGyroZ, rawMagX, rawMagY, rawMagZ;
ivanff15 0:5af6fad57e1a 74 unsigned short sendAccX, sendAccY, sendAccZ, sendGyroX, sendGyroY, sendGyroZ, sendMagX, sendMagY, sendMagZ;
ivanff15 0:5af6fad57e1a 75 char baca;
ivanff15 0:5af6fad57e1a 76
ivanff15 0:5af6fad57e1a 77 void bin_dec_conv(unsigned short data)
ivanff15 0:5af6fad57e1a 78 {
ivanff15 0:5af6fad57e1a 79 unsigned short hasil;
ivanff15 0:5af6fad57e1a 80 // unsigned char kirim[16];
ivanff15 0:5af6fad57e1a 81
ivanff15 0:5af6fad57e1a 82 hasil = data%100;
ivanff15 0:5af6fad57e1a 83 pc.printf("%d%d%d",(data/100),(hasil/10),(hasil%10));
ivanff15 0:5af6fad57e1a 84
ivanff15 0:5af6fad57e1a 85 }
ivanff15 0:5af6fad57e1a 86
ivanff15 0:5af6fad57e1a 87 void tulis_i2c(unsigned char devadd, unsigned char regadd, unsigned char data)
ivanff15 0:5af6fad57e1a 88 {
ivanff15 0:5af6fad57e1a 89 i2c.start();
ivanff15 0:5af6fad57e1a 90 i2c.write(devadd);
ivanff15 0:5af6fad57e1a 91 i2c.write(regadd);
ivanff15 0:5af6fad57e1a 92 i2c.write(data);
ivanff15 0:5af6fad57e1a 93 i2c.stop();
ivanff15 0:5af6fad57e1a 94
ivanff15 0:5af6fad57e1a 95 }
ivanff15 0:5af6fad57e1a 96 unsigned char baca_i2c(unsigned char devadd, unsigned char regadd)
ivanff15 0:5af6fad57e1a 97 {
ivanff15 0:5af6fad57e1a 98 unsigned char data;
ivanff15 0:5af6fad57e1a 99 i2c.start();
ivanff15 0:5af6fad57e1a 100 i2c.write(devadd);
ivanff15 0:5af6fad57e1a 101 i2c.write(regadd);
ivanff15 0:5af6fad57e1a 102 i2c.start();
ivanff15 0:5af6fad57e1a 103 i2c.write(devadd|1);
ivanff15 0:5af6fad57e1a 104 data=i2c.read(0);
ivanff15 0:5af6fad57e1a 105 i2c.stop();
ivanff15 0:5af6fad57e1a 106 return data;
ivanff15 0:5af6fad57e1a 107 }
ivanff15 0:5af6fad57e1a 108
ivanff15 0:5af6fad57e1a 109 int baca_adxl()
ivanff15 0:5af6fad57e1a 110 {
ivanff15 0:5af6fad57e1a 111 unsigned char acc_x_msb, acc_x_lsb;
ivanff15 0:5af6fad57e1a 112 unsigned char acc_y_msb, acc_y_lsb;
ivanff15 0:5af6fad57e1a 113 unsigned char acc_z_msb, acc_z_lsb;
ivanff15 0:5af6fad57e1a 114
ivanff15 0:5af6fad57e1a 115 tulis_i2c(ACCEL, adxl345_reg_pwr_ctl, 0);
ivanff15 0:5af6fad57e1a 116 tulis_i2c(ACCEL, adxl345_reg_pwr_ctl, 16);
ivanff15 0:5af6fad57e1a 117 tulis_i2c(ACCEL, adxl345_reg_pwr_ctl, 8);
ivanff15 0:5af6fad57e1a 118
ivanff15 0:5af6fad57e1a 119 tulis_i2c(ACCEL, adxl345_reg_data_format, 0x03);
ivanff15 0:5af6fad57e1a 120
ivanff15 0:5af6fad57e1a 121 acc_x_lsb = baca_i2c(ACCEL,0x32);
ivanff15 0:5af6fad57e1a 122 acc_x_msb = baca_i2c(ACCEL,0x33);
ivanff15 0:5af6fad57e1a 123 acc_y_lsb = baca_i2c(ACCEL,0x34);
ivanff15 0:5af6fad57e1a 124 acc_y_msb = baca_i2c(ACCEL,0x35);
ivanff15 0:5af6fad57e1a 125 acc_z_lsb = baca_i2c(ACCEL,0x36);
ivanff15 0:5af6fad57e1a 126 acc_z_msb = baca_i2c(ACCEL,0x37);
ivanff15 0:5af6fad57e1a 127
ivanff15 0:5af6fad57e1a 128 float acc_x = (signed short)((signed short)(acc_x_msb<<8) | acc_x_lsb);//*ADXL345_AXIS_X_SCALE/1000.0f;
ivanff15 0:5af6fad57e1a 129 float acc_y = 1*(signed short)((signed short)(acc_y_msb<<8) | acc_y_lsb);//*ADXL345_AXIS_X_SCALE/1000.0f;
ivanff15 0:5af6fad57e1a 130 float acc_z = (signed short)((signed short)(acc_z_msb<<8) | acc_z_lsb);//*ADXL345_AXIS_X_SCALE/1000.0f
ivanff15 0:5af6fad57e1a 131
ivanff15 0:5af6fad57e1a 132 rawAccX=-1*acc_y;
ivanff15 0:5af6fad57e1a 133 rawAccY=acc_x;
ivanff15 0:5af6fad57e1a 134 rawAccZ=acc_z;
ivanff15 0:5af6fad57e1a 135 }
ivanff15 0:5af6fad57e1a 136
ivanff15 0:5af6fad57e1a 137 void baca_itg()
ivanff15 0:5af6fad57e1a 138 {
ivanff15 0:5af6fad57e1a 139 tulis_i2c(itg3200_address, itg3200_reg_dlpf_fs, (DLPF_FS_SEL_0|DLPF_FS_SEL_1|DLPF_CFG_0));
ivanff15 0:5af6fad57e1a 140 tulis_i2c(itg3200_address, itg3200_reg_smplrt_div, 9);
ivanff15 0:5af6fad57e1a 141 char xh,xl,yh,yl,zh,zl;
ivanff15 0:5af6fad57e1a 142 short x,y,z;
ivanff15 0:5af6fad57e1a 143 xl = baca_i2c(itg3200_address,0x1E);
ivanff15 0:5af6fad57e1a 144 xh = baca_i2c(itg3200_address,0x1D);
ivanff15 0:5af6fad57e1a 145 yh = baca_i2c(itg3200_address,0x1F);
ivanff15 0:5af6fad57e1a 146 yl = baca_i2c(itg3200_address,0x20);
ivanff15 0:5af6fad57e1a 147 zh = baca_i2c(itg3200_address,0x21);
ivanff15 0:5af6fad57e1a 148 zl = baca_i2c(itg3200_address,0x22);
ivanff15 0:5af6fad57e1a 149
ivanff15 0:5af6fad57e1a 150 x=1*(signed short)((signed short)(xl | (xh<<8)));//*0.0695652174;
ivanff15 0:5af6fad57e1a 151 y=1*(signed short)((signed short)(yl | (yh<<8)));//*0.0695652174;
ivanff15 0:5af6fad57e1a 152 z=1*(signed short)((signed short)(zl | (zh<<8)));
ivanff15 0:5af6fad57e1a 153
ivanff15 0:5af6fad57e1a 154 rawGyroX=-1*y;
ivanff15 0:5af6fad57e1a 155 rawGyroY=x;
ivanff15 0:5af6fad57e1a 156 rawGyroZ=z;
ivanff15 0:5af6fad57e1a 157 }
ivanff15 0:5af6fad57e1a 158
ivanff15 0:5af6fad57e1a 159 void i2c_tulis_hmc(unsigned char address, unsigned char data)
ivanff15 0:5af6fad57e1a 160 {
ivanff15 0:5af6fad57e1a 161 i2c.start();
ivanff15 0:5af6fad57e1a 162 i2c.write(HMC5843_W); // write 0x
ivanff15 0:5af6fad57e1a 163 i2c.write(address); // write register address
ivanff15 0:5af6fad57e1a 164 i2c.write(data); // write register address
ivanff15 0:5af6fad57e1a 165 i2c.stop();
ivanff15 0:5af6fad57e1a 166 //__delay_ms(10);
ivanff15 0:5af6fad57e1a 167 }
ivanff15 0:5af6fad57e1a 168
ivanff15 0:5af6fad57e1a 169 unsigned char i2c_baca_hmc(unsigned char address)
ivanff15 0:5af6fad57e1a 170 {
ivanff15 0:5af6fad57e1a 171 unsigned char data;
ivanff15 0:5af6fad57e1a 172 i2c.start();
ivanff15 0:5af6fad57e1a 173 i2c.write(HMC5843_W); // write 0x
ivanff15 0:5af6fad57e1a 174 i2c.write(address); // write register address
ivanff15 0:5af6fad57e1a 175 i2c.start();
ivanff15 0:5af6fad57e1a 176 i2c.write(HMC5843_R); // write 0x
ivanff15 0:5af6fad57e1a 177 data = i2c.read(0); // Get MSB result
ivanff15 0:5af6fad57e1a 178 i2c.stop();
ivanff15 0:5af6fad57e1a 179 //__delay_ms(10);
ivanff15 0:5af6fad57e1a 180 return data;
ivanff15 0:5af6fad57e1a 181 }
ivanff15 0:5af6fad57e1a 182
ivanff15 0:5af6fad57e1a 183 int baca_hmc() // baca_itg(raw) untuk ambil data raw : baca_itg(scaled) untuk ambil data terskala
ivanff15 0:5af6fad57e1a 184 {
ivanff15 0:5af6fad57e1a 185 // char xh, xl, yh, yl, zh, zl;
ivanff15 0:5af6fad57e1a 186 // short x, y, z;
ivanff15 0:5af6fad57e1a 187 int xh, xl, yh, yl, zh, zl;
ivanff15 0:5af6fad57e1a 188 float x, y, z;
ivanff15 0:5af6fad57e1a 189
ivanff15 0:5af6fad57e1a 190 i2c_tulis_hmc(0x02,0x01);
ivanff15 0:5af6fad57e1a 191 xh=i2c_baca_hmc(0x03);
ivanff15 0:5af6fad57e1a 192 xl=i2c_baca_hmc(0x04);
ivanff15 0:5af6fad57e1a 193 zh=i2c_baca_hmc(0x05);
ivanff15 0:5af6fad57e1a 194 zl=i2c_baca_hmc(0x06);
ivanff15 0:5af6fad57e1a 195 yh=i2c_baca_hmc(0x07);
ivanff15 0:5af6fad57e1a 196 yl=i2c_baca_hmc(0x08);
ivanff15 0:5af6fad57e1a 197 x=(signed short)((signed short)(xl | (xh<<8)));
ivanff15 0:5af6fad57e1a 198 y=(signed short)((signed short)(yl | (yh<<8)));
ivanff15 0:5af6fad57e1a 199 z=(signed short)((signed short)(zl | (zh<<8)));
ivanff15 0:5af6fad57e1a 200 // rawMagX=-1*y;
ivanff15 0:5af6fad57e1a 201 // rawMagY=-1*x;
ivanff15 0:5af6fad57e1a 202 // rawMagZ=-1*z;
ivanff15 0:5af6fad57e1a 203 rawMagX=x;
ivanff15 0:5af6fad57e1a 204 rawMagY=y;
ivanff15 0:5af6fad57e1a 205 rawMagZ=z;
ivanff15 0:5af6fad57e1a 206 }
ivanff15 0:5af6fad57e1a 207
ivanff15 0:5af6fad57e1a 208 void raw_to_send()
ivanff15 0:5af6fad57e1a 209 {
ivanff15 0:5af6fad57e1a 210 sendAccX = (unsigned short) (rawAccX + 512);//if(sendAccX>999) sendAccX=999;
ivanff15 0:5af6fad57e1a 211 sendAccY = (unsigned short) (rawAccY + 512);//if(sendAccY>999) sendAccY=999;
ivanff15 0:5af6fad57e1a 212 sendAccZ = (unsigned short) (rawAccZ + 512);//if(sendAccZ>999) sendAccZ=999;
ivanff15 0:5af6fad57e1a 213 sendGyroX = (unsigned short)((rawGyroX*0.06956+2400)*0.2083);//if(sendGyroX>999) sendGyroX=999;
ivanff15 0:5af6fad57e1a 214 sendGyroY = (unsigned short)((rawGyroY*0.06956+2400)*0.2083);//if(sendGyroY>999) sendGyroY=999;
ivanff15 0:5af6fad57e1a 215 sendGyroZ = (unsigned short)((rawGyroZ*0.06956+2400)*0.2083);//if(sendGyroZ>999) sendGyroZ=999;
ivanff15 0:5af6fad57e1a 216 sendMagX = (unsigned short)((rawMagX+2048)*0.25);//if(sendMagX>999) sendMagX=999;
ivanff15 0:5af6fad57e1a 217 sendMagY = (unsigned short)((rawMagY+2048)*0.25);//if(sendMagY>999) sendMagY=999;
ivanff15 0:5af6fad57e1a 218 sendMagZ = (unsigned short)((rawMagZ+2048)*0.25);//if(sendMagZ>999) sendMagZ=999;
ivanff15 0:5af6fad57e1a 219 }
ivanff15 0:5af6fad57e1a 220
ivanff15 0:5af6fad57e1a 221 int main()
ivanff15 0:5af6fad57e1a 222 {
ivanff15 0:5af6fad57e1a 223 pc.baud(57600);
ivanff15 0:5af6fad57e1a 224 while(1)
ivanff15 0:5af6fad57e1a 225 {
ivanff15 0:5af6fad57e1a 226 if(kirim.readable())
ivanff15 0:5af6fad57e1a 227 {
ivanff15 0:5af6fad57e1a 228 baca=kirim.getc();
ivanff15 0:5af6fad57e1a 229 if(baca=='s')
ivanff15 0:5af6fad57e1a 230 baca='0';
ivanff15 0:5af6fad57e1a 231 while(1)
ivanff15 0:5af6fad57e1a 232 {
ivanff15 0:5af6fad57e1a 233 //pc.printf("ivan\n");
ivanff15 0:5af6fad57e1a 234 baca_adxl();
ivanff15 0:5af6fad57e1a 235 baca_itg();
ivanff15 0:5af6fad57e1a 236 baca_hmc();
ivanff15 0:5af6fad57e1a 237 raw_to_send();
ivanff15 0:5af6fad57e1a 238
ivanff15 0:5af6fad57e1a 239 pc.putc(0x0D);
ivanff15 0:5af6fad57e1a 240 bin_dec_conv(ID_GATHOTKACA);
ivanff15 0:5af6fad57e1a 241 pc.putc(0x20);
ivanff15 0:5af6fad57e1a 242
ivanff15 0:5af6fad57e1a 243
ivanff15 0:5af6fad57e1a 244 bin_dec_conv(sendAccX);
ivanff15 0:5af6fad57e1a 245 pc.putc(0x20);
ivanff15 0:5af6fad57e1a 246
ivanff15 0:5af6fad57e1a 247
ivanff15 0:5af6fad57e1a 248 bin_dec_conv(sendAccY);
ivanff15 0:5af6fad57e1a 249 pc.putc(0x20);
ivanff15 0:5af6fad57e1a 250
ivanff15 0:5af6fad57e1a 251
ivanff15 0:5af6fad57e1a 252 bin_dec_conv(sendAccZ);
ivanff15 0:5af6fad57e1a 253 pc.putc(0x20);
ivanff15 0:5af6fad57e1a 254
ivanff15 0:5af6fad57e1a 255
ivanff15 0:5af6fad57e1a 256 bin_dec_conv(sendGyroX);
ivanff15 0:5af6fad57e1a 257 pc.putc(0x20);
ivanff15 0:5af6fad57e1a 258
ivanff15 0:5af6fad57e1a 259
ivanff15 0:5af6fad57e1a 260 bin_dec_conv(sendGyroY);
ivanff15 0:5af6fad57e1a 261 pc.putc(0x20);
ivanff15 0:5af6fad57e1a 262
ivanff15 0:5af6fad57e1a 263
ivanff15 0:5af6fad57e1a 264 bin_dec_conv(sendGyroZ);
ivanff15 0:5af6fad57e1a 265 pc.putc(0x20);
ivanff15 0:5af6fad57e1a 266
ivanff15 0:5af6fad57e1a 267
ivanff15 0:5af6fad57e1a 268 bin_dec_conv(sendMagX);
ivanff15 0:5af6fad57e1a 269 pc.putc(0x20);
ivanff15 0:5af6fad57e1a 270
ivanff15 0:5af6fad57e1a 271
ivanff15 0:5af6fad57e1a 272 bin_dec_conv(sendMagY);
ivanff15 0:5af6fad57e1a 273 pc.putc(0x20);
ivanff15 0:5af6fad57e1a 274
ivanff15 0:5af6fad57e1a 275
ivanff15 0:5af6fad57e1a 276 bin_dec_conv(sendMagZ);
ivanff15 0:5af6fad57e1a 277
ivanff15 0:5af6fad57e1a 278 wait_ms(75);
ivanff15 0:5af6fad57e1a 279
ivanff15 0:5af6fad57e1a 280 baca=kirim.getc();
ivanff15 0:5af6fad57e1a 281 if(baca=='x')
ivanff15 0:5af6fad57e1a 282 {
ivanff15 0:5af6fad57e1a 283 baca='0';
ivanff15 0:5af6fad57e1a 284 break;
ivanff15 0:5af6fad57e1a 285 }
ivanff15 0:5af6fad57e1a 286 }
ivanff15 0:5af6fad57e1a 287 }
ivanff15 0:5af6fad57e1a 288 }
ivanff15 0:5af6fad57e1a 289 }