Diff: main.cpp

Revision:

0:c1b538c0d17b

Child:

1:13e640890938

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Aug 09 02:56:22 2022 +0000
@@ -0,0 +1,45 @@
+#include "mbed.h"
+#include "HEPTA_EPS.h"
+#define GYRO 0x69<<1
+Serial pc(USBTX, USBRX);
+I2C i2c(p28,p27);
+DigitalOut pin(p26);
+
+float gyroscope[3];
+char cmd[2]={0};
+const double dt = 1;
+uint8_t data[6]={0};
+char send[1], get[1];
+char temp;
+
+int main() {
+    pin=1;
+    i2c.frequency(100000);
+    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; 
+    cmd[1]=0x00;
+    i2c.write(GYRO,cmd,2); 
+    printf("\r\nrun\r\n");
+    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;
+        }
+        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]);
+        wait(dt);
+    }
+}