TVZ2019 / Mbed 2 deprecated 1_MIKROUPRAVLJACI

Programsko rješenje za uređaj koji mjeri nagib i udaljenost.

Dependencies:   mbed TextLCD

/https:/os.mbed.com/media/uploads/jpapratov/skiron_ii.jpeg

MPU_Library/MPU.cpp@1:024485d1c677, 2019-11-16 (annotated)

Committer:
jpapratov
Date:
Sat Nov 16 13:15:11 2019 +0000
Revision:
1:024485d1c677
Parent:
0:34c1f05d8d2c 
uC

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jpapratov 0:34c1f05d8d2c 1 #include "MPU.h"
jpapratov 0:34c1f05d8d2c 2 #include "mbed.h"
jpapratov 0:34c1f05d8d2c 3
jpapratov 0:34c1f05d8d2c 4 Senzor::Senzor(PinName Sda, PinName Scl): mpu(Sda, Scl){
jpapratov 0:34c1f05d8d2c 5 mpu.frequency(400000);
jpapratov 0:34c1f05d8d2c 6 setPowerManagement (0x00);//enable measurement
jpapratov 0:34c1f05d8d2c 7
jpapratov 0:34c1f05d8d2c 8 GYRO_CONFIG(0x10);//Set the register bits as 00010000 (1000dps full scale)
jpapratov 0:34c1f05d8d2c 9
jpapratov 0:34c1f05d8d2c 10 ACCEL_CONFIG(0x10);//Set the register bits as 00010000 (+/- 8g full scale range)
jpapratov 0:34c1f05d8d2c 11
jpapratov 0:34c1f05d8d2c 12 setBW(0x00);//Set Bandwidth 0 -> 256Hz
jpapratov 0:34c1f05d8d2c 13 }
jpapratov 0:34c1f05d8d2c 14
jpapratov 0:34c1f05d8d2c 15 char Senzor::testConnection ()
jpapratov 0:34c1f05d8d2c 16 {
jpapratov 0:34c1f05d8d2c 17 char dat;
jpapratov 0:34c1f05d8d2c 18 char reg=WHO_AM_I_REG;
jpapratov 0:34c1f05d8d2c 19 mpu.write(ADDRESS, &reg, 1, true);
jpapratov 0:34c1f05d8d2c 20 mpu.read(ADDRESS, &dat, 1, false);
jpapratov 0:34c1f05d8d2c 21 return dat;
jpapratov 0:34c1f05d8d2c 22 }
jpapratov 0:34c1f05d8d2c 23 void Senzor::setBW (char command)
jpapratov 0:34c1f05d8d2c 24 {
jpapratov 0:34c1f05d8d2c 25 char dat[2];
jpapratov 0:34c1f05d8d2c 26
jpapratov 0:34c1f05d8d2c 27 dat[0]=CONFIG_REG;
jpapratov 0:34c1f05d8d2c 28 dat[1]=command;
jpapratov 0:34c1f05d8d2c 29
jpapratov 0:34c1f05d8d2c 30 mpu.write(ADDRESS, dat, 2, false);
jpapratov 0:34c1f05d8d2c 31 }
jpapratov 0:34c1f05d8d2c 32
jpapratov 0:34c1f05d8d2c 33 void Senzor::setPowerManagement (char command)
jpapratov 0:34c1f05d8d2c 34 {
jpapratov 0:34c1f05d8d2c 35 char dat[2];
jpapratov 0:34c1f05d8d2c 36
jpapratov 0:34c1f05d8d2c 37 dat[0]=PWR_MGMT_1_REG;
jpapratov 0:34c1f05d8d2c 38 dat[1]=command;
jpapratov 0:34c1f05d8d2c 39
jpapratov 0:34c1f05d8d2c 40 mpu.write(ADDRESS, dat, 2, false);
jpapratov 0:34c1f05d8d2c 41
jpapratov 0:34c1f05d8d2c 42 }
jpapratov 0:34c1f05d8d2c 43
jpapratov 0:34c1f05d8d2c 44 void Senzor::GYRO_CONFIG(char command)
jpapratov 0:34c1f05d8d2c 45 {
jpapratov 0:34c1f05d8d2c 46 char dat[2];
jpapratov 0:34c1f05d8d2c 47
jpapratov 0:34c1f05d8d2c 48 dat[0] = GYRO_CONFIG_REG; //GYRO_CONFIG register
jpapratov 0:34c1f05d8d2c 49 dat[1] = command;
jpapratov 0:34c1f05d8d2c 50
jpapratov 0:34c1f05d8d2c 51 mpu.write(ADDRESS, dat, 2, false);
jpapratov 0:34c1f05d8d2c 52 }
jpapratov 0:34c1f05d8d2c 53
jpapratov 0:34c1f05d8d2c 54 void Senzor::ACCEL_CONFIG(char command)
jpapratov 0:34c1f05d8d2c 55 {
jpapratov 0:34c1f05d8d2c 56 char dat[2];
jpapratov 0:34c1f05d8d2c 57
jpapratov 0:34c1f05d8d2c 58 dat[0] = ACCELERO_CONFIG_REG; //ACCEL_CONFIG register
jpapratov 0:34c1f05d8d2c 59 dat[1] = command;
jpapratov 0:34c1f05d8d2c 60
jpapratov 0:34c1f05d8d2c 61 mpu.write(ADDRESS, dat, 2, false);
jpapratov 0:34c1f05d8d2c 62 }
jpapratov 0:34c1f05d8d2c 63
jpapratov 0:34c1f05d8d2c 64 void Senzor::getAccel(float *accel)
jpapratov 0:34c1f05d8d2c 65 {
jpapratov 0:34c1f05d8d2c 66 char dat[6];
jpapratov 0:34c1f05d8d2c 67 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
jpapratov 0:34c1f05d8d2c 68
jpapratov 0:34c1f05d8d2c 69 mpu.write(ADDRESS, &dat[0], 1, true);
jpapratov 0:34c1f05d8d2c 70 mpu.read(ADDRESS, dat, 6, false);//krece od 3B -> 40
jpapratov 0:34c1f05d8d2c 71
jpapratov 0:34c1f05d8d2c 72 accel[0] = short(dat[0] << 8 | dat[1])/ 4096.0 * 9.81;// 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
jpapratov 0:34c1f05d8d2c 73 accel[1] = short(dat[2] << 8 | dat[3])/ 4096.0 * 9.81;// 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
jpapratov 0:34c1f05d8d2c 74 accel[2] = short(dat[4] << 8 | dat[5])/ 4096.0 * 9.81;// 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
jpapratov 0:34c1f05d8d2c 75
jpapratov 0:34c1f05d8d2c 76 }
jpapratov 0:34c1f05d8d2c 77 void Senzor::getGyro(float *gyro)//očitavanje žiroskopa i spremanje podataka
jpapratov 0:34c1f05d8d2c 78 {
jpapratov 0:34c1f05d8d2c 79 char dat[6];
jpapratov 0:34c1f05d8d2c 80 dat[0] = GYRO_XOUT_H_REG; //GYRO_XOUT_H,GYRO_XOUT_L,GYRO_YOUT_H,GYRO_YOUT_L,GYRO_ZOUT_H,GYRO_ZOUT_L
jpapratov 0:34c1f05d8d2c 81
jpapratov 0:34c1f05d8d2c 82 mpu.write(ADDRESS, &dat[0], 1, true);
jpapratov 0:34c1f05d8d2c 83 mpu.read(ADDRESS, dat, 6, false);//krece od 43 -> 48
jpapratov 0:34c1f05d8d2c 84
jpapratov 0:34c1f05d8d2c 85 gyro[0] = short(dat[0] << 8 | dat[1])/ 1879.3;// 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
jpapratov 0:34c1f05d8d2c 86 gyro[1] = short(dat[2] << 8 | dat[3])/ 1879.3;// 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
jpapratov 0:34c1f05d8d2c 87 gyro[2] = short(dat[4] << 8 | dat[5])/ 1879.3;// 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
jpapratov 0:34c1f05d8d2c 88 }
jpapratov 0:34c1f05d8d2c 89 void Senzor::getAccAngle(float *angle)//računanje kuteva x i y pomoću akceleracije
jpapratov 0:34c1f05d8d2c 90 {
jpapratov 0:34c1f05d8d2c 91 float temp[3];
jpapratov 0:34c1f05d8d2c 92 getAccel(temp);
jpapratov 0:34c1f05d8d2c 93 angle[0] = atan (temp[1]/sqrt(pow(temp[0], 2) + pow(temp[2], 2))) * 57.3; //calculate angle x(pitch/roll?) from accellerometer reading
jpapratov 0:34c1f05d8d2c 94 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
jpapratov 0:34c1f05d8d2c 95 angle[2] = atan (sqrt(pow(temp[0], 2) + pow(temp[1], 2))/temp[2]) * 57.3;//calculate angle z
jpapratov 0:34c1f05d8d2c 96 }
jpapratov 0:34c1f05d8d2c 97 void Senzor::getOffset(float *accOffset, float *gyroOffset, int sample)//računanje odstupanja od stvarne vrijednosti te oduzimanje eventualne pogreške očitanja
jpapratov 0:34c1f05d8d2c 98 {
jpapratov 0:34c1f05d8d2c 99 float gyro[3];
jpapratov 0:34c1f05d8d2c 100 float accAngle[3];
jpapratov 0:34c1f05d8d2c 101 sample=200;
jpapratov 0:34c1f05d8d2c 102
jpapratov 0:34c1f05d8d2c 103 for (int i = 0; i < 3; i++) {
jpapratov 0:34c1f05d8d2c 104 accOffset[i] = 0.0; //initialise offsets to 0.0
jpapratov 0:34c1f05d8d2c 105 gyroOffset[i] = 0.0;
jpapratov 0:34c1f05d8d2c 106 }
jpapratov 0:34c1f05d8d2c 107
jpapratov 0:34c1f05d8d2c 108 for (int i = 0; i < sample; i++) {
jpapratov 0:34c1f05d8d2c 109 getGyro(gyro); //take real life measurements
jpapratov 0:34c1f05d8d2c 110 getAccAngle(accAngle);
jpapratov 0:34c1f05d8d2c 111
jpapratov 0:34c1f05d8d2c 112 for (int j = 0; j < 3; j++) {
jpapratov 0:34c1f05d8d2c 113 *(accOffset+j) += accAngle[j]/sample; //average measurements
jpapratov 0:34c1f05d8d2c 114 *(gyroOffset+j) += gyro[j]/sample;
jpapratov 0:34c1f05d8d2c 115 }
jpapratov 0:34c1f05d8d2c 116 wait_ms (10); //wait between each reading for accuracy
jpapratov 0:34c1f05d8d2c 117 }
jpapratov 0:34c1f05d8d2c 118 for (int k = 0; k < 3; k++) {
jpapratov 0:34c1f05d8d2c 119 gyro[k] -= gyroOffset[k]; //substract offset values
jpapratov 0:34c1f05d8d2c 120 accAngle[k] -= accOffset[k];
jpapratov 0:34c1f05d8d2c 121 }
jpapratov 0:34c1f05d8d2c 122 }
jpapratov 0:34c1f05d8d2c 123 void Senzor::finalAngle (float *angle)
jpapratov 0:34c1f05d8d2c 124 {
jpapratov 0:34c1f05d8d2c 125 float GyroAng[2];
jpapratov 0:34c1f05d8d2c 126 float gyro[3];
jpapratov 0:34c1f05d8d2c 127 float accAngle[3];
jpapratov 0:34c1f05d8d2c 128 float interval;
jpapratov 0:34c1f05d8d2c 129 Timer t;
jpapratov 0:34c1f05d8d2c 130
jpapratov 0:34c1f05d8d2c 131 interval=t.read();
jpapratov 0:34c1f05d8d2c 132 t.reset();
jpapratov 0:34c1f05d8d2c 133 getGyro(gyro); //get gyro value in rad/s
jpapratov 0:34c1f05d8d2c 134 getAccAngle(accAngle); //get angle from accelerometer
jpapratov 0:34c1f05d8d2c 135 t.start();
jpapratov 0:34c1f05d8d2c 136 // računanje stupnjeva Gyro kuteva rad/s*s=rad*57.3=deg
jpapratov 0:34c1f05d8d2c 137 GyroAng[0]=GyroAng[0] + gyro[0]* interval*57.3;
jpapratov 0:34c1f05d8d2c 138 GyroAng[1]=GyroAng[1] + gyro[1] * interval*57.3;
jpapratov 0:34c1f05d8d2c 139 angle[2]= angle[2] + gyro[2] * interval*57.3; //yaw
jpapratov 0:34c1f05d8d2c 140
jpapratov 0:34c1f05d8d2c 141 angle[0]=0.96*GyroAng[0]+0.04*accAngle[0]*25; //pitch
jpapratov 0:34c1f05d8d2c 142 angle[1] = 0.96 * GyroAng[1] + 0.04 * accAngle[1]*25; //roll
jpapratov 0:34c1f05d8d2c 143
jpapratov 0:34c1f05d8d2c 144 }
jpapratov 0:34c1f05d8d2c 145 float Senzor::getTemp()//očitavanje i spremanje temperature
jpapratov 0:34c1f05d8d2c 146 {
jpapratov 0:34c1f05d8d2c 147 char dat[2];
jpapratov 0:34c1f05d8d2c 148 float temp;
jpapratov 0:34c1f05d8d2c 149
jpapratov 0:34c1f05d8d2c 150 dat[0]=TEMP_H_REG;
jpapratov 0:34c1f05d8d2c 151
jpapratov 0:34c1f05d8d2c 152 mpu.write(ADDRESS, &dat[0], 1, true);
jpapratov 0:34c1f05d8d2c 153 mpu.read(ADDRESS, dat, 2, false);//krece od 41 -> 42
jpapratov 0:34c1f05d8d2c 154
jpapratov 0:34c1f05d8d2c 155 temp =short(dat[0] << 8 | dat[1]);// 0x41 (TEMP_H_REG) & 0x42 (TEMP_H_REG)
jpapratov 0:34c1f05d8d2c 156
jpapratov 0:34c1f05d8d2c 157 temp=((double)temp+521.0)/340.0+35.0;
jpapratov 0:34c1f05d8d2c 158
jpapratov 0:34c1f05d8d2c 159 return temp;
jpapratov 0:34c1f05d8d2c 160 }