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