Revision 0:70eb7a2966f1, committed 2018-05-16

Comitter:

ckalintra

Date:

Wed May 16 10:27:51 2018 +0000

Commit message:

MPU6050

Changed in this revision

--- /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
+    }
+} 
+
+    
+
+
+    
+
+
+
+

--- /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