島根短大　ライントレーサ制作チーム / Mbed 2 deprecated i2c_gyro_mpu_6050

ジャイロセンサを使用してLCDディスプレイに角速度情報を表示するプログラム

Dependencies:   mbed

Dependents:   line_trace_sample line_trace_sample_saitou line_trace_sample_tamada line_trace_ver2

mpu6050.cpp@0:abd3a0fd55a9, 2018-12-12 (annotated)

Committer:
satoshi1204
Date:
Wed Dec 12 12:42:30 2018 +0000
Revision:
0:abd3a0fd55a9
Child:
1:6f743fe9a027
implement MPU library

Who changed what in which revision?

UserRevisionLine numberNew contents of line
satoshi1204 0:abd3a0fd55a9 1 #include "mpu6050.h"
satoshi1204 0:abd3a0fd55a9 2
satoshi1204 0:abd3a0fd55a9 3 DigitalOut led(LED3);
satoshi1204 0:abd3a0fd55a9 4
satoshi1204 0:abd3a0fd55a9 5 static double RATE_ACC_LIST[4] = {
satoshi1204 0:abd3a0fd55a9 6 16384.0,
satoshi1204 0:abd3a0fd55a9 7 8192.0,
satoshi1204 0:abd3a0fd55a9 8 4096.0,
satoshi1204 0:abd3a0fd55a9 9 2048.0,
satoshi1204 0:abd3a0fd55a9 10 };
satoshi1204 0:abd3a0fd55a9 11
satoshi1204 0:abd3a0fd55a9 12 static double RATE_GYRO_LIST[4] = {
satoshi1204 0:abd3a0fd55a9 13 131.0,
satoshi1204 0:abd3a0fd55a9 14 65.5,
satoshi1204 0:abd3a0fd55a9 15 32.8,
satoshi1204 0:abd3a0fd55a9 16 16.4,
satoshi1204 0:abd3a0fd55a9 17 };
satoshi1204 0:abd3a0fd55a9 18
satoshi1204 0:abd3a0fd55a9 19 MPU6050::MPU6050(PinName i2c_sda, PinName i2c_scl) : i2c(i2c_sda, i2c_scl), address(208), rate_accelemeter(16384.0), rate_gyroscope(131.0)
satoshi1204 0:abd3a0fd55a9 20 {
satoshi1204 0:abd3a0fd55a9 21 setMaxScale(MAX_ACC_2G, MAX_GYRO_250degpersec);
satoshi1204 0:abd3a0fd55a9 22 }
satoshi1204 0:abd3a0fd55a9 23
satoshi1204 0:abd3a0fd55a9 24 MPU6050::~MPU6050()
satoshi1204 0:abd3a0fd55a9 25 {
satoshi1204 0:abd3a0fd55a9 26 }
satoshi1204 0:abd3a0fd55a9 27
satoshi1204 0:abd3a0fd55a9 28 char MPU6050::readByte(char reg)
satoshi1204 0:abd3a0fd55a9 29 {
satoshi1204 0:abd3a0fd55a9 30 int result_write = i2c.write(address, &reg, 1);
satoshi1204 0:abd3a0fd55a9 31 if(result_write) {
satoshi1204 0:abd3a0fd55a9 32 // failed
satoshi1204 0:abd3a0fd55a9 33 //pc.printf("i2c error\r\n");
satoshi1204 0:abd3a0fd55a9 34 led = 1;
satoshi1204 0:abd3a0fd55a9 35 }
satoshi1204 0:abd3a0fd55a9 36 char read_data;
satoshi1204 0:abd3a0fd55a9 37 int result_read = i2c.read(address, &read_data, 1);
satoshi1204 0:abd3a0fd55a9 38 if(result_read) {
satoshi1204 0:abd3a0fd55a9 39 // failed
satoshi1204 0:abd3a0fd55a9 40 //pc.printf("i2c error\r\n");
satoshi1204 0:abd3a0fd55a9 41 led = 1;
satoshi1204 0:abd3a0fd55a9 42 }
satoshi1204 0:abd3a0fd55a9 43 return read_data;
satoshi1204 0:abd3a0fd55a9 44 }
satoshi1204 0:abd3a0fd55a9 45
satoshi1204 0:abd3a0fd55a9 46 void MPU6050::writeByte(char reg, char data)
satoshi1204 0:abd3a0fd55a9 47 {
satoshi1204 0:abd3a0fd55a9 48 char buf[2];
satoshi1204 0:abd3a0fd55a9 49 buf[0] = reg;
satoshi1204 0:abd3a0fd55a9 50 buf[1] = data;
satoshi1204 0:abd3a0fd55a9 51 int result = i2c.write(address, buf, 2);
satoshi1204 0:abd3a0fd55a9 52 if(result) {
satoshi1204 0:abd3a0fd55a9 53 // failed
satoshi1204 0:abd3a0fd55a9 54 //pc.printf("i2c error\r\n");
satoshi1204 0:abd3a0fd55a9 55 led = 1;
satoshi1204 0:abd3a0fd55a9 56 }
satoshi1204 0:abd3a0fd55a9 57 }
satoshi1204 0:abd3a0fd55a9 58
satoshi1204 0:abd3a0fd55a9 59 void MPU6050::readAccelemeter(double &acc_x, double &acc_y, double &acc_z)
satoshi1204 0:abd3a0fd55a9 60 {
satoshi1204 0:abd3a0fd55a9 61 char h, l;
satoshi1204 0:abd3a0fd55a9 62 short x, y, z;
satoshi1204 0:abd3a0fd55a9 63 // read X
satoshi1204 0:abd3a0fd55a9 64 h = readByte(0x3B);
satoshi1204 0:abd3a0fd55a9 65 l = readByte(0x3C);
satoshi1204 0:abd3a0fd55a9 66 x = h << 8 | l;
satoshi1204 0:abd3a0fd55a9 67 // read Y
satoshi1204 0:abd3a0fd55a9 68 h = readByte(0x3D);
satoshi1204 0:abd3a0fd55a9 69 l = readByte(0x3E);
satoshi1204 0:abd3a0fd55a9 70 y = h << 8 | l;
satoshi1204 0:abd3a0fd55a9 71 // read Z
satoshi1204 0:abd3a0fd55a9 72 h = readByte(0x3F);
satoshi1204 0:abd3a0fd55a9 73 l = readByte(0x40);
satoshi1204 0:abd3a0fd55a9 74 z = h << 8 | l;
satoshi1204 0:abd3a0fd55a9 75 acc_x = (double)x / rate_accelemeter;
satoshi1204 0:abd3a0fd55a9 76 acc_y = (double)y / rate_accelemeter;
satoshi1204 0:abd3a0fd55a9 77 acc_z = (double)z / rate_accelemeter;
satoshi1204 0:abd3a0fd55a9 78 }
satoshi1204 0:abd3a0fd55a9 79
satoshi1204 0:abd3a0fd55a9 80 void MPU6050::readGyroscope(double &gyro_x, double &gyro_y, double &gyro_z)
satoshi1204 0:abd3a0fd55a9 81 {
satoshi1204 0:abd3a0fd55a9 82 char h, l;
satoshi1204 0:abd3a0fd55a9 83 short x, y, z;
satoshi1204 0:abd3a0fd55a9 84 // read X
satoshi1204 0:abd3a0fd55a9 85 h = readByte(0x43);
satoshi1204 0:abd3a0fd55a9 86 l = readByte(0x44);
satoshi1204 0:abd3a0fd55a9 87 x = h << 8 | l;
satoshi1204 0:abd3a0fd55a9 88 // read Y
satoshi1204 0:abd3a0fd55a9 89 h = readByte(0x45);
satoshi1204 0:abd3a0fd55a9 90 l = readByte(0x46);
satoshi1204 0:abd3a0fd55a9 91 y = h << 8 | l;
satoshi1204 0:abd3a0fd55a9 92 // read Z
satoshi1204 0:abd3a0fd55a9 93 h = readByte(0x47);
satoshi1204 0:abd3a0fd55a9 94 l = readByte(0x48);
satoshi1204 0:abd3a0fd55a9 95 z = h << 8 | l;
satoshi1204 0:abd3a0fd55a9 96 gyro_x = (double)x / rate_gyroscope;
satoshi1204 0:abd3a0fd55a9 97 gyro_y = (double)y / rate_gyroscope;
satoshi1204 0:abd3a0fd55a9 98 gyro_z = (double)z / rate_gyroscope;
satoshi1204 0:abd3a0fd55a9 99 }
satoshi1204 0:abd3a0fd55a9 100
satoshi1204 0:abd3a0fd55a9 101 void MPU6050::getMaxScale(int &acc, int &gyro)
satoshi1204 0:abd3a0fd55a9 102 {
satoshi1204 0:abd3a0fd55a9 103 char reg;
satoshi1204 0:abd3a0fd55a9 104 reg = readByte(0x1C);
satoshi1204 0:abd3a0fd55a9 105 acc = reg;
satoshi1204 0:abd3a0fd55a9 106 reg = readByte(0x1B);
satoshi1204 0:abd3a0fd55a9 107 gyro = reg;
satoshi1204 0:abd3a0fd55a9 108 }
satoshi1204 0:abd3a0fd55a9 109
satoshi1204 0:abd3a0fd55a9 110 void MPU6050::setMaxScale(int max_acc, int max_gyro)
satoshi1204 0:abd3a0fd55a9 111 {
satoshi1204 0:abd3a0fd55a9 112 switch(max_acc) {
satoshi1204 0:abd3a0fd55a9 113 case MAX_ACC_2G:
satoshi1204 0:abd3a0fd55a9 114 writeByte(0x1C, 0x00);
satoshi1204 0:abd3a0fd55a9 115 break;
satoshi1204 0:abd3a0fd55a9 116 case MAX_ACC_4G:
satoshi1204 0:abd3a0fd55a9 117 writeByte(0x1C, 0x08);
satoshi1204 0:abd3a0fd55a9 118 break;
satoshi1204 0:abd3a0fd55a9 119 case MAX_ACC_8G:
satoshi1204 0:abd3a0fd55a9 120 writeByte(0x1C, 0x10);
satoshi1204 0:abd3a0fd55a9 121 break;
satoshi1204 0:abd3a0fd55a9 122 case MAX_ACC_16G:
satoshi1204 0:abd3a0fd55a9 123 writeByte(0x1C, 0x18);
satoshi1204 0:abd3a0fd55a9 124 break;
satoshi1204 0:abd3a0fd55a9 125 }
satoshi1204 0:abd3a0fd55a9 126 rate_accelemeter = RATE_ACC_LIST[max_acc];
satoshi1204 0:abd3a0fd55a9 127 switch(max_gyro) {
satoshi1204 0:abd3a0fd55a9 128 case MAX_GYRO_250degpersec:
satoshi1204 0:abd3a0fd55a9 129 writeByte(0x1B, 0x00);
satoshi1204 0:abd3a0fd55a9 130 break;
satoshi1204 0:abd3a0fd55a9 131 case MAX_GYRO_500degpersec:
satoshi1204 0:abd3a0fd55a9 132 writeByte(0x1B, 0x08);
satoshi1204 0:abd3a0fd55a9 133 break;
satoshi1204 0:abd3a0fd55a9 134 case MAX_GYRO_1000degpersec:
satoshi1204 0:abd3a0fd55a9 135 writeByte(0x1B, 0x10);
satoshi1204 0:abd3a0fd55a9 136 break;
satoshi1204 0:abd3a0fd55a9 137 case MAX_GYRO_2000degpersec:
satoshi1204 0:abd3a0fd55a9 138 writeByte(0x1B, 0x18);
satoshi1204 0:abd3a0fd55a9 139 break;
satoshi1204 0:abd3a0fd55a9 140 }
satoshi1204 0:abd3a0fd55a9 141 rate_gyroscope = RATE_GYRO_LIST[max_gyro];
satoshi1204 0:abd3a0fd55a9 142 }