ivan fajar / Mbed 2 deprecated 10dof

jfd

Dependencies:   ADXL345 BufferSerial HMC5883L ITG3200 mbed

main.cpp@0:149c2a43ef70, 2014-02-27 (annotated)

Committer:
ivanff15
Date:
Thu Feb 27 13:23:14 2014 +0000
Revision:
0:149c2a43ef70
nnn

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ivanff15 0:149c2a43ef70 1 #include "mbed.h"
ivanff15 0:149c2a43ef70 2 #include <stdio.h>
ivanff15 0:149c2a43ef70 3 #include "ADXL345.h"
ivanff15 0:149c2a43ef70 4 #include "ITG3200.h"
ivanff15 0:149c2a43ef70 5 #include "HMC5883L.h"
ivanff15 0:149c2a43ef70 6 #include "BufferSerial.h"
ivanff15 0:149c2a43ef70 7
ivanff15 0:149c2a43ef70 8 #define ID_GATHOTKACA 106
ivanff15 0:149c2a43ef70 9 #define itg3200_address (0xD0)
ivanff15 0:149c2a43ef70 10 #define itg3200_reg_xmsb (0x1D)
ivanff15 0:149c2a43ef70 11 #define itg3200_reg_who_am_I (0x00)
ivanff15 0:149c2a43ef70 12 #define itg3200_reg_smplrt_div (0x15)
ivanff15 0:149c2a43ef70 13 #define itg3200_reg_dlpf_fs (0x16)
ivanff15 0:149c2a43ef70 14 #define DLPF_CFG_0 (1<<0)
ivanff15 0:149c2a43ef70 15 #define DLPF_CFG_1 (1<<1)
ivanff15 0:149c2a43ef70 16 #define DLPF_CFG_2 (1<<2)
ivanff15 0:149c2a43ef70 17 #define DLPF_FS_SEL_0 (1<<3)
ivanff15 0:149c2a43ef70 18 #define DLPF_FS_SEL_1 (1<<4)
ivanff15 0:149c2a43ef70 19
ivanff15 0:149c2a43ef70 20 Serial pc(USBTX,USBRX);
ivanff15 0:149c2a43ef70 21 BufferSerial kirim(USBTX,USBRX,1);
ivanff15 0:149c2a43ef70 22 ADXL345 acc(p28,p27);
ivanff15 0:149c2a43ef70 23 ITG3200 gyro(p28,p27);
ivanff15 0:149c2a43ef70 24 HMC5883L magneto(p28,p27);
ivanff15 0:149c2a43ef70 25 I2C i2c(p28,p27);
ivanff15 0:149c2a43ef70 26
ivanff15 0:149c2a43ef70 27 int bacaXYZ[3]={0,0,0};
ivanff15 0:149c2a43ef70 28 char gyroX,gyroY,gyroZ;
ivanff15 0:149c2a43ef70 29 char accX,accY,accZ;
ivanff15 0:149c2a43ef70 30 char mx,my,mz;
ivanff15 0:149c2a43ef70 31 char baca;
ivanff15 0:149c2a43ef70 32 short rawAccX, rawAccY, rawAccZ, rawGyroX, rawGyroY, rawGyroZ, rawMagX, rawMagY, rawMagZ;
ivanff15 0:149c2a43ef70 33 unsigned short sendAccX, sendAccY, sendAccZ, sendGyroX, sendGyroY, sendGyroZ, sendMagX, sendMagY, sendMagZ;
ivanff15 0:149c2a43ef70 34
ivanff15 0:149c2a43ef70 35 void bin_dec_conv(unsigned short data)
ivanff15 0:149c2a43ef70 36 {
ivanff15 0:149c2a43ef70 37 unsigned short hasil;
ivanff15 0:149c2a43ef70 38 // unsigned char kirim[16];
ivanff15 0:149c2a43ef70 39
ivanff15 0:149c2a43ef70 40 hasil = data%100;
ivanff15 0:149c2a43ef70 41 pc.printf("%d%d%d",(data/100),(hasil/10),(hasil%10));
ivanff15 0:149c2a43ef70 42
ivanff15 0:149c2a43ef70 43 }
ivanff15 0:149c2a43ef70 44
ivanff15 0:149c2a43ef70 45 void baca_sensor()
ivanff15 0:149c2a43ef70 46 {
ivanff15 0:149c2a43ef70 47 acc.getOutput(bacaXYZ);
ivanff15 0:149c2a43ef70 48 accX=(signed short)bacaXYZ[0];
ivanff15 0:149c2a43ef70 49 accY=(signed short)bacaXYZ[1];
ivanff15 0:149c2a43ef70 50 accZ=(signed short)bacaXYZ[2];
ivanff15 0:149c2a43ef70 51 gyroX=gyro.getGyroX();
ivanff15 0:149c2a43ef70 52 gyroY=gyro.getGyroY();
ivanff15 0:149c2a43ef70 53 gyroZ=gyro.getGyroZ();
ivanff15 0:149c2a43ef70 54 mx=magneto.getMx();
ivanff15 0:149c2a43ef70 55 my=magneto.getMy();
ivanff15 0:149c2a43ef70 56 mz=magneto.getMz();
ivanff15 0:149c2a43ef70 57 rawAccX=-1*accY;
ivanff15 0:149c2a43ef70 58 rawAccY=accX;
ivanff15 0:149c2a43ef70 59 rawAccZ=accZ;
ivanff15 0:149c2a43ef70 60 rawGyroX=-1*gyroY;
ivanff15 0:149c2a43ef70 61 rawGyroY=gyroX;
ivanff15 0:149c2a43ef70 62 rawGyroZ=gyroZ;
ivanff15 0:149c2a43ef70 63 rawMagX=mx;
ivanff15 0:149c2a43ef70 64 rawMagY=my;
ivanff15 0:149c2a43ef70 65 rawMagZ=mz;
ivanff15 0:149c2a43ef70 66 // rawAccX=accX;
ivanff15 0:149c2a43ef70 67 // rawAccY=accY;
ivanff15 0:149c2a43ef70 68 // rawAccZ=accZ;
ivanff15 0:149c2a43ef70 69 // rawMagX=mx;
ivanff15 0:149c2a43ef70 70 // rawMagY=my;
ivanff15 0:149c2a43ef70 71 // rawMagZ=mz;
ivanff15 0:149c2a43ef70 72 // rawGyroX=gyroY;
ivanff15 0:149c2a43ef70 73 // rawGyroY=gyroX;
ivanff15 0:149c2a43ef70 74 // rawGyroZ=gyroZ;
ivanff15 0:149c2a43ef70 75 }
ivanff15 0:149c2a43ef70 76
ivanff15 0:149c2a43ef70 77 void raw_to_send()
ivanff15 0:149c2a43ef70 78 {
ivanff15 0:149c2a43ef70 79 sendAccX = (unsigned short) (rawAccX + 512);//if(sendAccX>999) sendAccX=999;
ivanff15 0:149c2a43ef70 80 sendAccY = (unsigned short) (rawAccY + 512);//if(sendAccY>999) sendAccY=999;
ivanff15 0:149c2a43ef70 81 sendAccZ = (unsigned short) (rawAccZ + 512);//if(sendAccZ>999) sendAccZ=999;
ivanff15 0:149c2a43ef70 82 sendGyroX = (unsigned short)((rawGyroX*0.06956+2400)*0.2083);//if(sendGyroX>999) sendGyroX=999;
ivanff15 0:149c2a43ef70 83 sendGyroY = (unsigned short)((rawGyroY*0.06956+2400)*0.2083);//if(sendGyroY>999) sendGyroY=999;
ivanff15 0:149c2a43ef70 84 sendGyroZ = (unsigned short)((rawGyroZ*0.06956+2400)*0.2083);//if(sendGyroZ>999) sendGyroZ=999;
ivanff15 0:149c2a43ef70 85 sendMagX = (unsigned short)((rawMagX+2048)*0.25);//if(sendMagX>999) sendMagX=999;
ivanff15 0:149c2a43ef70 86 sendMagY = (unsigned short)((rawMagY+2048)*0.25);//if(sendMagY>999) sendMagY=999;
ivanff15 0:149c2a43ef70 87 sendMagZ = (unsigned short)((rawMagZ+2048)*0.25);//if(sendMagZ>999) sendMagZ=999;
ivanff15 0:149c2a43ef70 88 }
ivanff15 0:149c2a43ef70 89
ivanff15 0:149c2a43ef70 90 int main()
ivanff15 0:149c2a43ef70 91 {
ivanff15 0:149c2a43ef70 92 // pc.printf("start");
ivanff15 0:149c2a43ef70 93 pc.baud(57600);
ivanff15 0:149c2a43ef70 94 acc.setPowerControl(0);
ivanff15 0:149c2a43ef70 95 acc.setPowerControl(16);
ivanff15 0:149c2a43ef70 96 acc.setPowerControl(8);
ivanff15 0:149c2a43ef70 97 acc.setDataFormatControl(0x03);
ivanff15 0:149c2a43ef70 98 //acc.setDataRate(ADXL345_3200HZ);
ivanff15 0:149c2a43ef70 99 gyro.setLpBandwidth(DLPF_FS_SEL_0|DLPF_FS_SEL_1|DLPF_CFG_0);
ivanff15 0:149c2a43ef70 100 gyro.setSampleRateDivider(9);
ivanff15 0:149c2a43ef70 101 magneto.setDefault();
ivanff15 0:149c2a43ef70 102 // pc.printf("mulai");
ivanff15 0:149c2a43ef70 103 while(1)
ivanff15 0:149c2a43ef70 104 {
ivanff15 0:149c2a43ef70 105 // acc.getOutput(bacaXYZ);
ivanff15 0:149c2a43ef70 106 // accX=(signed short)bacaXYZ[0];
ivanff15 0:149c2a43ef70 107 // accY=(signed short)bacaXYZ[1];
ivanff15 0:149c2a43ef70 108 // accZ=(signed short)bacaXYZ[2];
ivanff15 0:149c2a43ef70 109 // gyroX=gyro.getGyroX();
ivanff15 0:149c2a43ef70 110 // gyroY=gyro.getGyroY();
ivanff15 0:149c2a43ef70 111 // gyroZ=gyro.getGyroZ();
ivanff15 0:149c2a43ef70 112 // mx=magneto.getMx();
ivanff15 0:149c2a43ef70 113 // my=magneto.getMy();
ivanff15 0:149c2a43ef70 114 // mz=magneto.getMz();
ivanff15 0:149c2a43ef70 115 // pc.printf("%d %d %d|%d %d %d|%d %d %d\n",accX,accY,accZ,gyroX,gyroY,gyroZ,mx,my,mz);
ivanff15 0:149c2a43ef70 116 wait_ms(200);
ivanff15 0:149c2a43ef70 117 if(kirim.readable())
ivanff15 0:149c2a43ef70 118 {
ivanff15 0:149c2a43ef70 119 baca=kirim.getc();
ivanff15 0:149c2a43ef70 120 if(baca=='s')
ivanff15 0:149c2a43ef70 121 baca='0';
ivanff15 0:149c2a43ef70 122 while(1)
ivanff15 0:149c2a43ef70 123 {
ivanff15 0:149c2a43ef70 124 //pc.printf("ivan\n");
ivanff15 0:149c2a43ef70 125 baca_sensor();
ivanff15 0:149c2a43ef70 126 raw_to_send();
ivanff15 0:149c2a43ef70 127
ivanff15 0:149c2a43ef70 128 pc.putc(0x0D);
ivanff15 0:149c2a43ef70 129 bin_dec_conv(ID_GATHOTKACA);
ivanff15 0:149c2a43ef70 130 pc.putc(0x20);
ivanff15 0:149c2a43ef70 131
ivanff15 0:149c2a43ef70 132
ivanff15 0:149c2a43ef70 133 bin_dec_conv(sendAccX);
ivanff15 0:149c2a43ef70 134 pc.putc(0x20);
ivanff15 0:149c2a43ef70 135
ivanff15 0:149c2a43ef70 136
ivanff15 0:149c2a43ef70 137 bin_dec_conv(sendAccY);
ivanff15 0:149c2a43ef70 138 pc.putc(0x20);
ivanff15 0:149c2a43ef70 139
ivanff15 0:149c2a43ef70 140
ivanff15 0:149c2a43ef70 141 bin_dec_conv(sendAccZ);
ivanff15 0:149c2a43ef70 142 pc.putc(0x20);
ivanff15 0:149c2a43ef70 143
ivanff15 0:149c2a43ef70 144
ivanff15 0:149c2a43ef70 145 bin_dec_conv(sendGyroX);
ivanff15 0:149c2a43ef70 146 pc.putc(0x20);
ivanff15 0:149c2a43ef70 147
ivanff15 0:149c2a43ef70 148
ivanff15 0:149c2a43ef70 149 bin_dec_conv(sendGyroY);
ivanff15 0:149c2a43ef70 150 pc.putc(0x20);
ivanff15 0:149c2a43ef70 151
ivanff15 0:149c2a43ef70 152
ivanff15 0:149c2a43ef70 153 bin_dec_conv(sendGyroZ);
ivanff15 0:149c2a43ef70 154 pc.putc(0x20);
ivanff15 0:149c2a43ef70 155
ivanff15 0:149c2a43ef70 156
ivanff15 0:149c2a43ef70 157 bin_dec_conv(sendMagX);
ivanff15 0:149c2a43ef70 158 pc.putc(0x20);
ivanff15 0:149c2a43ef70 159
ivanff15 0:149c2a43ef70 160
ivanff15 0:149c2a43ef70 161 bin_dec_conv(sendMagY);
ivanff15 0:149c2a43ef70 162 pc.putc(0x20);
ivanff15 0:149c2a43ef70 163
ivanff15 0:149c2a43ef70 164
ivanff15 0:149c2a43ef70 165 bin_dec_conv(sendMagZ);
ivanff15 0:149c2a43ef70 166
ivanff15 0:149c2a43ef70 167 wait_ms(75);
ivanff15 0:149c2a43ef70 168
ivanff15 0:149c2a43ef70 169 baca=kirim.getc();
ivanff15 0:149c2a43ef70 170 if(baca=='x')
ivanff15 0:149c2a43ef70 171 {
ivanff15 0:149c2a43ef70 172 baca='0';
ivanff15 0:149c2a43ef70 173 break;
ivanff15 0:149c2a43ef70 174 }
ivanff15 0:149c2a43ef70 175 }
ivanff15 0:149c2a43ef70 176 }
ivanff15 0:149c2a43ef70 177
ivanff15 0:149c2a43ef70 178
ivanff15 0:149c2a43ef70 179 }
ivanff15 0:149c2a43ef70 180 }