MPU

Dependents:   balance_all

Files at this revision

API Documentation at this revision

Comitter:
ckalintra
Date:
Wed May 16 10:27:51 2018 +0000
Commit message:
MPU6050

Changed in this revision

MPU6050.cpp Show annotated file Show diff for this revision Revisions of this file
MPU6050.h Show annotated file Show diff for this revision Revisions of this file
diff -r 000000000000 -r 70eb7a2966f1 MPU6050.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MPU6050.cpp	Wed May 16 10:27:51 2018 +0000
@@ -0,0 +1,133 @@
+#include "MPU6050.h"
+#define pi 3.14159265358979323846
+
+ 
+
+void MPU6050::writeByte(char pointer, char byte)
+{
+    char write[2];
+    write[0]=pointer;           // I2C sends MSB first. Namely  >>|subAddress|>>|data|
+    write[1]=byte;
+    i2c.write(slaveAddress, write, 2, 0);  // i2c.write(int address, char* data, int length, bool repeated=false);  
+}
+
+
+char MPU6050::readByte(char pointer)
+{
+    char read;  
+    i2c.write(slaveAddress,&pointer, 1, 1);//write the register that needs to be read
+    i2c.read(slaveAddress,&read, 1, 0);
+    return read; 
+
+}
+
+void MPU6050::readBytes(char pointer, char *bytes, char length)
+{
+      
+    i2c.write(slaveAddress,&pointer,1,1);
+    i2c.read(slaveAddress,bytes,length);
+}
+
+void MPU6050::whoAmI()
+{
+    uint8_t whoAmI = this->readByte(0x75);
+    pc.printf("0x%x ", whoAmI);//debug
+    if(whoAmI == 0x68)//if a0 pin is floating or grounded should return 0x68
+    {
+        pc.printf("connection detected\n\r");
+    }
+    
+    else//if no connection detected
+    {
+        pc.printf("check connection");
+    }
+}
+
+void MPU6050::sleep(bool state) {
+    char temp;
+    temp = this->readByte(0x6B);//MPU6050_PWR_MGMT_1_REG
+    if (state == true)
+        temp |= 1<<6;
+    if (state == false)
+        temp &= 0<<6;
+    this->writeByte(0x6B, temp);
+}
+
+void MPU6050::init()
+{
+    i2c.frequency(400000); //400 kHz
+    sleep(false);
+    wait_ms(100);
+    uint8_t temp = this->readByte(0x1A);
+    this->writeByte(0x1A, temp & 0xC0);//DSYNC disabled bandwidth selected 44HZ for accelometer and 42HZ for gyroscope
+    temp = readByte(0x19);
+    this->writeByte(0x19, temp | 0x04);//sample rate = 200hz
+    
+     
+    temp = readByte(0x1B);
+    this->writeByte(0x1B, temp & ~0xE0);
+    temp = readByte(0x1B);      // clear self test x,y,z for gyro
+    this->writeByte(0x1B, temp & ~0x18); 
+    temp = readByte(0x1B);     // clear FS_SEL
+    this->writeByte(0x1B, temp | 0<<3 | 0<<4);  // Set range 2000 degree/sec    
+    temp = readByte(0x1C);
+    this->writeByte(0x1C, temp & ~0xE0); 
+    temp = readByte(0x1C);     
+    this->writeByte(0x1C, temp & ~0x18);  
+    temp = readByte(0x1C);    
+    this->writeByte(0x1C, temp | 0<<3 | 0<<4); //16G
+    
+}
+
+void MPU6050::gyro(float *val)
+{
+    char temp_val[6]; 
+    this->readBytes(0x43, temp_val, 6);
+    int16_t tempy_val[3];
+    tempy_val[0] = (int16_t)((temp_val[0]<<8) | temp_val[1]);//patch the gyro value
+    tempy_val[1] = (int16_t)((temp_val[2]<<8) | temp_val[3]);
+    tempy_val[2] = (int16_t)((temp_val[4]<<8) | temp_val[5]);
+    val[0] = (float)tempy_val[0]/131;//divide by sensitivity
+    val[1] = (float)tempy_val[1]/131;
+    val[2] = (float)tempy_val[2]/131;
+}
+
+void MPU6050::acc(float *val)
+{
+    char temp_val[6]; 
+    this->readBytes(0x3B, temp_val, 6);
+    int16_t tempy_val[3];
+    tempy_val[0] = (int16_t)((temp_val[0]<<8) | temp_val[1]);
+    tempy_val[1] = (int16_t)((temp_val[2]<<8) | temp_val[3]);
+    tempy_val[2] = (int16_t)((temp_val[4]<<8) | temp_val[5]);
+    
+    val[0] = (float)tempy_val[0]/16384;
+    val[1] = (float)tempy_val[1]/16384;
+    val[2] = (float)tempy_val[2]/16384;//divide by sensitivity
+}
+
+void MPU6050::filtered(float old_p, float *val, float time, float offset, float sp)//complimentary filter
+{
+    float gyro_val[3], acc_val[3];
+    float pitch_temp;
+    gyro(gyro_val);
+    acc(acc_val);               
+    val[0] = old_p + ((gyro_val[0]-offset)*time);//gyro uses the previous added error to get position + the current value*time
+    float gravity_sum = abs(acc_val[0]) + abs(acc_val[1]) + abs(acc_val[2]);
+    if (0.5 < gravity_sum && gravity_sum < 2)//if accelerometer value is acceptable
+    {
+        float r = sqrt(acc_val[0]*acc_val[0]+acc_val[1]*acc_val[1]+acc_val[2]*acc_val[2]);
+        float x = acos(acc_val[0]/r);//get angle of x axis
+        x = x*180/pi;//convert to degree
+        val[0] = val[0] * 0.02 + (x-sp) * 0.98;//complimentary filter
+    }
+} 
+
+    
+
+
+    
+
+
+
+
diff -r 000000000000 -r 70eb7a2966f1 MPU6050.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MPU6050.h	Wed May 16 10:27:51 2018 +0000
@@ -0,0 +1,26 @@
+#ifndef MPU6050_H
+#define MPU6050_H
+
+#include "mbed.h"
+
+
+#define slaveAddress 0x68<<1
+extern Serial pc;
+extern I2C i2c;
+
+class MPU6050 
+{
+    protected:
+    public: 
+    void writeByte(char pointer, char bit);//write byte to register
+    char readByte(char pointer);//read 1 byte from register
+    void readBytes(char pointer, char *bytes, char length);//read more than 1 byte from register
+    void whoAmI();//check the whoAmI register
+    void init();//initialize the mpu
+    void gyro(float *val);//read gyro value (in degree/sec)
+    void acc(float *val);//read acc value (in g)
+    void sleep(bool state);
+    void filtered(float old_p, float *val, float time, float offset, float sp);//complimentary filter
+};
+
+#endif
\ No newline at end of file