jfd
Dependencies: ADXL345 BufferSerial HMC5883L ITG3200 mbed
main.cpp
- Committer:
- ivanff15
- Date:
- 2014-02-27
- Revision:
- 0:149c2a43ef70
File content as of revision 0:149c2a43ef70:
#include "mbed.h" #include <stdio.h> #include "ADXL345.h" #include "ITG3200.h" #include "HMC5883L.h" #include "BufferSerial.h" #define ID_GATHOTKACA 106 #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) Serial pc(USBTX,USBRX); BufferSerial kirim(USBTX,USBRX,1); ADXL345 acc(p28,p27); ITG3200 gyro(p28,p27); HMC5883L magneto(p28,p27); I2C i2c(p28,p27); int bacaXYZ[3]={0,0,0}; char gyroX,gyroY,gyroZ; char accX,accY,accZ; char mx,my,mz; char baca; short rawAccX, rawAccY, rawAccZ, rawGyroX, rawGyroY, rawGyroZ, rawMagX, rawMagY, rawMagZ; unsigned short sendAccX, sendAccY, sendAccZ, sendGyroX, sendGyroY, sendGyroZ, sendMagX, sendMagY, sendMagZ; 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 baca_sensor() { acc.getOutput(bacaXYZ); accX=(signed short)bacaXYZ[0]; accY=(signed short)bacaXYZ[1]; accZ=(signed short)bacaXYZ[2]; gyroX=gyro.getGyroX(); gyroY=gyro.getGyroY(); gyroZ=gyro.getGyroZ(); mx=magneto.getMx(); my=magneto.getMy(); mz=magneto.getMz(); rawAccX=-1*accY; rawAccY=accX; rawAccZ=accZ; rawGyroX=-1*gyroY; rawGyroY=gyroX; rawGyroZ=gyroZ; rawMagX=mx; rawMagY=my; rawMagZ=mz; // rawAccX=accX; // rawAccY=accY; // rawAccZ=accZ; // rawMagX=mx; // rawMagY=my; // rawMagZ=mz; // rawGyroX=gyroY; // rawGyroY=gyroX; // rawGyroZ=gyroZ; } 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.printf("start"); pc.baud(57600); acc.setPowerControl(0); acc.setPowerControl(16); acc.setPowerControl(8); acc.setDataFormatControl(0x03); //acc.setDataRate(ADXL345_3200HZ); gyro.setLpBandwidth(DLPF_FS_SEL_0|DLPF_FS_SEL_1|DLPF_CFG_0); gyro.setSampleRateDivider(9); magneto.setDefault(); // pc.printf("mulai"); while(1) { // acc.getOutput(bacaXYZ); // accX=(signed short)bacaXYZ[0]; // accY=(signed short)bacaXYZ[1]; // accZ=(signed short)bacaXYZ[2]; // gyroX=gyro.getGyroX(); // gyroY=gyro.getGyroY(); // gyroZ=gyro.getGyroZ(); // mx=magneto.getMx(); // my=magneto.getMy(); // mz=magneto.getMz(); // pc.printf("%d %d %d|%d %d %d|%d %d %d\n",accX,accY,accZ,gyroX,gyroY,gyroZ,mx,my,mz); wait_ms(200); if(kirim.readable()) { baca=kirim.getc(); if(baca=='s') baca='0'; while(1) { //pc.printf("ivan\n"); baca_sensor(); 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; } } } } }