Hiroki Tanaka / Mbed 2 deprecated foot_F446RE_nomal

STM32足回りプログラム

Dependencies:   mbed ros_lib_kinetic

library/GY521.cpp@3:012e5d7bbfd5, 2019-07-09 (annotated)

Committer:
TanakaRobo
Date:
Tue Jul 09 16:00:03 2019 +0000
Revision:
3:012e5d7bbfd5
Parent:
0:a202e1bc38a1 
first commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
TanakaRobo 0:a202e1bc38a1 1 #include "GY521.hpp"
TanakaRobo 0:a202e1bc38a1 2
TanakaRobo 0:a202e1bc38a1 3 GY521::GY521(I2C &i2c,int bit,int calibration,double user_reg):i2c(i2c),calibration(calibration){
TanakaRobo 0:a202e1bc38a1 4 dev_id = 0x68 << 1;
TanakaRobo 0:a202e1bc38a1 5 char check;
TanakaRobo 0:a202e1bc38a1 6 char data[2] = {WHO_AM_I,0x00};
TanakaRobo 0:a202e1bc38a1 7 i2c.write(dev_id,data,1,true);
TanakaRobo 0:a202e1bc38a1 8 i2c.read(dev_id,&check,1);
TanakaRobo 0:a202e1bc38a1 9 if(check == dev_id >> 1){
TanakaRobo 0:a202e1bc38a1 10 data[0] = PWR_MGMT_1;
TanakaRobo 0:a202e1bc38a1 11 i2c.write(dev_id,data,1,true);
TanakaRobo 0:a202e1bc38a1 12 i2c.read(dev_id,&check,1);
TanakaRobo 0:a202e1bc38a1 13 if(check == 0x40){
TanakaRobo 0:a202e1bc38a1 14 i2c.write(dev_id,data,2);
TanakaRobo 0:a202e1bc38a1 15 }
TanakaRobo 0:a202e1bc38a1 16 }
TanakaRobo 0:a202e1bc38a1 17 data[0] = LPF;
TanakaRobo 0:a202e1bc38a1 18 data[1] = 0x00;
TanakaRobo 0:a202e1bc38a1 19 i2c.write(dev_id,data,2);
TanakaRobo 0:a202e1bc38a1 20 data[0] = AFS_SEL;
TanakaRobo 0:a202e1bc38a1 21 data[1] = 0x00;
TanakaRobo 0:a202e1bc38a1 22 i2c.write(dev_id,data,2);
TanakaRobo 0:a202e1bc38a1 23 short accel_x_now = 0,accel_y_now = 0,accel_z_now = 0;
TanakaRobo 0:a202e1bc38a1 24 double accel_x_aver = 0, accel_y_aver = 0, accel_z_aver = 0;
TanakaRobo 0:a202e1bc38a1 25 for (int i = 0; i < calibration; ++i) {
TanakaRobo 0:a202e1bc38a1 26 accel_x_now = gyroRead2(ACCEL_XOUT_H);
TanakaRobo 0:a202e1bc38a1 27 accel_y_now = gyroRead2(ACCEL_YOUT_H);
TanakaRobo 0:a202e1bc38a1 28 accel_z_now = gyroRead2(ACCEL_ZOUT_H);
TanakaRobo 0:a202e1bc38a1 29 accel_x_aver += accel_x_now;
TanakaRobo 0:a202e1bc38a1 30 accel_y_aver += accel_y_now;
TanakaRobo 0:a202e1bc38a1 31 accel_z_aver += accel_z_now;
TanakaRobo 0:a202e1bc38a1 32 }
TanakaRobo 0:a202e1bc38a1 33 accel_x_aver /= calibration;
TanakaRobo 0:a202e1bc38a1 34 accel_y_aver /= calibration;
TanakaRobo 0:a202e1bc38a1 35 accel_z_aver /= calibration;
TanakaRobo 0:a202e1bc38a1 36 double gyro_reg = hypot(hypot(accel_x_aver, accel_y_aver), accel_z_aver) / accel_z_aver;
TanakaRobo 0:a202e1bc38a1 37 data[0] = FS_SEL;
TanakaRobo 0:a202e1bc38a1 38 data[1] = bit << 3;
TanakaRobo 0:a202e1bc38a1 39 i2c.write(dev_id,data,2);
TanakaRobo 0:a202e1bc38a1 40 bit_ = bit;
TanakaRobo 0:a202e1bc38a1 41 gyro_LSB = GY521_LSB_MAP[bit_] / gyro_reg / user_reg;
TanakaRobo 0:a202e1bc38a1 42
TanakaRobo 0:a202e1bc38a1 43 short gyro_z_now;
TanakaRobo 0:a202e1bc38a1 44 gyro_z_aver = 0;
TanakaRobo 0:a202e1bc38a1 45 for (int i = 0; i < calibration; ++i) {
TanakaRobo 0:a202e1bc38a1 46 gyro_z_now = gyroRead2(GYRO_ZOUT_H);
TanakaRobo 0:a202e1bc38a1 47 gyro_z_aver += gyro_z_now;
TanakaRobo 0:a202e1bc38a1 48 }
TanakaRobo 0:a202e1bc38a1 49 gyro_z_aver /= calibration;
TanakaRobo 0:a202e1bc38a1 50
TanakaRobo 0:a202e1bc38a1 51 yaw = diffyaw = 0;
TanakaRobo 0:a202e1bc38a1 52 time.start();
TanakaRobo 0:a202e1bc38a1 53 flag = false;
TanakaRobo 0:a202e1bc38a1 54 }
TanakaRobo 0:a202e1bc38a1 55
TanakaRobo 0:a202e1bc38a1 56 void GY521::updata(){
TanakaRobo 0:a202e1bc38a1 57 if(flag)return;
TanakaRobo 0:a202e1bc38a1 58 short gyro_z_now_ = gyroRead2(GYRO_ZOUT_H);
TanakaRobo 0:a202e1bc38a1 59 gyro_z_now = ((double)gyro_z_now_ - gyro_z_aver) / gyro_LSB;
TanakaRobo 0:a202e1bc38a1 60 diffyaw = (gyro_z_now + gyro_z_prev) / 2 * time.read();
TanakaRobo 0:a202e1bc38a1 61 time.reset();
TanakaRobo 0:a202e1bc38a1 62 yaw += diffyaw;
TanakaRobo 0:a202e1bc38a1 63 gyro_z_prev = gyro_z_now;
TanakaRobo 0:a202e1bc38a1 64 //temp = (double)gyroRead2(TEMPERATURE)/340+36.53;
TanakaRobo 0:a202e1bc38a1 65 if (yaw > 180) {
TanakaRobo 0:a202e1bc38a1 66 yaw -= 360;
TanakaRobo 0:a202e1bc38a1 67 } else if (yaw <= -180) {
TanakaRobo 0:a202e1bc38a1 68 yaw += 360;
TanakaRobo 0:a202e1bc38a1 69 }
TanakaRobo 0:a202e1bc38a1 70 }
TanakaRobo 0:a202e1bc38a1 71
TanakaRobo 0:a202e1bc38a1 72 void GY521::reset(int user){
TanakaRobo 0:a202e1bc38a1 73 yaw = user;
TanakaRobo 0:a202e1bc38a1 74 short gyro_z_now;
TanakaRobo 0:a202e1bc38a1 75 flag = true;
TanakaRobo 0:a202e1bc38a1 76 gyro_z_aver = 0;
TanakaRobo 0:a202e1bc38a1 77 for (int i = 0; i < calibration; i++) {
TanakaRobo 0:a202e1bc38a1 78 gyro_z_now = gyroRead2(GYRO_ZOUT_H);
TanakaRobo 0:a202e1bc38a1 79 gyro_z_aver += gyro_z_now;
TanakaRobo 0:a202e1bc38a1 80 }
TanakaRobo 0:a202e1bc38a1 81 gyro_z_aver /= calibration;
TanakaRobo 0:a202e1bc38a1 82 flag = false;
TanakaRobo 0:a202e1bc38a1 83 }
TanakaRobo 0:a202e1bc38a1 84
TanakaRobo 0:a202e1bc38a1 85 double GY521::checkStatus(int mode) {
TanakaRobo 0:a202e1bc38a1 86 if (mode == 0) {
TanakaRobo 0:a202e1bc38a1 87 return gyro_LSB;
TanakaRobo 0:a202e1bc38a1 88 } else if (mode == 1) {
TanakaRobo 0:a202e1bc38a1 89 return gyro_z_aver;
TanakaRobo 0:a202e1bc38a1 90 } else if (mode == 2) {
TanakaRobo 0:a202e1bc38a1 91 return acos(gyro_LSB / GY521_LSB_MAP[bit_]);
TanakaRobo 0:a202e1bc38a1 92 }
TanakaRobo 0:a202e1bc38a1 93 return 0;
TanakaRobo 0:a202e1bc38a1 94 }
TanakaRobo 0:a202e1bc38a1 95
TanakaRobo 0:a202e1bc38a1 96 int16_t GY521::gyroRead2(enum GY521RegisterMap reg) {
TanakaRobo 0:a202e1bc38a1 97 char data[2];
TanakaRobo 0:a202e1bc38a1 98 char addr = reg;
TanakaRobo 0:a202e1bc38a1 99 i2c.write(dev_id,&addr,1,true);
TanakaRobo 0:a202e1bc38a1 100 i2c.read(dev_id,data,2);
TanakaRobo 0:a202e1bc38a1 101 return (data[0] << 8) + data[1];
TanakaRobo 0:a202e1bc38a1 102 }