Diff: main.cpp

Revision:

3:a1a4491e2ab8

Parent:

2:4fcc41c10f22

Child:

4:c041a3581bc7

--- a/main.cpp	Tue Aug 09 06:25:09 2022 +0000
+++ b/main.cpp	Tue Nov 08 03:00:54 2022 +0000
@@ -1,43 +1,38 @@
 #include "mbed.h"
 #include "HEPTA_EPS.h"
-#define GYRO 0x69<<1 //addr_gyro
+#define ACC 0x19<<1 //addr_accel
 
-Serial pc(USBTX, USBRX);
+RawSerial pc(USBTX,USBRX,9600);
 HEPTA_EPS eps(p16,p26);
 I2C i2c(p28,p27);
 
-float gyroscope[3];
+float accel[3];
 char cmd[2]={0};
 uint8_t data[6]={0};
-char send[1], get[1];
-char temp; //temperature
+char send[1];
 
 int main() {
     i2c.frequency(100000);
-    pc.printf("gyroscope setting\r\n");
-    cmd[0]=0x0F; 
-    cmd[1]=0x04;
-    i2c.write(GYRO,cmd,2); 
-    cmd[0]=0x10; 
-    cmd[1]=0x07;
-    i2c.write(GYRO,cmd,2); 
-    cmd[0]=0x11; 
+    pc.printf("accel setting\r\n");
+    cmd[0]=0x14; //softwareset 
+    cmd[1]=0xB6; //triggers a reset
+    i2c.write(ACC,cmd,2); 
+    cmd[0]=0x0F; //acceleration measurement range
+    cmd[1]=0x05; //+-4g
+    i2c.write(ACC,cmd,2); 
+    cmd[0]=0x11; //
     cmd[1]=0x00;
-    i2c.write(GYRO,cmd,2); 
+    i2c.write(ACC,cmd,2); 
     while(1) {
-        for(int i=0;i<6;i++){
-            send[0]=(char)(2+i);
-            i2c.write(GYRO,send,1);
-            i2c.read(GYRO,get,1);
-            temp=get[0];
-            data[i]=temp;
+        send[0]=(char)(2);
+        i2c.write(ACC,send,1,true);
+        i2c.read(ACC,(char*)data,6);
+        for(int i=0;i<3;i++){
+            accel[i]=(int16_t)(((int16_t)data[i*2+1]<<8) | data[i*2]) >> 4;
+            if(accel[i]>2047)accel[i]-=4096;
+            accel[i]=accel[i]/519*9.8;
         }
-        for(int i=0;i<3;i++){
-            gyroscope[i]=(int16_t)(((int16_t)data[i*2+1]<<8) | data[i*2]) >> 4;
-            if(gyroscope[i]>32767)gyroscope[i]-=65536;
-            gyroscope[i]=gyroscope[i]*125/2048;
-        }
-        pc.printf("gx = %2.4f, gy = %2.4f, gz = %2.4f\r\n",gyroscope[0],gyroscope[1],gyroscope[2]);
+        pc.printf("ax = %2.4f, ay = %2.4f, az = %2.4f\r\n",accel[0],accel[1],accel[2]);
         wait(1);
     }
 }
\ No newline at end of file