ジャイロセンサを使用してLCDディスプレイに角速度情報を表示するプログラム
Dependencies: mbed
Dependents: line_trace_sample line_trace_sample_saitou line_trace_sample_tamada line_trace_ver2
mpu6050.cpp@0:abd3a0fd55a9, 2018-12-12 (annotated)
- Committer:
- satoshi1204
- Date:
- Wed Dec 12 12:42:30 2018 +0000
- Revision:
- 0:abd3a0fd55a9
- Child:
- 1:6f743fe9a027
implement MPU library
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
satoshi1204 | 0:abd3a0fd55a9 | 1 | #include "mpu6050.h" |
satoshi1204 | 0:abd3a0fd55a9 | 2 | |
satoshi1204 | 0:abd3a0fd55a9 | 3 | DigitalOut led(LED3); |
satoshi1204 | 0:abd3a0fd55a9 | 4 | |
satoshi1204 | 0:abd3a0fd55a9 | 5 | static double RATE_ACC_LIST[4] = { |
satoshi1204 | 0:abd3a0fd55a9 | 6 | 16384.0, |
satoshi1204 | 0:abd3a0fd55a9 | 7 | 8192.0, |
satoshi1204 | 0:abd3a0fd55a9 | 8 | 4096.0, |
satoshi1204 | 0:abd3a0fd55a9 | 9 | 2048.0, |
satoshi1204 | 0:abd3a0fd55a9 | 10 | }; |
satoshi1204 | 0:abd3a0fd55a9 | 11 | |
satoshi1204 | 0:abd3a0fd55a9 | 12 | static double RATE_GYRO_LIST[4] = { |
satoshi1204 | 0:abd3a0fd55a9 | 13 | 131.0, |
satoshi1204 | 0:abd3a0fd55a9 | 14 | 65.5, |
satoshi1204 | 0:abd3a0fd55a9 | 15 | 32.8, |
satoshi1204 | 0:abd3a0fd55a9 | 16 | 16.4, |
satoshi1204 | 0:abd3a0fd55a9 | 17 | }; |
satoshi1204 | 0:abd3a0fd55a9 | 18 | |
satoshi1204 | 0:abd3a0fd55a9 | 19 | MPU6050::MPU6050(PinName i2c_sda, PinName i2c_scl) : i2c(i2c_sda, i2c_scl), address(208), rate_accelemeter(16384.0), rate_gyroscope(131.0) |
satoshi1204 | 0:abd3a0fd55a9 | 20 | { |
satoshi1204 | 0:abd3a0fd55a9 | 21 | setMaxScale(MAX_ACC_2G, MAX_GYRO_250degpersec); |
satoshi1204 | 0:abd3a0fd55a9 | 22 | } |
satoshi1204 | 0:abd3a0fd55a9 | 23 | |
satoshi1204 | 0:abd3a0fd55a9 | 24 | MPU6050::~MPU6050() |
satoshi1204 | 0:abd3a0fd55a9 | 25 | { |
satoshi1204 | 0:abd3a0fd55a9 | 26 | } |
satoshi1204 | 0:abd3a0fd55a9 | 27 | |
satoshi1204 | 0:abd3a0fd55a9 | 28 | char MPU6050::readByte(char reg) |
satoshi1204 | 0:abd3a0fd55a9 | 29 | { |
satoshi1204 | 0:abd3a0fd55a9 | 30 | int result_write = i2c.write(address, ®, 1); |
satoshi1204 | 0:abd3a0fd55a9 | 31 | if(result_write) { |
satoshi1204 | 0:abd3a0fd55a9 | 32 | // failed |
satoshi1204 | 0:abd3a0fd55a9 | 33 | //pc.printf("i2c error\r\n"); |
satoshi1204 | 0:abd3a0fd55a9 | 34 | led = 1; |
satoshi1204 | 0:abd3a0fd55a9 | 35 | } |
satoshi1204 | 0:abd3a0fd55a9 | 36 | char read_data; |
satoshi1204 | 0:abd3a0fd55a9 | 37 | int result_read = i2c.read(address, &read_data, 1); |
satoshi1204 | 0:abd3a0fd55a9 | 38 | if(result_read) { |
satoshi1204 | 0:abd3a0fd55a9 | 39 | // failed |
satoshi1204 | 0:abd3a0fd55a9 | 40 | //pc.printf("i2c error\r\n"); |
satoshi1204 | 0:abd3a0fd55a9 | 41 | led = 1; |
satoshi1204 | 0:abd3a0fd55a9 | 42 | } |
satoshi1204 | 0:abd3a0fd55a9 | 43 | return read_data; |
satoshi1204 | 0:abd3a0fd55a9 | 44 | } |
satoshi1204 | 0:abd3a0fd55a9 | 45 | |
satoshi1204 | 0:abd3a0fd55a9 | 46 | void MPU6050::writeByte(char reg, char data) |
satoshi1204 | 0:abd3a0fd55a9 | 47 | { |
satoshi1204 | 0:abd3a0fd55a9 | 48 | char buf[2]; |
satoshi1204 | 0:abd3a0fd55a9 | 49 | buf[0] = reg; |
satoshi1204 | 0:abd3a0fd55a9 | 50 | buf[1] = data; |
satoshi1204 | 0:abd3a0fd55a9 | 51 | int result = i2c.write(address, buf, 2); |
satoshi1204 | 0:abd3a0fd55a9 | 52 | if(result) { |
satoshi1204 | 0:abd3a0fd55a9 | 53 | // failed |
satoshi1204 | 0:abd3a0fd55a9 | 54 | //pc.printf("i2c error\r\n"); |
satoshi1204 | 0:abd3a0fd55a9 | 55 | led = 1; |
satoshi1204 | 0:abd3a0fd55a9 | 56 | } |
satoshi1204 | 0:abd3a0fd55a9 | 57 | } |
satoshi1204 | 0:abd3a0fd55a9 | 58 | |
satoshi1204 | 0:abd3a0fd55a9 | 59 | void MPU6050::readAccelemeter(double &acc_x, double &acc_y, double &acc_z) |
satoshi1204 | 0:abd3a0fd55a9 | 60 | { |
satoshi1204 | 0:abd3a0fd55a9 | 61 | char h, l; |
satoshi1204 | 0:abd3a0fd55a9 | 62 | short x, y, z; |
satoshi1204 | 0:abd3a0fd55a9 | 63 | // read X |
satoshi1204 | 0:abd3a0fd55a9 | 64 | h = readByte(0x3B); |
satoshi1204 | 0:abd3a0fd55a9 | 65 | l = readByte(0x3C); |
satoshi1204 | 0:abd3a0fd55a9 | 66 | x = h << 8 | l; |
satoshi1204 | 0:abd3a0fd55a9 | 67 | // read Y |
satoshi1204 | 0:abd3a0fd55a9 | 68 | h = readByte(0x3D); |
satoshi1204 | 0:abd3a0fd55a9 | 69 | l = readByte(0x3E); |
satoshi1204 | 0:abd3a0fd55a9 | 70 | y = h << 8 | l; |
satoshi1204 | 0:abd3a0fd55a9 | 71 | // read Z |
satoshi1204 | 0:abd3a0fd55a9 | 72 | h = readByte(0x3F); |
satoshi1204 | 0:abd3a0fd55a9 | 73 | l = readByte(0x40); |
satoshi1204 | 0:abd3a0fd55a9 | 74 | z = h << 8 | l; |
satoshi1204 | 0:abd3a0fd55a9 | 75 | acc_x = (double)x / rate_accelemeter; |
satoshi1204 | 0:abd3a0fd55a9 | 76 | acc_y = (double)y / rate_accelemeter; |
satoshi1204 | 0:abd3a0fd55a9 | 77 | acc_z = (double)z / rate_accelemeter; |
satoshi1204 | 0:abd3a0fd55a9 | 78 | } |
satoshi1204 | 0:abd3a0fd55a9 | 79 | |
satoshi1204 | 0:abd3a0fd55a9 | 80 | void MPU6050::readGyroscope(double &gyro_x, double &gyro_y, double &gyro_z) |
satoshi1204 | 0:abd3a0fd55a9 | 81 | { |
satoshi1204 | 0:abd3a0fd55a9 | 82 | char h, l; |
satoshi1204 | 0:abd3a0fd55a9 | 83 | short x, y, z; |
satoshi1204 | 0:abd3a0fd55a9 | 84 | // read X |
satoshi1204 | 0:abd3a0fd55a9 | 85 | h = readByte(0x43); |
satoshi1204 | 0:abd3a0fd55a9 | 86 | l = readByte(0x44); |
satoshi1204 | 0:abd3a0fd55a9 | 87 | x = h << 8 | l; |
satoshi1204 | 0:abd3a0fd55a9 | 88 | // read Y |
satoshi1204 | 0:abd3a0fd55a9 | 89 | h = readByte(0x45); |
satoshi1204 | 0:abd3a0fd55a9 | 90 | l = readByte(0x46); |
satoshi1204 | 0:abd3a0fd55a9 | 91 | y = h << 8 | l; |
satoshi1204 | 0:abd3a0fd55a9 | 92 | // read Z |
satoshi1204 | 0:abd3a0fd55a9 | 93 | h = readByte(0x47); |
satoshi1204 | 0:abd3a0fd55a9 | 94 | l = readByte(0x48); |
satoshi1204 | 0:abd3a0fd55a9 | 95 | z = h << 8 | l; |
satoshi1204 | 0:abd3a0fd55a9 | 96 | gyro_x = (double)x / rate_gyroscope; |
satoshi1204 | 0:abd3a0fd55a9 | 97 | gyro_y = (double)y / rate_gyroscope; |
satoshi1204 | 0:abd3a0fd55a9 | 98 | gyro_z = (double)z / rate_gyroscope; |
satoshi1204 | 0:abd3a0fd55a9 | 99 | } |
satoshi1204 | 0:abd3a0fd55a9 | 100 | |
satoshi1204 | 0:abd3a0fd55a9 | 101 | void MPU6050::getMaxScale(int &acc, int &gyro) |
satoshi1204 | 0:abd3a0fd55a9 | 102 | { |
satoshi1204 | 0:abd3a0fd55a9 | 103 | char reg; |
satoshi1204 | 0:abd3a0fd55a9 | 104 | reg = readByte(0x1C); |
satoshi1204 | 0:abd3a0fd55a9 | 105 | acc = reg; |
satoshi1204 | 0:abd3a0fd55a9 | 106 | reg = readByte(0x1B); |
satoshi1204 | 0:abd3a0fd55a9 | 107 | gyro = reg; |
satoshi1204 | 0:abd3a0fd55a9 | 108 | } |
satoshi1204 | 0:abd3a0fd55a9 | 109 | |
satoshi1204 | 0:abd3a0fd55a9 | 110 | void MPU6050::setMaxScale(int max_acc, int max_gyro) |
satoshi1204 | 0:abd3a0fd55a9 | 111 | { |
satoshi1204 | 0:abd3a0fd55a9 | 112 | switch(max_acc) { |
satoshi1204 | 0:abd3a0fd55a9 | 113 | case MAX_ACC_2G: |
satoshi1204 | 0:abd3a0fd55a9 | 114 | writeByte(0x1C, 0x00); |
satoshi1204 | 0:abd3a0fd55a9 | 115 | break; |
satoshi1204 | 0:abd3a0fd55a9 | 116 | case MAX_ACC_4G: |
satoshi1204 | 0:abd3a0fd55a9 | 117 | writeByte(0x1C, 0x08); |
satoshi1204 | 0:abd3a0fd55a9 | 118 | break; |
satoshi1204 | 0:abd3a0fd55a9 | 119 | case MAX_ACC_8G: |
satoshi1204 | 0:abd3a0fd55a9 | 120 | writeByte(0x1C, 0x10); |
satoshi1204 | 0:abd3a0fd55a9 | 121 | break; |
satoshi1204 | 0:abd3a0fd55a9 | 122 | case MAX_ACC_16G: |
satoshi1204 | 0:abd3a0fd55a9 | 123 | writeByte(0x1C, 0x18); |
satoshi1204 | 0:abd3a0fd55a9 | 124 | break; |
satoshi1204 | 0:abd3a0fd55a9 | 125 | } |
satoshi1204 | 0:abd3a0fd55a9 | 126 | rate_accelemeter = RATE_ACC_LIST[max_acc]; |
satoshi1204 | 0:abd3a0fd55a9 | 127 | switch(max_gyro) { |
satoshi1204 | 0:abd3a0fd55a9 | 128 | case MAX_GYRO_250degpersec: |
satoshi1204 | 0:abd3a0fd55a9 | 129 | writeByte(0x1B, 0x00); |
satoshi1204 | 0:abd3a0fd55a9 | 130 | break; |
satoshi1204 | 0:abd3a0fd55a9 | 131 | case MAX_GYRO_500degpersec: |
satoshi1204 | 0:abd3a0fd55a9 | 132 | writeByte(0x1B, 0x08); |
satoshi1204 | 0:abd3a0fd55a9 | 133 | break; |
satoshi1204 | 0:abd3a0fd55a9 | 134 | case MAX_GYRO_1000degpersec: |
satoshi1204 | 0:abd3a0fd55a9 | 135 | writeByte(0x1B, 0x10); |
satoshi1204 | 0:abd3a0fd55a9 | 136 | break; |
satoshi1204 | 0:abd3a0fd55a9 | 137 | case MAX_GYRO_2000degpersec: |
satoshi1204 | 0:abd3a0fd55a9 | 138 | writeByte(0x1B, 0x18); |
satoshi1204 | 0:abd3a0fd55a9 | 139 | break; |
satoshi1204 | 0:abd3a0fd55a9 | 140 | } |
satoshi1204 | 0:abd3a0fd55a9 | 141 | rate_gyroscope = RATE_GYRO_LIST[max_gyro]; |
satoshi1204 | 0:abd3a0fd55a9 | 142 | } |