Diff: main.cpp

Revision:

0:c92d21a4e8c2

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Nov 01 07:44:54 2021 +0000
@@ -0,0 +1,85 @@
+#include "mbed.h"
+I2C i2c(PB_7, PB_6);
+Serial pc(USBTX, USBRX, 9600);
+const int addr_accel_gyro = 0xD0;
+const int addr_compus = 0x18;
+
+
+char cmd[2];
+char data[1];
+char adata[1];
+char mxl[1], mxh[1], myl[1], myh[1], mzl[1], mzh[1] , st[1];
+double mg_x, mg_y, mg_z, MX, MY, MZ;
+
+int main() {
+    pc.printf("Start detectiong magnetism\r\n");
+    wait(1.0);
+    i2c.frequency(100000);
+    
+    cmd[0]=0x6B;
+    cmd[1]=0x00;
+    i2c.write(addr_accel_gyro,cmd,2);
+    cmd[0] = 0x37;
+    cmd[1] = 0x02;
+    i2c.write(addr_accel_gyro,cmd,2);
+    
+    //write address of compus
+    adata[0] = 0x0A;
+    adata[1] = 0x12;
+    i2c.write(addr_compus,adata,2);
+    i2c.stop();
+    wait(1);
+    
+    while(1) {
+        
+        //=========== x ===========
+        //read mxl
+        adata[0] = 0x03;
+        i2c.write(addr_compus,adata,1);
+        i2c.read(addr_compus|0x01,mxl,1);
+
+        //read mxh
+        adata[0] = 0x04;
+        i2c.write(addr_compus,adata,1);
+        i2c.read(addr_compus|0x01,mxh,1);
+        
+        
+        //=========== y ===========
+        adata[0] = 0x05;
+        i2c.write(addr_compus,adata,1);
+        i2c.read(addr_compus|0x01,myl,1);
+        
+        adata[0] = 0x06;
+        i2c.write(addr_compus,adata,1);
+        i2c.read(addr_compus|0x01,myh,1);
+        
+        
+        //=========== z ===========
+        adata[0] = 0x07;
+        i2c.write(addr_compus,adata,1);
+        i2c.read(addr_compus|0x01,mzl,1);
+        
+        adata[0] = 0x08;
+        i2c.write(addr_compus,adata,1);
+        i2c.read(addr_compus|0x01,mzh,1);
+        
+        //status 
+        adata[0] = 0x09;
+        i2c.write(addr_compus,adata,1);
+        i2c.read(addr_compus|0x01,st,1);
+        
+        
+        
+        mg_x = short((mxh[0]<<8)|(mxl[0]));
+        mg_y = short((myh[0]<<8)|(myl[0]));
+        mg_z = short((mzh[0]<<8)|(mzl[0]));
+        MX = mg_x*0.15;
+        MY = mg_y*0.15;
+        MZ = mg_z*0.15;
+        pc.printf("MX= %f,MY= %f,MZ= %f\r\n",MX,MY,MZ);
+
+        wait(0.5);
+        
+    }
+    
+}