Akito / Mbed 2 deprecated L3GD20_test

gyro sensor L3GD20 test

Dependencies:   mbed

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?

UserRevisionLine numberNew 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