go-roboB / Mbed 2 deprecated arrc_mbed

明石高専ロボ研 mbedライブラリ

Dependencies:   mbed

gy521.cpp@3:28c77df7c0b6, 2020-01-06 (annotated)

Committer:
TanakaRobo
Date:
Mon Jan 06 11:06:26 2020 +0000
Revision:
3:28c77df7c0b6
Parent:
0:ca84ed7518f5
Child:
4:39ef4d91dc34
send not available

Who changed what in which revision?

UserRevisionLine numberNew contents of line
TanakaRobo 0:ca84ed7518f5 1 #include "gy521.hpp"
TanakaRobo 0:ca84ed7518f5 2
TanakaRobo 0:ca84ed7518f5 3 const double GY521_LSB_MAP[4] = {131, 65.5, 32.8, 16.4};
TanakaRobo 0:ca84ed7518f5 4 const unsigned int dev_id = 0x68 << 1;
TanakaRobo 0:ca84ed7518f5 5 enum GY521RegisterMap {
TanakaRobo 0:ca84ed7518f5 6 WHO_AM_I = 0x75,
TanakaRobo 0:ca84ed7518f5 7 PWR_MGMT_1 = 0x6B,
TanakaRobo 0:ca84ed7518f5 8 LPF = 0x1A,
TanakaRobo 0:ca84ed7518f5 9 FS_SEL = 0x1B,
TanakaRobo 0:ca84ed7518f5 10 AFS_SEL = 0x1C,
TanakaRobo 0:ca84ed7518f5 11 ACCEL_XOUT_H = 0x3B,
TanakaRobo 0:ca84ed7518f5 12 ACCEL_YOUT_H = 0x3D,
TanakaRobo 0:ca84ed7518f5 13 ACCEL_ZOUT_H = 0x3F,
TanakaRobo 0:ca84ed7518f5 14 //TEMPERATURE = 0x41,
TanakaRobo 0:ca84ed7518f5 15 //GYRO_XOUT_H = 0x43,
TanakaRobo 0:ca84ed7518f5 16 //GYRO_YOUT_H = 0x45,
TanakaRobo 0:ca84ed7518f5 17 GYRO_ZOUT_H = 0x47
TanakaRobo 0:ca84ed7518f5 18 };
TanakaRobo 0:ca84ed7518f5 19
TanakaRobo 0:ca84ed7518f5 20 GY521::GY521(I2C &i2c,int bit,int calibration,double user_reg):i2c_(i2c),bit_(bit),calibration_(calibration){
TanakaRobo 0:ca84ed7518f5 21 char check;
TanakaRobo 0:ca84ed7518f5 22 char data[2] = {WHO_AM_I,0x00};
TanakaRobo 0:ca84ed7518f5 23 i2c_.write(dev_id,data,1,true);
TanakaRobo 0:ca84ed7518f5 24 i2c_.read(dev_id,&check,1);
TanakaRobo 0:ca84ed7518f5 25 if(check == dev_id >> 1){
TanakaRobo 0:ca84ed7518f5 26 data[0] = PWR_MGMT_1;
TanakaRobo 0:ca84ed7518f5 27 i2c_.write(dev_id,data,1,true);
TanakaRobo 0:ca84ed7518f5 28 i2c_.read(dev_id,&check,1);
TanakaRobo 0:ca84ed7518f5 29 if(check == 0x40){
TanakaRobo 0:ca84ed7518f5 30 i2c_.write(dev_id,data,2);
TanakaRobo 0:ca84ed7518f5 31 }
TanakaRobo 0:ca84ed7518f5 32 }
TanakaRobo 0:ca84ed7518f5 33 data[0] = LPF;
TanakaRobo 0:ca84ed7518f5 34 data[1] = 0x00;
TanakaRobo 0:ca84ed7518f5 35 i2c_.write(dev_id,data,2);
TanakaRobo 0:ca84ed7518f5 36 data[0] = AFS_SEL;
TanakaRobo 0:ca84ed7518f5 37 data[1] = 0x00;
TanakaRobo 0:ca84ed7518f5 38 i2c_.write(dev_id,data,2);
TanakaRobo 0:ca84ed7518f5 39 short accel_x_now = 0,accel_y_now = 0,accel_z_now = 0;
TanakaRobo 0:ca84ed7518f5 40 double accel_x_aver = 0, accel_y_aver = 0, accel_z_aver = 0;
TanakaRobo 0:ca84ed7518f5 41 for (int i = 0; i < calibration_; ++i) {
TanakaRobo 0:ca84ed7518f5 42 accel_x_now = gyroRead2(ACCEL_XOUT_H);
TanakaRobo 0:ca84ed7518f5 43 accel_y_now = gyroRead2(ACCEL_YOUT_H);
TanakaRobo 0:ca84ed7518f5 44 accel_z_now = gyroRead2(ACCEL_ZOUT_H);
TanakaRobo 0:ca84ed7518f5 45 accel_x_aver += accel_x_now;
TanakaRobo 0:ca84ed7518f5 46 accel_y_aver += accel_y_now;
TanakaRobo 0:ca84ed7518f5 47 accel_z_aver += accel_z_now;
TanakaRobo 0:ca84ed7518f5 48 }
TanakaRobo 0:ca84ed7518f5 49 accel_x_aver /= calibration_;
TanakaRobo 0:ca84ed7518f5 50 accel_y_aver /= calibration_;
TanakaRobo 0:ca84ed7518f5 51 accel_z_aver /= calibration_;
TanakaRobo 0:ca84ed7518f5 52 double gyro_reg = hypot(hypot(accel_x_aver, accel_y_aver), accel_z_aver) / accel_z_aver;
TanakaRobo 0:ca84ed7518f5 53 data[0] = FS_SEL;
TanakaRobo 0:ca84ed7518f5 54 data[1] = bit << 3;
TanakaRobo 0:ca84ed7518f5 55 i2c_.write(dev_id,data,2);
TanakaRobo 0:ca84ed7518f5 56 gyro_LSB_ = GY521_LSB_MAP[bit_] / gyro_reg / user_reg;
TanakaRobo 0:ca84ed7518f5 57
TanakaRobo 0:ca84ed7518f5 58 short gyro_z_now_;
TanakaRobo 0:ca84ed7518f5 59 gyro_z_aver_ = 0;
TanakaRobo 0:ca84ed7518f5 60 for (int i = 0; i < calibration_; ++i) {
TanakaRobo 0:ca84ed7518f5 61 gyro_z_now_ = gyroRead2(GYRO_ZOUT_H);
TanakaRobo 0:ca84ed7518f5 62 gyro_z_aver_ += gyro_z_now_;
TanakaRobo 0:ca84ed7518f5 63 }
TanakaRobo 0:ca84ed7518f5 64 gyro_z_aver_ /= calibration_;
TanakaRobo 0:ca84ed7518f5 65
TanakaRobo 0:ca84ed7518f5 66 yaw = diff_yaw_ = 0;
TanakaRobo 0:ca84ed7518f5 67 timer_.start();
TanakaRobo 0:ca84ed7518f5 68 flag_ = false;
TanakaRobo 0:ca84ed7518f5 69 }
TanakaRobo 0:ca84ed7518f5 70
TanakaRobo 3:28c77df7c0b6 71 void GY521::update(){
TanakaRobo 0:ca84ed7518f5 72 if(flag_){
TanakaRobo 0:ca84ed7518f5 73 return;
TanakaRobo 0:ca84ed7518f5 74 }
TanakaRobo 0:ca84ed7518f5 75 short gyro_z_now_ = gyroRead2(GYRO_ZOUT_H);
TanakaRobo 0:ca84ed7518f5 76 gyro_z_now_ = ((double)gyro_z_now_ - gyro_z_aver_) / gyro_LSB_;
TanakaRobo 0:ca84ed7518f5 77 diff_yaw_ = (gyro_z_now_ + gyro_z_prev_) / 2 * timer_.read();
TanakaRobo 0:ca84ed7518f5 78 timer_.reset();
TanakaRobo 0:ca84ed7518f5 79 yaw += diff_yaw_;
TanakaRobo 0:ca84ed7518f5 80 gyro_z_prev_ = gyro_z_now_;
TanakaRobo 0:ca84ed7518f5 81 //temp = (double)gyroRead2(TEMPERATURE)/340+36.53;
TanakaRobo 0:ca84ed7518f5 82 if (yaw > 180) {
TanakaRobo 0:ca84ed7518f5 83 yaw -= 360;
TanakaRobo 0:ca84ed7518f5 84 } else if (yaw <= -180) {
TanakaRobo 0:ca84ed7518f5 85 yaw += 360;
TanakaRobo 0:ca84ed7518f5 86 }
TanakaRobo 0:ca84ed7518f5 87 }
TanakaRobo 0:ca84ed7518f5 88
TanakaRobo 0:ca84ed7518f5 89 void GY521::reset(int user){
TanakaRobo 0:ca84ed7518f5 90 yaw = user;
TanakaRobo 0:ca84ed7518f5 91 short gyro_z_now_;
TanakaRobo 0:ca84ed7518f5 92 flag_ = true;
TanakaRobo 0:ca84ed7518f5 93 gyro_z_aver_ = 0;
TanakaRobo 0:ca84ed7518f5 94 for (int i = 0; i < calibration_; i++) {
TanakaRobo 0:ca84ed7518f5 95 gyro_z_now_ = gyroRead2(GYRO_ZOUT_H);
TanakaRobo 0:ca84ed7518f5 96 gyro_z_aver_ += gyro_z_now_;
TanakaRobo 0:ca84ed7518f5 97 }
TanakaRobo 0:ca84ed7518f5 98 gyro_z_aver_ /= calibration_;
TanakaRobo 0:ca84ed7518f5 99 flag_ = false;
TanakaRobo 0:ca84ed7518f5 100 }
TanakaRobo 0:ca84ed7518f5 101
TanakaRobo 0:ca84ed7518f5 102 double GY521::checkStatus(int mode) {
TanakaRobo 0:ca84ed7518f5 103 if (mode == 0) {
TanakaRobo 0:ca84ed7518f5 104 return gyro_LSB_;
TanakaRobo 0:ca84ed7518f5 105 } else if (mode == 1) {
TanakaRobo 0:ca84ed7518f5 106 return gyro_z_aver_;
TanakaRobo 0:ca84ed7518f5 107 } else if (mode == 2) {
TanakaRobo 0:ca84ed7518f5 108 return acos(gyro_LSB_ / GY521_LSB_MAP[bit_]);
TanakaRobo 0:ca84ed7518f5 109 }
TanakaRobo 0:ca84ed7518f5 110 return 0;
TanakaRobo 0:ca84ed7518f5 111 }
TanakaRobo 0:ca84ed7518f5 112
TanakaRobo 0:ca84ed7518f5 113 int16_t GY521::gyroRead2(enum GY521RegisterMap reg) {
TanakaRobo 0:ca84ed7518f5 114 char data[2];
TanakaRobo 0:ca84ed7518f5 115 char addr = reg;
TanakaRobo 0:ca84ed7518f5 116 i2c_.write(dev_id,&addr,1,true);
TanakaRobo 0:ca84ed7518f5 117 i2c_.read(dev_id,data,2);
TanakaRobo 0:ca84ed7518f5 118 return (data[0] << 8) + data[1];
TanakaRobo 0:ca84ed7518f5 119 }