main.cpp

00001 #include "mbed.h"
00002 #include <stdio.h>
00003 #include "ADXL345.h"
00004 #include "ITG3200.h"
00005 #include "HMC5883L.h"
00006 #include "BufferSerial.h"
00007 
00008 #define ID_GATHOTKACA 106
00009 #define itg3200_address (0xD0)
00010 #define itg3200_reg_xmsb (0x1D)
00011 #define itg3200_reg_who_am_I (0x00)
00012 #define itg3200_reg_smplrt_div (0x15)
00013 #define itg3200_reg_dlpf_fs (0x16)
00014 #define DLPF_CFG_0 (1<<0)
00015 #define DLPF_CFG_1 (1<<1)
00016 #define DLPF_CFG_2 (1<<2)
00017 #define DLPF_FS_SEL_0 (1<<3)
00018 #define DLPF_FS_SEL_1 (1<<4)
00019 
00020 Serial pc(USBTX,USBRX);
00021 BufferSerial kirim(USBTX,USBRX,1);
00022 ADXL345 acc(p28,p27);
00023 ITG3200 gyro(p28,p27);
00024 HMC5883L magneto(p28,p27);
00025 I2C i2c(p28,p27);
00026 
00027 int bacaXYZ[3]={0,0,0};
00028 char gyroX,gyroY,gyroZ;
00029 char accX,accY,accZ;
00030 char mx,my,mz;
00031 char baca;
00032 short rawAccX, rawAccY, rawAccZ, rawGyroX, rawGyroY, rawGyroZ, rawMagX, rawMagY, rawMagZ;
00033 unsigned short sendAccX, sendAccY, sendAccZ, sendGyroX, sendGyroY, sendGyroZ, sendMagX, sendMagY, sendMagZ;
00034 
00035 void bin_dec_conv(unsigned short data)
00036 {
00037     unsigned short hasil;
00038 //    unsigned char kirim[16];
00039 
00040     hasil = data%100;
00041     pc.printf("%d%d%d",(data/100),(hasil/10),(hasil%10));
00042 
00043 }
00044 
00045 void baca_sensor()
00046 {
00047         acc.getOutput(bacaXYZ);
00048         accX=(signed short)bacaXYZ[0];
00049         accY=(signed short)bacaXYZ[1];
00050         accZ=(signed short)bacaXYZ[2];
00051         gyroX=gyro.getGyroX();
00052         gyroY=gyro.getGyroY();
00053         gyroZ=gyro.getGyroZ();
00054         mx=magneto.getMx();
00055         my=magneto.getMy();
00056         mz=magneto.getMz();
00057         rawAccX=-1*accY;
00058         rawAccY=accX;
00059         rawAccZ=accZ;
00060         rawGyroX=-1*gyroY;
00061         rawGyroY=gyroX;
00062         rawGyroZ=gyroZ;
00063         rawMagX=mx;
00064         rawMagY=my;
00065         rawMagZ=mz;
00066 //        rawAccX=accX;
00067 //        rawAccY=accY;
00068 //        rawAccZ=accZ;
00069 //        rawMagX=mx;
00070 //        rawMagY=my;
00071 //        rawMagZ=mz;
00072 //        rawGyroX=gyroY;
00073 //        rawGyroY=gyroX;
00074 //        rawGyroZ=gyroZ;
00075 }
00076 
00077 void raw_to_send()
00078 {
00079     sendAccX = (unsigned short) (rawAccX + 512);//if(sendAccX>999) sendAccX=999;
00080     sendAccY = (unsigned short) (rawAccY + 512);//if(sendAccY>999) sendAccY=999;
00081     sendAccZ = (unsigned short) (rawAccZ + 512);//if(sendAccZ>999) sendAccZ=999;
00082     sendGyroX = (unsigned short)((rawGyroX*0.06956+2400)*0.2083);//if(sendGyroX>999) sendGyroX=999;
00083     sendGyroY = (unsigned short)((rawGyroY*0.06956+2400)*0.2083);//if(sendGyroY>999) sendGyroY=999;
00084     sendGyroZ = (unsigned short)((rawGyroZ*0.06956+2400)*0.2083);//if(sendGyroZ>999) sendGyroZ=999;
00085     sendMagX = (unsigned short)((rawMagX+2048)*0.25);//if(sendMagX>999) sendMagX=999;
00086     sendMagY = (unsigned short)((rawMagY+2048)*0.25);//if(sendMagY>999) sendMagY=999;
00087     sendMagZ = (unsigned short)((rawMagZ+2048)*0.25);//if(sendMagZ>999) sendMagZ=999;
00088 }
00089 
00090 int main()
00091 {
00092 //    pc.printf("start");
00093     pc.baud(57600);
00094     acc.setPowerControl(0);
00095     acc.setPowerControl(16);
00096     acc.setPowerControl(8);
00097     acc.setDataFormatControl(0x03);
00098     //acc.setDataRate(ADXL345_3200HZ);
00099     gyro.setLpBandwidth(DLPF_FS_SEL_0|DLPF_FS_SEL_1|DLPF_CFG_0);
00100     gyro.setSampleRateDivider(9);
00101     magneto.setDefault();
00102 //    pc.printf("mulai");
00103     while(1)
00104     {
00105 //        acc.getOutput(bacaXYZ);
00106 //        accX=(signed short)bacaXYZ[0];
00107 //        accY=(signed short)bacaXYZ[1];
00108 //        accZ=(signed short)bacaXYZ[2];
00109 //        gyroX=gyro.getGyroX();
00110 //        gyroY=gyro.getGyroY();
00111 //        gyroZ=gyro.getGyroZ();
00112 //        mx=magneto.getMx();
00113 //        my=magneto.getMy();
00114 //        mz=magneto.getMz();
00115        // pc.printf("%d %d %d|%d %d %d|%d %d %d\n",accX,accY,accZ,gyroX,gyroY,gyroZ,mx,my,mz);
00116         wait_ms(200);
00117           if(kirim.readable())
00118           {
00119               baca=kirim.getc();
00120               if(baca=='s')
00121               baca='0';
00122               while(1)
00123               {
00124                   //pc.printf("ivan\n");
00125                   baca_sensor();
00126                   raw_to_send();
00127 
00128                   pc.putc(0x0D);
00129                   bin_dec_conv(ID_GATHOTKACA);
00130                   pc.putc(0x20);
00131                   
00132                   
00133                   bin_dec_conv(sendAccX);
00134                   pc.putc(0x20);
00135                   
00136                   
00137                   bin_dec_conv(sendAccY);
00138                   pc.putc(0x20);
00139                   
00140                   
00141                   bin_dec_conv(sendAccZ);
00142                   pc.putc(0x20);
00143                   
00144                   
00145                   bin_dec_conv(sendGyroX);
00146                   pc.putc(0x20);
00147                   
00148                   
00149                   bin_dec_conv(sendGyroY);
00150                   pc.putc(0x20);
00151                   
00152                   
00153                   bin_dec_conv(sendGyroZ);
00154                   pc.putc(0x20);
00155                   
00156                   
00157                   bin_dec_conv(sendMagX);
00158                   pc.putc(0x20);
00159                   
00160                   
00161                   bin_dec_conv(sendMagY);
00162                   pc.putc(0x20);
00163                   
00164                   
00165                   bin_dec_conv(sendMagZ);
00166                   
00167                   wait_ms(75); 
00168                   
00169                   baca=kirim.getc();
00170                   if(baca=='x')
00171                   {
00172                       baca='0';
00173                       break;
00174                   }
00175               }
00176           }
00177 
00178         
00179     }
00180 }