Diff: MPU_Library/MPU.cpp

Revision:

0:34c1f05d8d2c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MPU_Library/MPU.cpp	Sat Nov 16 10:14:58 2019 +0000
@@ -0,0 +1,160 @@
+#include "MPU.h"
+#include "mbed.h"
+
+Senzor::Senzor(PinName Sda, PinName Scl): mpu(Sda, Scl){
+    mpu.frequency(400000);
+    setPowerManagement (0x00);//enable measurement
+
+    GYRO_CONFIG(0x10);//Set the register bits as 00010000 (1000dps full scale)
+
+    ACCEL_CONFIG(0x10);//Set the register bits as 00010000 (+/- 8g full scale range)
+
+    setBW(0x00);//Set Bandwidth 0 -> 256Hz
+}
+
+    char Senzor::testConnection ()
+{
+    char dat;
+    char reg=WHO_AM_I_REG;
+    mpu.write(ADDRESS, &reg, 1, true);
+    mpu.read(ADDRESS, &dat, 1, false);
+    return dat;
+}
+void Senzor::setBW (char command)
+{
+    char dat[2];
+
+    dat[0]=CONFIG_REG;
+    dat[1]=command;
+
+    mpu.write(ADDRESS, dat, 2, false);
+}
+
+void Senzor::setPowerManagement (char command)
+{
+    char dat[2];
+
+    dat[0]=PWR_MGMT_1_REG;
+    dat[1]=command;
+
+    mpu.write(ADDRESS, dat, 2, false);
+
+}
+
+void Senzor::GYRO_CONFIG(char command)
+{
+    char dat[2];
+
+    dat[0] = GYRO_CONFIG_REG; //GYRO_CONFIG register
+    dat[1] = command;
+
+    mpu.write(ADDRESS, dat, 2, false);
+}
+
+void Senzor::ACCEL_CONFIG(char command)
+{
+    char dat[2];
+
+    dat[0] = ACCELERO_CONFIG_REG; //ACCEL_CONFIG register
+    dat[1] = command;
+
+    mpu.write(ADDRESS, dat, 2, false);
+}
+
+void Senzor::getAccel(float *accel)
+{
+    char dat[6];
+    dat[0] = ACCEL_XOUT_H_REG; //ACCEL_XOUT_H, ACCEL_XOUT_L, ACCEL_YOUT_H, ACCEL_YOUT_L, ACCEL_ZOUT_H, and ACCEL_ZOUT_L
+
+    mpu.write(ADDRESS, &dat[0], 1, true);
+    mpu.read(ADDRESS, dat, 6, false);//krece od 3B -> 40
+
+    accel[0] = short(dat[0] << 8 | dat[1])/ 4096.0 * 9.81;// 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
+    accel[1] = short(dat[2] << 8 | dat[3])/ 4096.0 * 9.81;// 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
+    accel[2] = short(dat[4] << 8 | dat[5])/ 4096.0 * 9.81;// 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
+
+}
+void Senzor::getGyro(float *gyro)//očitavanje žiroskopa i spremanje podataka
+{
+    char dat[6];
+    dat[0] = GYRO_XOUT_H_REG; //GYRO_XOUT_H,GYRO_XOUT_L,GYRO_YOUT_H,GYRO_YOUT_L,GYRO_ZOUT_H,GYRO_ZOUT_L
+
+    mpu.write(ADDRESS, &dat[0], 1, true);
+    mpu.read(ADDRESS, dat, 6, false);//krece od 43 -> 48
+
+    gyro[0] = short(dat[0] << 8 | dat[1])/ 1879.3;// 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
+    gyro[1] = short(dat[2] << 8 | dat[3])/ 1879.3;// 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
+    gyro[2] = short(dat[4] << 8 | dat[5])/ 1879.3;// 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
+}
+void Senzor::getAccAngle(float *angle)//računanje kuteva x i y pomoću akceleracije
+{
+    float temp[3];
+    getAccel(temp);
+    angle[0] = atan (temp[1]/sqrt(pow(temp[0], 2) + pow(temp[2], 2))) * 57.3; //calculate angle x(pitch/roll?) from accellerometer reading
+    angle[1] = atan (-1*temp[0]/sqrt(pow(temp[1], 2) + pow(temp[2], 2))) * 57.3; //calculate angle y(pitch/roll?) from accellerometer reading
+    angle[2] = atan (sqrt(pow(temp[0], 2) + pow(temp[1], 2))/temp[2]) * 57.3;//calculate angle z
+}
+void Senzor::getOffset(float *accOffset, float *gyroOffset, int sample)//računanje odstupanja od stvarne vrijednosti te oduzimanje eventualne pogreške očitanja
+{
+    float gyro[3];
+    float accAngle[3];
+    sample=200;
+
+    for (int i = 0; i < 3; i++) {
+        accOffset[i] = 0.0;     //initialise offsets to 0.0
+        gyroOffset[i] = 0.0;
+    }
+
+    for (int i = 0; i < sample; i++) {
+        getGyro(gyro); //take real life measurements
+        getAccAngle(accAngle);
+
+        for (int j = 0; j < 3; j++) {
+            *(accOffset+j) += accAngle[j]/sample;    //average measurements
+            *(gyroOffset+j) += gyro[j]/sample;
+        }
+        wait_ms (10);    //wait between each reading for accuracy
+    }
+    for (int k = 0; k < 3; k++) {
+        gyro[k] -= gyroOffset[k];       //substract offset values
+        accAngle[k] -= accOffset[k];
+    }
+}
+void Senzor::finalAngle (float *angle)
+{
+    float GyroAng[2];
+    float gyro[3];
+    float accAngle[3];
+    float interval;
+    Timer t;
+
+    interval=t.read();
+    t.reset();
+    getGyro(gyro);           //get gyro value in rad/s
+    getAccAngle(accAngle);   //get angle from accelerometer
+    t.start();
+    // računanje stupnjeva Gyro kuteva rad/s*s=rad*57.3=deg
+    GyroAng[0]=GyroAng[0] + gyro[0]* interval*57.3;
+    GyroAng[1]=GyroAng[1] + gyro[1] * interval*57.3;
+    angle[2]= angle[2] + gyro[2] * interval*57.3; //yaw
+    
+    angle[0]=0.96*GyroAng[0]+0.04*accAngle[0]*25; //pitch
+    angle[1] = 0.96 * GyroAng[1] + 0.04 * accAngle[1]*25; //roll
+
+}
+float Senzor::getTemp()//očitavanje i spremanje temperature
+{
+    char dat[2];
+    float temp;
+
+    dat[0]=TEMP_H_REG;
+
+    mpu.write(ADDRESS, &dat[0], 1, true);
+    mpu.read(ADDRESS, dat, 2, false);//krece od 41 -> 42
+
+    temp =short(dat[0] << 8 | dat[1]);// 0x41 (TEMP_H_REG) & 0x42 (TEMP_H_REG)
+
+    temp=((double)temp+521.0)/340.0+35.0;
+
+    return temp;
+}
\ No newline at end of file