Diff: main.cpp

Revision:

0:149c2a43ef70

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Feb 27 13:23:14 2014 +0000
@@ -0,0 +1,180 @@
+#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;
+                  }
+              }
+          }
+
+        
+    }
+}
\ No newline at end of file