![](/media/cache/profiles/P1030212_resized.JPG.50x50_q85.jpg)
明石高専ロボ研 mbedライブラリ
Dependents: MDD_L432KC USB2RS485 pathtracking odometry ... more
Diff: gy521.cpp
- Revision:
- 0:ca84ed7518f5
- Child:
- 3:28c77df7c0b6
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/gy521.cpp Thu Jan 02 09:30:06 2020 +0000 @@ -0,0 +1,119 @@ +#include "gy521.hpp" + +const double GY521_LSB_MAP[4] = {131, 65.5, 32.8, 16.4}; +const unsigned int dev_id = 0x68 << 1; +enum GY521RegisterMap { + WHO_AM_I = 0x75, + PWR_MGMT_1 = 0x6B, + LPF = 0x1A, + FS_SEL = 0x1B, + AFS_SEL = 0x1C, + ACCEL_XOUT_H = 0x3B, + ACCEL_YOUT_H = 0x3D, + ACCEL_ZOUT_H = 0x3F, + //TEMPERATURE = 0x41, + //GYRO_XOUT_H = 0x43, + //GYRO_YOUT_H = 0x45, + GYRO_ZOUT_H = 0x47 +}; + +GY521::GY521(I2C &i2c,int bit,int calibration,double user_reg):i2c_(i2c),bit_(bit),calibration_(calibration){ + char check; + char data[2] = {WHO_AM_I,0x00}; + i2c_.write(dev_id,data,1,true); + i2c_.read(dev_id,&check,1); + if(check == dev_id >> 1){ + data[0] = PWR_MGMT_1; + i2c_.write(dev_id,data,1,true); + i2c_.read(dev_id,&check,1); + if(check == 0x40){ + i2c_.write(dev_id,data,2); + } + } + data[0] = LPF; + data[1] = 0x00; + i2c_.write(dev_id,data,2); + data[0] = AFS_SEL; + data[1] = 0x00; + i2c_.write(dev_id,data,2); + short accel_x_now = 0,accel_y_now = 0,accel_z_now = 0; + double accel_x_aver = 0, accel_y_aver = 0, accel_z_aver = 0; + for (int i = 0; i < calibration_; ++i) { + accel_x_now = gyroRead2(ACCEL_XOUT_H); + accel_y_now = gyroRead2(ACCEL_YOUT_H); + accel_z_now = gyroRead2(ACCEL_ZOUT_H); + accel_x_aver += accel_x_now; + accel_y_aver += accel_y_now; + accel_z_aver += accel_z_now; + } + accel_x_aver /= calibration_; + accel_y_aver /= calibration_; + accel_z_aver /= calibration_; + double gyro_reg = hypot(hypot(accel_x_aver, accel_y_aver), accel_z_aver) / accel_z_aver; + data[0] = FS_SEL; + data[1] = bit << 3; + i2c_.write(dev_id,data,2); + gyro_LSB_ = GY521_LSB_MAP[bit_] / gyro_reg / user_reg; + + short gyro_z_now_; + gyro_z_aver_ = 0; + for (int i = 0; i < calibration_; ++i) { + gyro_z_now_ = gyroRead2(GYRO_ZOUT_H); + gyro_z_aver_ += gyro_z_now_; + } + gyro_z_aver_ /= calibration_; + + yaw = diff_yaw_ = 0; + timer_.start(); + flag_ = false; +} + +void GY521::updata(){ + if(flag_){ + return; + } + short gyro_z_now_ = gyroRead2(GYRO_ZOUT_H); + gyro_z_now_ = ((double)gyro_z_now_ - gyro_z_aver_) / gyro_LSB_; + diff_yaw_ = (gyro_z_now_ + gyro_z_prev_) / 2 * timer_.read(); + timer_.reset(); + yaw += diff_yaw_; + gyro_z_prev_ = gyro_z_now_; + //temp = (double)gyroRead2(TEMPERATURE)/340+36.53; + if (yaw > 180) { + yaw -= 360; + } else if (yaw <= -180) { + yaw += 360; + } +} + +void GY521::reset(int user){ + yaw = user; + short gyro_z_now_; + flag_ = true; + gyro_z_aver_ = 0; + for (int i = 0; i < calibration_; i++) { + gyro_z_now_ = gyroRead2(GYRO_ZOUT_H); + gyro_z_aver_ += gyro_z_now_; + } + gyro_z_aver_ /= calibration_; + flag_ = false; +} + +double GY521::checkStatus(int mode) { + if (mode == 0) { + return gyro_LSB_; + } else if (mode == 1) { + return gyro_z_aver_; + } else if (mode == 2) { + return acos(gyro_LSB_ / GY521_LSB_MAP[bit_]); + } + return 0; +} + +int16_t GY521::gyroRead2(enum GY521RegisterMap reg) { + char data[2]; + char addr = reg; + i2c_.write(dev_id,&addr,1,true); + i2c_.read(dev_id,data,2); + return (data[0] << 8) + data[1]; +} \ No newline at end of file