Diff: main.cpp

Revision:

1:635b9b4215b1

Parent:

0:24c1e45f1a27

--- a/main.cpp	Thu Sep 14 09:02:51 2017 +0000
+++ b/main.cpp	Thu Sep 14 11:48:38 2017 +0000
@@ -9,6 +9,7 @@
 #define ADDR_CTRL_REG3  0x22
 #define ADDR_CTRL_REG4  0x23
 #define ADDR_CTRL_REG5  0x24
+#define ADDR_OUT_TEMP   0x26
 #define ADDR_STATUS_REG 0x27
 #define ADDR_OUT_X_L    0x28
 #define ADDR_OUT_X_H    0x29
@@ -23,6 +24,8 @@
 
 Serial pc(USBTX, USBRX);
 
+double omega_offset[3] = {0, 0, 0};
+
 char getData(char addr){
     char readData;
     i2c.write(I2C_ADDRESS, &addr, 1, true);
@@ -57,8 +60,8 @@
         default : return 0.0;
     }
     omega = omega_h << 8 | omega_l;
-    //return omega * 0.00875; // +- 250 dps mode
-    return omega * 0.01750; // +- 500 dps mode
+    return omega * 0.00875; // +- 250 dps mode
+    //return omega * 0.01750; // +- 500 dps mode
     //return omega * 0.070; // +- 2000 dps mode
     
 }
@@ -75,9 +78,9 @@
     int read_period_us;
     double omega[3] = {0, 0, 0};
     
-    omega[0] = get_angular_velocity('X');
-    omega[1] = get_angular_velocity('Y');
-    omega[2] = get_angular_velocity('Z');
+    omega[0] = get_angular_velocity('X') - omega_offset[0];
+    omega[1] = get_angular_velocity('Y') - omega_offset[1];
+    omega[2] = get_angular_velocity('Z') - omega_offset[2];
     
     read_period_us = gyro_timer.read_us();
     gyro_timer.reset();
@@ -91,8 +94,14 @@
     return;
 }
 
+int getTemp(){
+    return (int)getData(ADDR_OUT_TEMP) * -1 + 40; // 40 : Set manually
+}
+
+
 int main(){
     double angle[3] = {0, 0, 0};
+    double accum[3] = {0, 0, 0};
     
     i2c.frequency(400000);
     
@@ -104,12 +113,26 @@
     
     setData(ADDR_CTRL_REG1, 0x0f);
     
-    //setData(ADDR_CTRL_REG4, 0b00000000); // +- 250 dps mode
-    setData(ADDR_CTRL_REG4, 0b00010000); // +- 500 dps mode
+    setData(ADDR_CTRL_REG4, 0b00000000); // +- 250 dps mode
+    //setData(ADDR_CTRL_REG4, 0b00010000); // +- 500 dps mode
     //setData(ADDR_CTRL_REG4, 0b00100000); // +- 2000 dps mode
     
     gyro_timer.start();
     
+    for(int count = 0; count < 1000; count++){
+        accum[0] += get_angular_velocity('X');
+        accum[1] += get_angular_velocity('Y');
+        accum[2] += get_angular_velocity('Z');
+        wait_ms(2);
+    }
+    
+    for(int axis = 0; axis < 3; axis++){
+        omega_offset[axis] = accum[axis] / 1000.0;
+    }
+    
+    pc.printf("accum(%f, %f, %f)\r\n", accum[0], accum[1], accum[2]);
+    pc.printf("offset(%f, %f, %f)\r\n", omega_offset[0], omega_offset[1], omega_offset[2]);
+    
     while(1){
         
         wait_ms(1);
@@ -121,6 +144,8 @@
         getAngle(angle, false);
         pc.printf("(%f, %f, %f)\r\n", angle[0], angle[1], angle[2]);
         
+        pc.printf("temp = %d ℃\r\n", getTemp());
+        
         //pc.printf("(%f, %f, %f)\r\n", get_angular_velocity('X'), get_angular_velocity('Y'), get_angular_velocity('Z'));