Calvin Kalintra / MPU6050

MPU

Dependents:   balance_all

MPU6050.cpp@0:70eb7a2966f1, 2018-05-16 (annotated)

Committer:
ckalintra
Date:
Wed May 16 10:27:51 2018 +0000
Revision:
0:70eb7a2966f1
MPU6050

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ckalintra 0:70eb7a2966f1 1 #include "MPU6050.h"
ckalintra 0:70eb7a2966f1 2 #define pi 3.14159265358979323846
ckalintra 0:70eb7a2966f1 3
ckalintra 0:70eb7a2966f1 4
ckalintra 0:70eb7a2966f1 5
ckalintra 0:70eb7a2966f1 6 void MPU6050::writeByte(char pointer, char byte)
ckalintra 0:70eb7a2966f1 7 {
ckalintra 0:70eb7a2966f1 8 char write[2];
ckalintra 0:70eb7a2966f1 9 write[0]=pointer; // I2C sends MSB first. Namely >>|subAddress|>>|data|
ckalintra 0:70eb7a2966f1 10 write[1]=byte;
ckalintra 0:70eb7a2966f1 11 i2c.write(slaveAddress, write, 2, 0); // i2c.write(int address, char* data, int length, bool repeated=false);
ckalintra 0:70eb7a2966f1 12 }
ckalintra 0:70eb7a2966f1 13
ckalintra 0:70eb7a2966f1 14
ckalintra 0:70eb7a2966f1 15 char MPU6050::readByte(char pointer)
ckalintra 0:70eb7a2966f1 16 {
ckalintra 0:70eb7a2966f1 17 char read;
ckalintra 0:70eb7a2966f1 18 i2c.write(slaveAddress,&pointer, 1, 1);//write the register that needs to be read
ckalintra 0:70eb7a2966f1 19 i2c.read(slaveAddress,&read, 1, 0);
ckalintra 0:70eb7a2966f1 20 return read;
ckalintra 0:70eb7a2966f1 21
ckalintra 0:70eb7a2966f1 22 }
ckalintra 0:70eb7a2966f1 23
ckalintra 0:70eb7a2966f1 24 void MPU6050::readBytes(char pointer, char *bytes, char length)
ckalintra 0:70eb7a2966f1 25 {
ckalintra 0:70eb7a2966f1 26
ckalintra 0:70eb7a2966f1 27 i2c.write(slaveAddress,&pointer,1,1);
ckalintra 0:70eb7a2966f1 28 i2c.read(slaveAddress,bytes,length);
ckalintra 0:70eb7a2966f1 29 }
ckalintra 0:70eb7a2966f1 30
ckalintra 0:70eb7a2966f1 31 void MPU6050::whoAmI()
ckalintra 0:70eb7a2966f1 32 {
ckalintra 0:70eb7a2966f1 33 uint8_t whoAmI = this->readByte(0x75);
ckalintra 0:70eb7a2966f1 34 pc.printf("0x%x ", whoAmI);//debug
ckalintra 0:70eb7a2966f1 35 if(whoAmI == 0x68)//if a0 pin is floating or grounded should return 0x68
ckalintra 0:70eb7a2966f1 36 {
ckalintra 0:70eb7a2966f1 37 pc.printf("connection detected\n\r");
ckalintra 0:70eb7a2966f1 38 }
ckalintra 0:70eb7a2966f1 39
ckalintra 0:70eb7a2966f1 40 else//if no connection detected
ckalintra 0:70eb7a2966f1 41 {
ckalintra 0:70eb7a2966f1 42 pc.printf("check connection");
ckalintra 0:70eb7a2966f1 43 }
ckalintra 0:70eb7a2966f1 44 }
ckalintra 0:70eb7a2966f1 45
ckalintra 0:70eb7a2966f1 46 void MPU6050::sleep(bool state) {
ckalintra 0:70eb7a2966f1 47 char temp;
ckalintra 0:70eb7a2966f1 48 temp = this->readByte(0x6B);//MPU6050_PWR_MGMT_1_REG
ckalintra 0:70eb7a2966f1 49 if (state == true)
ckalintra 0:70eb7a2966f1 50 temp |= 1<<6;
ckalintra 0:70eb7a2966f1 51 if (state == false)
ckalintra 0:70eb7a2966f1 52 temp &= 0<<6;
ckalintra 0:70eb7a2966f1 53 this->writeByte(0x6B, temp);
ckalintra 0:70eb7a2966f1 54 }
ckalintra 0:70eb7a2966f1 55
ckalintra 0:70eb7a2966f1 56 void MPU6050::init()
ckalintra 0:70eb7a2966f1 57 {
ckalintra 0:70eb7a2966f1 58 i2c.frequency(400000); //400 kHz
ckalintra 0:70eb7a2966f1 59 sleep(false);
ckalintra 0:70eb7a2966f1 60 wait_ms(100);
ckalintra 0:70eb7a2966f1 61 uint8_t temp = this->readByte(0x1A);
ckalintra 0:70eb7a2966f1 62 this->writeByte(0x1A, temp & 0xC0);//DSYNC disabled bandwidth selected 44HZ for accelometer and 42HZ for gyroscope
ckalintra 0:70eb7a2966f1 63 temp = readByte(0x19);
ckalintra 0:70eb7a2966f1 64 this->writeByte(0x19, temp | 0x04);//sample rate = 200hz
ckalintra 0:70eb7a2966f1 65
ckalintra 0:70eb7a2966f1 66
ckalintra 0:70eb7a2966f1 67 temp = readByte(0x1B);
ckalintra 0:70eb7a2966f1 68 this->writeByte(0x1B, temp & ~0xE0);
ckalintra 0:70eb7a2966f1 69 temp = readByte(0x1B); // clear self test x,y,z for gyro
ckalintra 0:70eb7a2966f1 70 this->writeByte(0x1B, temp & ~0x18);
ckalintra 0:70eb7a2966f1 71 temp = readByte(0x1B); // clear FS_SEL
ckalintra 0:70eb7a2966f1 72 this->writeByte(0x1B, temp | 0<<3 | 0<<4); // Set range 2000 degree/sec
ckalintra 0:70eb7a2966f1 73 temp = readByte(0x1C);
ckalintra 0:70eb7a2966f1 74 this->writeByte(0x1C, temp & ~0xE0);
ckalintra 0:70eb7a2966f1 75 temp = readByte(0x1C);
ckalintra 0:70eb7a2966f1 76 this->writeByte(0x1C, temp & ~0x18);
ckalintra 0:70eb7a2966f1 77 temp = readByte(0x1C);
ckalintra 0:70eb7a2966f1 78 this->writeByte(0x1C, temp | 0<<3 | 0<<4); //16G
ckalintra 0:70eb7a2966f1 79
ckalintra 0:70eb7a2966f1 80 }
ckalintra 0:70eb7a2966f1 81
ckalintra 0:70eb7a2966f1 82 void MPU6050::gyro(float *val)
ckalintra 0:70eb7a2966f1 83 {
ckalintra 0:70eb7a2966f1 84 char temp_val[6];
ckalintra 0:70eb7a2966f1 85 this->readBytes(0x43, temp_val, 6);
ckalintra 0:70eb7a2966f1 86 int16_t tempy_val[3];
ckalintra 0:70eb7a2966f1 87 tempy_val[0] = (int16_t)((temp_val[0]<<8) | temp_val[1]);//patch the gyro value
ckalintra 0:70eb7a2966f1 88 tempy_val[1] = (int16_t)((temp_val[2]<<8) | temp_val[3]);
ckalintra 0:70eb7a2966f1 89 tempy_val[2] = (int16_t)((temp_val[4]<<8) | temp_val[5]);
ckalintra 0:70eb7a2966f1 90 val[0] = (float)tempy_val[0]/131;//divide by sensitivity
ckalintra 0:70eb7a2966f1 91 val[1] = (float)tempy_val[1]/131;
ckalintra 0:70eb7a2966f1 92 val[2] = (float)tempy_val[2]/131;
ckalintra 0:70eb7a2966f1 93 }
ckalintra 0:70eb7a2966f1 94
ckalintra 0:70eb7a2966f1 95 void MPU6050::acc(float *val)
ckalintra 0:70eb7a2966f1 96 {
ckalintra 0:70eb7a2966f1 97 char temp_val[6];
ckalintra 0:70eb7a2966f1 98 this->readBytes(0x3B, temp_val, 6);
ckalintra 0:70eb7a2966f1 99 int16_t tempy_val[3];
ckalintra 0:70eb7a2966f1 100 tempy_val[0] = (int16_t)((temp_val[0]<<8) | temp_val[1]);
ckalintra 0:70eb7a2966f1 101 tempy_val[1] = (int16_t)((temp_val[2]<<8) | temp_val[3]);
ckalintra 0:70eb7a2966f1 102 tempy_val[2] = (int16_t)((temp_val[4]<<8) | temp_val[5]);
ckalintra 0:70eb7a2966f1 103
ckalintra 0:70eb7a2966f1 104 val[0] = (float)tempy_val[0]/16384;
ckalintra 0:70eb7a2966f1 105 val[1] = (float)tempy_val[1]/16384;
ckalintra 0:70eb7a2966f1 106 val[2] = (float)tempy_val[2]/16384;//divide by sensitivity
ckalintra 0:70eb7a2966f1 107 }
ckalintra 0:70eb7a2966f1 108
ckalintra 0:70eb7a2966f1 109 void MPU6050::filtered(float old_p, float *val, float time, float offset, float sp)//complimentary filter
ckalintra 0:70eb7a2966f1 110 {
ckalintra 0:70eb7a2966f1 111 float gyro_val[3], acc_val[3];
ckalintra 0:70eb7a2966f1 112 float pitch_temp;
ckalintra 0:70eb7a2966f1 113 gyro(gyro_val);
ckalintra 0:70eb7a2966f1 114 acc(acc_val);
ckalintra 0:70eb7a2966f1 115 val[0] = old_p + ((gyro_val[0]-offset)*time);//gyro uses the previous added error to get position + the current value*time
ckalintra 0:70eb7a2966f1 116 float gravity_sum = abs(acc_val[0]) + abs(acc_val[1]) + abs(acc_val[2]);
ckalintra 0:70eb7a2966f1 117 if (0.5 < gravity_sum && gravity_sum < 2)//if accelerometer value is acceptable
ckalintra 0:70eb7a2966f1 118 {
ckalintra 0:70eb7a2966f1 119 float r = sqrt(acc_val[0]*acc_val[0]+acc_val[1]*acc_val[1]+acc_val[2]*acc_val[2]);
ckalintra 0:70eb7a2966f1 120 float x = acos(acc_val[0]/r);//get angle of x axis
ckalintra 0:70eb7a2966f1 121 x = x*180/pi;//convert to degree
ckalintra 0:70eb7a2966f1 122 val[0] = val[0] * 0.02 + (x-sp) * 0.98;//complimentary filter
ckalintra 0:70eb7a2966f1 123 }
ckalintra 0:70eb7a2966f1 124 }
ckalintra 0:70eb7a2966f1 125
ckalintra 0:70eb7a2966f1 126
ckalintra 0:70eb7a2966f1 127
ckalintra 0:70eb7a2966f1 128
ckalintra 0:70eb7a2966f1 129
ckalintra 0:70eb7a2966f1 130
ckalintra 0:70eb7a2966f1 131
ckalintra 0:70eb7a2966f1 132
ckalintra 0:70eb7a2966f1 133