Diff: main.cpp

Revision:

0:24c1e45f1a27

Child:

1:635b9b4215b1

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Sep 14 09:02:51 2017 +0000
@@ -0,0 +1,140 @@
+#include "mbed.h"
+
+#define I2C_ADDRESS     0xD4
+
+
+#define ADDR_WHO_AM_I   0x0f
+#define ADDR_CTRL_REG1  0x20
+#define ADDR_CTRL_REG2  0x21
+#define ADDR_CTRL_REG3  0x22
+#define ADDR_CTRL_REG4  0x23
+#define ADDR_CTRL_REG5  0x24
+#define ADDR_STATUS_REG 0x27
+#define ADDR_OUT_X_L    0x28
+#define ADDR_OUT_X_H    0x29
+#define ADDR_OUT_Y_L    0x2A
+#define ADDR_OUT_Y_H    0x2B
+#define ADDR_OUT_Z_L    0x2C
+#define ADDR_OUT_Z_H    0x2D
+
+I2C i2c(PB_9, PB_8);
+
+DigitalOut myled(LED1);
+
+Serial pc(USBTX, USBRX);
+
+char getData(char addr){
+    char readData;
+    i2c.write(I2C_ADDRESS, &addr, 1, true);
+    i2c.read(I2C_ADDRESS | 0x01, &readData, 1);
+    return readData;
+}
+
+void setData(char addr, char data){
+    char sendData[] = {addr, data};
+//    i2c.write(I2C_ADDRESS, &addr, 1, true);
+//    i2c.write(I2C_ADDRESS, &data, 1);
+    i2c.write(I2C_ADDRESS, sendData, 2);
+    return;
+}
+
+double get_angular_velocity(char axis){
+    int omega_l, omega_h;
+    int16_t omega;
+    switch(axis){
+        case 'X': case 'x':
+            omega_l = getData(ADDR_OUT_X_L);
+            omega_h = getData(ADDR_OUT_X_H);
+            break;
+        case 'Y': case 'y':
+            omega_l = getData(ADDR_OUT_Y_L);
+            omega_h = getData(ADDR_OUT_Y_H);
+            break;
+        case 'Z': case 'z':
+            omega_l = getData(ADDR_OUT_Z_L);
+            omega_h = getData(ADDR_OUT_Z_H);
+            break;
+        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.070; // +- 2000 dps mode
+    
+}
+
+double trapezoid_integr(double data, double ex_data, double period){
+    return (data + ex_data) * period / 2.0;
+}
+
+Timer gyro_timer;
+
+void getAngle(double *result, bool reset){
+    static double angle_list[3] = {0, 0, 0};
+    static double ex_omega[3] = {0, 0, 0};
+    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');
+    
+    read_period_us = gyro_timer.read_us();
+    gyro_timer.reset();
+    
+    for(int axis = 0; axis < 3; axis++){
+        angle_list[axis] += trapezoid_integr(omega[axis], ex_omega[axis], read_period_us / 1000000.0);
+        ex_omega[axis] = omega[axis];
+        if(reset)angle_list[axis] = 0;
+        result[axis] = angle_list[axis];
+    }
+    return;
+}
+
+int main(){
+    double angle[3] = {0, 0, 0};
+    
+    i2c.frequency(400000);
+    
+    pc.baud(115200);
+    
+    pc.printf("Hello\r\n");
+    
+    pc.printf("whoAmI = 0x%.2x\r\n", getData(ADDR_WHO_AM_I));
+    
+    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, 0b00100000); // +- 2000 dps mode
+    
+    gyro_timer.start();
+    
+    while(1){
+        
+        wait_ms(1);
+        
+        //pc.printf("whoAmI = 0x%.2x\r\n", getData(ADDR_WHO_AM_I));
+        
+        //pc.printf("status = 0x%.2x\r\n", getData(ADDR_STATUS_REG));
+        
+        getAngle(angle, false);
+        pc.printf("(%f, %f, %f)\r\n", angle[0], angle[1], angle[2]);
+        
+        //pc.printf("(%f, %f, %f)\r\n", get_angular_velocity('X'), get_angular_velocity('Y'), get_angular_velocity('Z'));
+        
+        
+        //get_angular_velocity_Z();
+        
+        
+        //pc.printf("CTRL_REG1 = 0x%.2x\r\n", getData(ADDR_CTRL_REG1));
+        //pc.printf("CTRL_REG2 = 0x%.2x\r\n", getData(ADDR_CTRL_REG2));
+        //pc.printf("CTRL_REG3 = 0x%.2x\r\n", getData(ADDR_CTRL_REG3));
+        //pc.printf("CTRL_REG4 = 0x%.2x\r\n", getData(ADDR_CTRL_REG4));
+        //pc.printf("CTRL_REG5 = 0x%.2x\r\n", getData(ADDR_CTRL_REG5));
+        
+    }
+}
+
+
+