ジャイロセンサを使用してLCDディスプレイに角速度情報を表示するプログラム
Dependencies: mbed
Dependents: line_trace_sample line_trace_sample_saitou line_trace_sample_tamada line_trace_ver2
Diff: mpu6050.cpp
- Revision:
- 0:abd3a0fd55a9
- Child:
- 1:6f743fe9a027
diff -r 000000000000 -r abd3a0fd55a9 mpu6050.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mpu6050.cpp Wed Dec 12 12:42:30 2018 +0000 @@ -0,0 +1,142 @@ +#include "mpu6050.h" + +DigitalOut led(LED3); + +static double RATE_ACC_LIST[4] = { + 16384.0, + 8192.0, + 4096.0, + 2048.0, +}; + +static double RATE_GYRO_LIST[4] = { + 131.0, + 65.5, + 32.8, + 16.4, +}; + +MPU6050::MPU6050(PinName i2c_sda, PinName i2c_scl) : i2c(i2c_sda, i2c_scl), address(208), rate_accelemeter(16384.0), rate_gyroscope(131.0) +{ + setMaxScale(MAX_ACC_2G, MAX_GYRO_250degpersec); +} + +MPU6050::~MPU6050() +{ +} + +char MPU6050::readByte(char reg) +{ + int result_write = i2c.write(address, ®, 1); + if(result_write) { + // failed + //pc.printf("i2c error\r\n"); + led = 1; + } + char read_data; + int result_read = i2c.read(address, &read_data, 1); + if(result_read) { + // failed + //pc.printf("i2c error\r\n"); + led = 1; + } + return read_data; +} + +void MPU6050::writeByte(char reg, char data) +{ + char buf[2]; + buf[0] = reg; + buf[1] = data; + int result = i2c.write(address, buf, 2); + if(result) { + // failed + //pc.printf("i2c error\r\n"); + led = 1; + } +} + +void MPU6050::readAccelemeter(double &acc_x, double &acc_y, double &acc_z) +{ + char h, l; + short x, y, z; + // read X + h = readByte(0x3B); + l = readByte(0x3C); + x = h << 8 | l; + // read Y + h = readByte(0x3D); + l = readByte(0x3E); + y = h << 8 | l; + // read Z + h = readByte(0x3F); + l = readByte(0x40); + z = h << 8 | l; + acc_x = (double)x / rate_accelemeter; + acc_y = (double)y / rate_accelemeter; + acc_z = (double)z / rate_accelemeter; +} + +void MPU6050::readGyroscope(double &gyro_x, double &gyro_y, double &gyro_z) +{ + char h, l; + short x, y, z; + // read X + h = readByte(0x43); + l = readByte(0x44); + x = h << 8 | l; + // read Y + h = readByte(0x45); + l = readByte(0x46); + y = h << 8 | l; + // read Z + h = readByte(0x47); + l = readByte(0x48); + z = h << 8 | l; + gyro_x = (double)x / rate_gyroscope; + gyro_y = (double)y / rate_gyroscope; + gyro_z = (double)z / rate_gyroscope; +} + +void MPU6050::getMaxScale(int &acc, int &gyro) +{ + char reg; + reg = readByte(0x1C); + acc = reg; + reg = readByte(0x1B); + gyro = reg; +} + +void MPU6050::setMaxScale(int max_acc, int max_gyro) +{ + switch(max_acc) { + case MAX_ACC_2G: + writeByte(0x1C, 0x00); + break; + case MAX_ACC_4G: + writeByte(0x1C, 0x08); + break; + case MAX_ACC_8G: + writeByte(0x1C, 0x10); + break; + case MAX_ACC_16G: + writeByte(0x1C, 0x18); + break; + } + rate_accelemeter = RATE_ACC_LIST[max_acc]; + switch(max_gyro) { + case MAX_GYRO_250degpersec: + writeByte(0x1B, 0x00); + break; + case MAX_GYRO_500degpersec: + writeByte(0x1B, 0x08); + break; + case MAX_GYRO_1000degpersec: + writeByte(0x1B, 0x10); + break; + case MAX_GYRO_2000degpersec: + writeByte(0x1B, 0x18); + break; + } + rate_gyroscope = RATE_GYRO_LIST[max_gyro]; +}