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?
User | Revision | Line number | New 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 | } |