Akito
/
L3GD20_test
gyro sensor L3GD20 test
main.cpp@1:635b9b4215b1, 2017-09-14 (annotated)
- Committer:
- Akito914
- Date:
- Thu Sep 14 11:48:38 2017 +0000
- Revision:
- 1:635b9b4215b1
- Parent:
- 0:24c1e45f1a27
???????
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Akito914 | 0:24c1e45f1a27 | 1 | #include "mbed.h" |
Akito914 | 0:24c1e45f1a27 | 2 | |
Akito914 | 0:24c1e45f1a27 | 3 | #define I2C_ADDRESS 0xD4 |
Akito914 | 0:24c1e45f1a27 | 4 | |
Akito914 | 0:24c1e45f1a27 | 5 | |
Akito914 | 0:24c1e45f1a27 | 6 | #define ADDR_WHO_AM_I 0x0f |
Akito914 | 0:24c1e45f1a27 | 7 | #define ADDR_CTRL_REG1 0x20 |
Akito914 | 0:24c1e45f1a27 | 8 | #define ADDR_CTRL_REG2 0x21 |
Akito914 | 0:24c1e45f1a27 | 9 | #define ADDR_CTRL_REG3 0x22 |
Akito914 | 0:24c1e45f1a27 | 10 | #define ADDR_CTRL_REG4 0x23 |
Akito914 | 0:24c1e45f1a27 | 11 | #define ADDR_CTRL_REG5 0x24 |
Akito914 | 1:635b9b4215b1 | 12 | #define ADDR_OUT_TEMP 0x26 |
Akito914 | 0:24c1e45f1a27 | 13 | #define ADDR_STATUS_REG 0x27 |
Akito914 | 0:24c1e45f1a27 | 14 | #define ADDR_OUT_X_L 0x28 |
Akito914 | 0:24c1e45f1a27 | 15 | #define ADDR_OUT_X_H 0x29 |
Akito914 | 0:24c1e45f1a27 | 16 | #define ADDR_OUT_Y_L 0x2A |
Akito914 | 0:24c1e45f1a27 | 17 | #define ADDR_OUT_Y_H 0x2B |
Akito914 | 0:24c1e45f1a27 | 18 | #define ADDR_OUT_Z_L 0x2C |
Akito914 | 0:24c1e45f1a27 | 19 | #define ADDR_OUT_Z_H 0x2D |
Akito914 | 0:24c1e45f1a27 | 20 | |
Akito914 | 0:24c1e45f1a27 | 21 | I2C i2c(PB_9, PB_8); |
Akito914 | 0:24c1e45f1a27 | 22 | |
Akito914 | 0:24c1e45f1a27 | 23 | DigitalOut myled(LED1); |
Akito914 | 0:24c1e45f1a27 | 24 | |
Akito914 | 0:24c1e45f1a27 | 25 | Serial pc(USBTX, USBRX); |
Akito914 | 0:24c1e45f1a27 | 26 | |
Akito914 | 1:635b9b4215b1 | 27 | double omega_offset[3] = {0, 0, 0}; |
Akito914 | 1:635b9b4215b1 | 28 | |
Akito914 | 0:24c1e45f1a27 | 29 | char getData(char addr){ |
Akito914 | 0:24c1e45f1a27 | 30 | char readData; |
Akito914 | 0:24c1e45f1a27 | 31 | i2c.write(I2C_ADDRESS, &addr, 1, true); |
Akito914 | 0:24c1e45f1a27 | 32 | i2c.read(I2C_ADDRESS | 0x01, &readData, 1); |
Akito914 | 0:24c1e45f1a27 | 33 | return readData; |
Akito914 | 0:24c1e45f1a27 | 34 | } |
Akito914 | 0:24c1e45f1a27 | 35 | |
Akito914 | 0:24c1e45f1a27 | 36 | void setData(char addr, char data){ |
Akito914 | 0:24c1e45f1a27 | 37 | char sendData[] = {addr, data}; |
Akito914 | 0:24c1e45f1a27 | 38 | // i2c.write(I2C_ADDRESS, &addr, 1, true); |
Akito914 | 0:24c1e45f1a27 | 39 | // i2c.write(I2C_ADDRESS, &data, 1); |
Akito914 | 0:24c1e45f1a27 | 40 | i2c.write(I2C_ADDRESS, sendData, 2); |
Akito914 | 0:24c1e45f1a27 | 41 | return; |
Akito914 | 0:24c1e45f1a27 | 42 | } |
Akito914 | 0:24c1e45f1a27 | 43 | |
Akito914 | 0:24c1e45f1a27 | 44 | double get_angular_velocity(char axis){ |
Akito914 | 0:24c1e45f1a27 | 45 | int omega_l, omega_h; |
Akito914 | 0:24c1e45f1a27 | 46 | int16_t omega; |
Akito914 | 0:24c1e45f1a27 | 47 | switch(axis){ |
Akito914 | 0:24c1e45f1a27 | 48 | case 'X': case 'x': |
Akito914 | 0:24c1e45f1a27 | 49 | omega_l = getData(ADDR_OUT_X_L); |
Akito914 | 0:24c1e45f1a27 | 50 | omega_h = getData(ADDR_OUT_X_H); |
Akito914 | 0:24c1e45f1a27 | 51 | break; |
Akito914 | 0:24c1e45f1a27 | 52 | case 'Y': case 'y': |
Akito914 | 0:24c1e45f1a27 | 53 | omega_l = getData(ADDR_OUT_Y_L); |
Akito914 | 0:24c1e45f1a27 | 54 | omega_h = getData(ADDR_OUT_Y_H); |
Akito914 | 0:24c1e45f1a27 | 55 | break; |
Akito914 | 0:24c1e45f1a27 | 56 | case 'Z': case 'z': |
Akito914 | 0:24c1e45f1a27 | 57 | omega_l = getData(ADDR_OUT_Z_L); |
Akito914 | 0:24c1e45f1a27 | 58 | omega_h = getData(ADDR_OUT_Z_H); |
Akito914 | 0:24c1e45f1a27 | 59 | break; |
Akito914 | 0:24c1e45f1a27 | 60 | default : return 0.0; |
Akito914 | 0:24c1e45f1a27 | 61 | } |
Akito914 | 0:24c1e45f1a27 | 62 | omega = omega_h << 8 | omega_l; |
Akito914 | 1:635b9b4215b1 | 63 | return omega * 0.00875; // +- 250 dps mode |
Akito914 | 1:635b9b4215b1 | 64 | //return omega * 0.01750; // +- 500 dps mode |
Akito914 | 0:24c1e45f1a27 | 65 | //return omega * 0.070; // +- 2000 dps mode |
Akito914 | 0:24c1e45f1a27 | 66 | |
Akito914 | 0:24c1e45f1a27 | 67 | } |
Akito914 | 0:24c1e45f1a27 | 68 | |
Akito914 | 0:24c1e45f1a27 | 69 | double trapezoid_integr(double data, double ex_data, double period){ |
Akito914 | 0:24c1e45f1a27 | 70 | return (data + ex_data) * period / 2.0; |
Akito914 | 0:24c1e45f1a27 | 71 | } |
Akito914 | 0:24c1e45f1a27 | 72 | |
Akito914 | 0:24c1e45f1a27 | 73 | Timer gyro_timer; |
Akito914 | 0:24c1e45f1a27 | 74 | |
Akito914 | 0:24c1e45f1a27 | 75 | void getAngle(double *result, bool reset){ |
Akito914 | 0:24c1e45f1a27 | 76 | static double angle_list[3] = {0, 0, 0}; |
Akito914 | 0:24c1e45f1a27 | 77 | static double ex_omega[3] = {0, 0, 0}; |
Akito914 | 0:24c1e45f1a27 | 78 | int read_period_us; |
Akito914 | 0:24c1e45f1a27 | 79 | double omega[3] = {0, 0, 0}; |
Akito914 | 0:24c1e45f1a27 | 80 | |
Akito914 | 1:635b9b4215b1 | 81 | omega[0] = get_angular_velocity('X') - omega_offset[0]; |
Akito914 | 1:635b9b4215b1 | 82 | omega[1] = get_angular_velocity('Y') - omega_offset[1]; |
Akito914 | 1:635b9b4215b1 | 83 | omega[2] = get_angular_velocity('Z') - omega_offset[2]; |
Akito914 | 0:24c1e45f1a27 | 84 | |
Akito914 | 0:24c1e45f1a27 | 85 | read_period_us = gyro_timer.read_us(); |
Akito914 | 0:24c1e45f1a27 | 86 | gyro_timer.reset(); |
Akito914 | 0:24c1e45f1a27 | 87 | |
Akito914 | 0:24c1e45f1a27 | 88 | for(int axis = 0; axis < 3; axis++){ |
Akito914 | 0:24c1e45f1a27 | 89 | angle_list[axis] += trapezoid_integr(omega[axis], ex_omega[axis], read_period_us / 1000000.0); |
Akito914 | 0:24c1e45f1a27 | 90 | ex_omega[axis] = omega[axis]; |
Akito914 | 0:24c1e45f1a27 | 91 | if(reset)angle_list[axis] = 0; |
Akito914 | 0:24c1e45f1a27 | 92 | result[axis] = angle_list[axis]; |
Akito914 | 0:24c1e45f1a27 | 93 | } |
Akito914 | 0:24c1e45f1a27 | 94 | return; |
Akito914 | 0:24c1e45f1a27 | 95 | } |
Akito914 | 0:24c1e45f1a27 | 96 | |
Akito914 | 1:635b9b4215b1 | 97 | int getTemp(){ |
Akito914 | 1:635b9b4215b1 | 98 | return (int)getData(ADDR_OUT_TEMP) * -1 + 40; // 40 : Set manually |
Akito914 | 1:635b9b4215b1 | 99 | } |
Akito914 | 1:635b9b4215b1 | 100 | |
Akito914 | 1:635b9b4215b1 | 101 | |
Akito914 | 0:24c1e45f1a27 | 102 | int main(){ |
Akito914 | 0:24c1e45f1a27 | 103 | double angle[3] = {0, 0, 0}; |
Akito914 | 1:635b9b4215b1 | 104 | double accum[3] = {0, 0, 0}; |
Akito914 | 0:24c1e45f1a27 | 105 | |
Akito914 | 0:24c1e45f1a27 | 106 | i2c.frequency(400000); |
Akito914 | 0:24c1e45f1a27 | 107 | |
Akito914 | 0:24c1e45f1a27 | 108 | pc.baud(115200); |
Akito914 | 0:24c1e45f1a27 | 109 | |
Akito914 | 0:24c1e45f1a27 | 110 | pc.printf("Hello\r\n"); |
Akito914 | 0:24c1e45f1a27 | 111 | |
Akito914 | 0:24c1e45f1a27 | 112 | pc.printf("whoAmI = 0x%.2x\r\n", getData(ADDR_WHO_AM_I)); |
Akito914 | 0:24c1e45f1a27 | 113 | |
Akito914 | 0:24c1e45f1a27 | 114 | setData(ADDR_CTRL_REG1, 0x0f); |
Akito914 | 0:24c1e45f1a27 | 115 | |
Akito914 | 1:635b9b4215b1 | 116 | setData(ADDR_CTRL_REG4, 0b00000000); // +- 250 dps mode |
Akito914 | 1:635b9b4215b1 | 117 | //setData(ADDR_CTRL_REG4, 0b00010000); // +- 500 dps mode |
Akito914 | 0:24c1e45f1a27 | 118 | //setData(ADDR_CTRL_REG4, 0b00100000); // +- 2000 dps mode |
Akito914 | 0:24c1e45f1a27 | 119 | |
Akito914 | 0:24c1e45f1a27 | 120 | gyro_timer.start(); |
Akito914 | 0:24c1e45f1a27 | 121 | |
Akito914 | 1:635b9b4215b1 | 122 | for(int count = 0; count < 1000; count++){ |
Akito914 | 1:635b9b4215b1 | 123 | accum[0] += get_angular_velocity('X'); |
Akito914 | 1:635b9b4215b1 | 124 | accum[1] += get_angular_velocity('Y'); |
Akito914 | 1:635b9b4215b1 | 125 | accum[2] += get_angular_velocity('Z'); |
Akito914 | 1:635b9b4215b1 | 126 | wait_ms(2); |
Akito914 | 1:635b9b4215b1 | 127 | } |
Akito914 | 1:635b9b4215b1 | 128 | |
Akito914 | 1:635b9b4215b1 | 129 | for(int axis = 0; axis < 3; axis++){ |
Akito914 | 1:635b9b4215b1 | 130 | omega_offset[axis] = accum[axis] / 1000.0; |
Akito914 | 1:635b9b4215b1 | 131 | } |
Akito914 | 1:635b9b4215b1 | 132 | |
Akito914 | 1:635b9b4215b1 | 133 | pc.printf("accum(%f, %f, %f)\r\n", accum[0], accum[1], accum[2]); |
Akito914 | 1:635b9b4215b1 | 134 | pc.printf("offset(%f, %f, %f)\r\n", omega_offset[0], omega_offset[1], omega_offset[2]); |
Akito914 | 1:635b9b4215b1 | 135 | |
Akito914 | 0:24c1e45f1a27 | 136 | while(1){ |
Akito914 | 0:24c1e45f1a27 | 137 | |
Akito914 | 0:24c1e45f1a27 | 138 | wait_ms(1); |
Akito914 | 0:24c1e45f1a27 | 139 | |
Akito914 | 0:24c1e45f1a27 | 140 | //pc.printf("whoAmI = 0x%.2x\r\n", getData(ADDR_WHO_AM_I)); |
Akito914 | 0:24c1e45f1a27 | 141 | |
Akito914 | 0:24c1e45f1a27 | 142 | //pc.printf("status = 0x%.2x\r\n", getData(ADDR_STATUS_REG)); |
Akito914 | 0:24c1e45f1a27 | 143 | |
Akito914 | 0:24c1e45f1a27 | 144 | getAngle(angle, false); |
Akito914 | 0:24c1e45f1a27 | 145 | pc.printf("(%f, %f, %f)\r\n", angle[0], angle[1], angle[2]); |
Akito914 | 0:24c1e45f1a27 | 146 | |
Akito914 | 1:635b9b4215b1 | 147 | pc.printf("temp = %d ℃\r\n", getTemp()); |
Akito914 | 1:635b9b4215b1 | 148 | |
Akito914 | 0:24c1e45f1a27 | 149 | //pc.printf("(%f, %f, %f)\r\n", get_angular_velocity('X'), get_angular_velocity('Y'), get_angular_velocity('Z')); |
Akito914 | 0:24c1e45f1a27 | 150 | |
Akito914 | 0:24c1e45f1a27 | 151 | |
Akito914 | 0:24c1e45f1a27 | 152 | //get_angular_velocity_Z(); |
Akito914 | 0:24c1e45f1a27 | 153 | |
Akito914 | 0:24c1e45f1a27 | 154 | |
Akito914 | 0:24c1e45f1a27 | 155 | //pc.printf("CTRL_REG1 = 0x%.2x\r\n", getData(ADDR_CTRL_REG1)); |
Akito914 | 0:24c1e45f1a27 | 156 | //pc.printf("CTRL_REG2 = 0x%.2x\r\n", getData(ADDR_CTRL_REG2)); |
Akito914 | 0:24c1e45f1a27 | 157 | //pc.printf("CTRL_REG3 = 0x%.2x\r\n", getData(ADDR_CTRL_REG3)); |
Akito914 | 0:24c1e45f1a27 | 158 | //pc.printf("CTRL_REG4 = 0x%.2x\r\n", getData(ADDR_CTRL_REG4)); |
Akito914 | 0:24c1e45f1a27 | 159 | //pc.printf("CTRL_REG5 = 0x%.2x\r\n", getData(ADDR_CTRL_REG5)); |
Akito914 | 0:24c1e45f1a27 | 160 | |
Akito914 | 0:24c1e45f1a27 | 161 | } |
Akito914 | 0:24c1e45f1a27 | 162 | } |
Akito914 | 0:24c1e45f1a27 | 163 | |
Akito914 | 0:24c1e45f1a27 | 164 | |
Akito914 | 0:24c1e45f1a27 | 165 |