Akito / Mbed 2 deprecated L3GD20_test

gyro sensor L3GD20 test

Dependencies:   mbed

main.cpp@0:24c1e45f1a27, 2017-09-14 (annotated)

Committer:
Akito914
Date:
Thu Sep 14 09:02:51 2017 +0000
Revision:
0:24c1e45f1a27
Child:
1:635b9b4215b1
??????

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 0:24c1e45f1a27 12 #define ADDR_STATUS_REG 0x27
Akito914 0:24c1e45f1a27 13 #define ADDR_OUT_X_L 0x28
Akito914 0:24c1e45f1a27 14 #define ADDR_OUT_X_H 0x29
Akito914 0:24c1e45f1a27 15 #define ADDR_OUT_Y_L 0x2A
Akito914 0:24c1e45f1a27 16 #define ADDR_OUT_Y_H 0x2B
Akito914 0:24c1e45f1a27 17 #define ADDR_OUT_Z_L 0x2C
Akito914 0:24c1e45f1a27 18 #define ADDR_OUT_Z_H 0x2D
Akito914 0:24c1e45f1a27 19
Akito914 0:24c1e45f1a27 20 I2C i2c(PB_9, PB_8);
Akito914 0:24c1e45f1a27 21
Akito914 0:24c1e45f1a27 22 DigitalOut myled(LED1);
Akito914 0:24c1e45f1a27 23
Akito914 0:24c1e45f1a27 24 Serial pc(USBTX, USBRX);
Akito914 0:24c1e45f1a27 25
Akito914 0:24c1e45f1a27 26 char getData(char addr){
Akito914 0:24c1e45f1a27 27 char readData;
Akito914 0:24c1e45f1a27 28 i2c.write(I2C_ADDRESS, &addr, 1, true);
Akito914 0:24c1e45f1a27 29 i2c.read(I2C_ADDRESS | 0x01, &readData, 1);
Akito914 0:24c1e45f1a27 30 return readData;
Akito914 0:24c1e45f1a27 31 }
Akito914 0:24c1e45f1a27 32
Akito914 0:24c1e45f1a27 33 void setData(char addr, char data){
Akito914 0:24c1e45f1a27 34 char sendData[] = {addr, data};
Akito914 0:24c1e45f1a27 35 // i2c.write(I2C_ADDRESS, &addr, 1, true);
Akito914 0:24c1e45f1a27 36 // i2c.write(I2C_ADDRESS, &data, 1);
Akito914 0:24c1e45f1a27 37 i2c.write(I2C_ADDRESS, sendData, 2);
Akito914 0:24c1e45f1a27 38 return;
Akito914 0:24c1e45f1a27 39 }
Akito914 0:24c1e45f1a27 40
Akito914 0:24c1e45f1a27 41 double get_angular_velocity(char axis){
Akito914 0:24c1e45f1a27 42 int omega_l, omega_h;
Akito914 0:24c1e45f1a27 43 int16_t omega;
Akito914 0:24c1e45f1a27 44 switch(axis){
Akito914 0:24c1e45f1a27 45 case 'X': case 'x':
Akito914 0:24c1e45f1a27 46 omega_l = getData(ADDR_OUT_X_L);
Akito914 0:24c1e45f1a27 47 omega_h = getData(ADDR_OUT_X_H);
Akito914 0:24c1e45f1a27 48 break;
Akito914 0:24c1e45f1a27 49 case 'Y': case 'y':
Akito914 0:24c1e45f1a27 50 omega_l = getData(ADDR_OUT_Y_L);
Akito914 0:24c1e45f1a27 51 omega_h = getData(ADDR_OUT_Y_H);
Akito914 0:24c1e45f1a27 52 break;
Akito914 0:24c1e45f1a27 53 case 'Z': case 'z':
Akito914 0:24c1e45f1a27 54 omega_l = getData(ADDR_OUT_Z_L);
Akito914 0:24c1e45f1a27 55 omega_h = getData(ADDR_OUT_Z_H);
Akito914 0:24c1e45f1a27 56 break;
Akito914 0:24c1e45f1a27 57 default : return 0.0;
Akito914 0:24c1e45f1a27 58 }
Akito914 0:24c1e45f1a27 59 omega = omega_h << 8 | omega_l;
Akito914 0:24c1e45f1a27 60 //return omega * 0.00875; // +- 250 dps mode
Akito914 0:24c1e45f1a27 61 return omega * 0.01750; // +- 500 dps mode
Akito914 0:24c1e45f1a27 62 //return omega * 0.070; // +- 2000 dps mode
Akito914 0:24c1e45f1a27 63
Akito914 0:24c1e45f1a27 64 }
Akito914 0:24c1e45f1a27 65
Akito914 0:24c1e45f1a27 66 double trapezoid_integr(double data, double ex_data, double period){
Akito914 0:24c1e45f1a27 67 return (data + ex_data) * period / 2.0;
Akito914 0:24c1e45f1a27 68 }
Akito914 0:24c1e45f1a27 69
Akito914 0:24c1e45f1a27 70 Timer gyro_timer;
Akito914 0:24c1e45f1a27 71
Akito914 0:24c1e45f1a27 72 void getAngle(double *result, bool reset){
Akito914 0:24c1e45f1a27 73 static double angle_list[3] = {0, 0, 0};
Akito914 0:24c1e45f1a27 74 static double ex_omega[3] = {0, 0, 0};
Akito914 0:24c1e45f1a27 75 int read_period_us;
Akito914 0:24c1e45f1a27 76 double omega[3] = {0, 0, 0};
Akito914 0:24c1e45f1a27 77
Akito914 0:24c1e45f1a27 78 omega[0] = get_angular_velocity('X');
Akito914 0:24c1e45f1a27 79 omega[1] = get_angular_velocity('Y');
Akito914 0:24c1e45f1a27 80 omega[2] = get_angular_velocity('Z');
Akito914 0:24c1e45f1a27 81
Akito914 0:24c1e45f1a27 82 read_period_us = gyro_timer.read_us();
Akito914 0:24c1e45f1a27 83 gyro_timer.reset();
Akito914 0:24c1e45f1a27 84
Akito914 0:24c1e45f1a27 85 for(int axis = 0; axis < 3; axis++){
Akito914 0:24c1e45f1a27 86 angle_list[axis] += trapezoid_integr(omega[axis], ex_omega[axis], read_period_us / 1000000.0);
Akito914 0:24c1e45f1a27 87 ex_omega[axis] = omega[axis];
Akito914 0:24c1e45f1a27 88 if(reset)angle_list[axis] = 0;
Akito914 0:24c1e45f1a27 89 result[axis] = angle_list[axis];
Akito914 0:24c1e45f1a27 90 }
Akito914 0:24c1e45f1a27 91 return;
Akito914 0:24c1e45f1a27 92 }
Akito914 0:24c1e45f1a27 93
Akito914 0:24c1e45f1a27 94 int main(){
Akito914 0:24c1e45f1a27 95 double angle[3] = {0, 0, 0};
Akito914 0:24c1e45f1a27 96
Akito914 0:24c1e45f1a27 97 i2c.frequency(400000);
Akito914 0:24c1e45f1a27 98
Akito914 0:24c1e45f1a27 99 pc.baud(115200);
Akito914 0:24c1e45f1a27 100
Akito914 0:24c1e45f1a27 101 pc.printf("Hello\r\n");
Akito914 0:24c1e45f1a27 102
Akito914 0:24c1e45f1a27 103 pc.printf("whoAmI = 0x%.2x\r\n", getData(ADDR_WHO_AM_I));
Akito914 0:24c1e45f1a27 104
Akito914 0:24c1e45f1a27 105 setData(ADDR_CTRL_REG1, 0x0f);
Akito914 0:24c1e45f1a27 106
Akito914 0:24c1e45f1a27 107 //setData(ADDR_CTRL_REG4, 0b00000000); // +- 250 dps mode
Akito914 0:24c1e45f1a27 108 setData(ADDR_CTRL_REG4, 0b00010000); // +- 500 dps mode
Akito914 0:24c1e45f1a27 109 //setData(ADDR_CTRL_REG4, 0b00100000); // +- 2000 dps mode
Akito914 0:24c1e45f1a27 110
Akito914 0:24c1e45f1a27 111 gyro_timer.start();
Akito914 0:24c1e45f1a27 112
Akito914 0:24c1e45f1a27 113 while(1){
Akito914 0:24c1e45f1a27 114
Akito914 0:24c1e45f1a27 115 wait_ms(1);
Akito914 0:24c1e45f1a27 116
Akito914 0:24c1e45f1a27 117 //pc.printf("whoAmI = 0x%.2x\r\n", getData(ADDR_WHO_AM_I));
Akito914 0:24c1e45f1a27 118
Akito914 0:24c1e45f1a27 119 //pc.printf("status = 0x%.2x\r\n", getData(ADDR_STATUS_REG));
Akito914 0:24c1e45f1a27 120
Akito914 0:24c1e45f1a27 121 getAngle(angle, false);
Akito914 0:24c1e45f1a27 122 pc.printf("(%f, %f, %f)\r\n", angle[0], angle[1], angle[2]);
Akito914 0:24c1e45f1a27 123
Akito914 0:24c1e45f1a27 124 //pc.printf("(%f, %f, %f)\r\n", get_angular_velocity('X'), get_angular_velocity('Y'), get_angular_velocity('Z'));
Akito914 0:24c1e45f1a27 125
Akito914 0:24c1e45f1a27 126
Akito914 0:24c1e45f1a27 127 //get_angular_velocity_Z();
Akito914 0:24c1e45f1a27 128
Akito914 0:24c1e45f1a27 129
Akito914 0:24c1e45f1a27 130 //pc.printf("CTRL_REG1 = 0x%.2x\r\n", getData(ADDR_CTRL_REG1));
Akito914 0:24c1e45f1a27 131 //pc.printf("CTRL_REG2 = 0x%.2x\r\n", getData(ADDR_CTRL_REG2));
Akito914 0:24c1e45f1a27 132 //pc.printf("CTRL_REG3 = 0x%.2x\r\n", getData(ADDR_CTRL_REG3));
Akito914 0:24c1e45f1a27 133 //pc.printf("CTRL_REG4 = 0x%.2x\r\n", getData(ADDR_CTRL_REG4));
Akito914 0:24c1e45f1a27 134 //pc.printf("CTRL_REG5 = 0x%.2x\r\n", getData(ADDR_CTRL_REG5));
Akito914 0:24c1e45f1a27 135
Akito914 0:24c1e45f1a27 136 }
Akito914 0:24c1e45f1a27 137 }
Akito914 0:24c1e45f1a27 138
Akito914 0:24c1e45f1a27 139
Akito914 0:24c1e45f1a27 140