Hello to everyone, this is my first post, I hope this not to be a a problem already solved.
I am using the following code to calculate angle on x and y axes through gyroscope and accelerometer measurements from MPU6050.
When I run the code on my Arduino Uno beetle, the code runs fine and I can get the angles correctly calculated. On the other hand if I try to run the same code using Arduino Giga R4, the angles are not correctly calculated, for example I get readings above 90 degrees for x and y angles that should not be allowed by the formula I am using to calculate them.
In the code the angles are calculated in Case B.
The connections on arduino Giga are:
SDA--> 20
SCL-->21
VCC-->5V
GND-->GND
/* http://www.youtube.com/c/electronoobs
*
* This is an example where we configure te data of the MPU6050
* and read the Acceleration data and print it to the serial monitor
*
* Arduino pin | MPU6050
* 5V | Vcc
* GND | GND
* A4 | SDA
* A5 | SCL
*/
//Includes
#include <Wire.h>
//Gyro Variables
float elapsedTime = 0, elapsedTime1, timex, timePrev, startTimeA; //Variables for time control
int gyro_error = 0; //We use this variable to only calculate once the gyro data error
float Gyr_rawX, Gyr_rawY, Gyr_rawZ; //Here we store the raw data read
float Gyro_angle_x, Gyro_angle_y; //Here we store the angle value obtained with Gyro data
float Gyro_raw_error_x, Gyro_raw_error_y; //Here we store the initial gyro data error
//Acc Variables
int acc_error = 0; //We use this variable to only calculate once the Acc data error
float rad_to_deg = 180 / 3.141592654; //This value is for pasing from radians to degrees values
float Acc_rawX, Acc_rawY, Acc_rawZ; //Here we store the raw data read
float Acc_angle_x, Acc_angle_y; //Here we store the angle value obtained with Acc data
float Acc_angle_error_x, Acc_angle_error_y; //Here we store the initial Acc data error
float Total_angle_x, Total_angle_y;
float angle_r_gyro_x, angle_r_gyro_y;
float Total_angle_r_gyro_x, Total_angle_r_gyro_y;
float Total_angle_r_acc_x, Total_angle_r_acc_y;
float timestep = 0.02;
char caso = 'Z';
int count1 = 0;
int count2 = 0;
unsigned long startTime;
unsigned long startTime0;
boolean offset1 = true;
// #define analogPin1 A2
// #define chargePin1 2
// #define dischargePin1 3
// #define resistorValue1 2000000.0F
// #define analogPin2 A1
// #define chargePin2 4
// #define dischargePin2 5
// #define resistorValue2 2000000.0F
unsigned long startTime1;
unsigned long startTime2;
unsigned long t1;
unsigned long t2;
unsigned long t3;
unsigned long t4;
unsigned long currentTime1;
unsigned long currentTime2;
float tau1;
float tau2;
const float soglia = 647.0;
int V1 = 0;
int V2 = 0;
int V3 = 0;
int V4 = 0;
float norm = 1000000.0;
float Capacita1;
float Capacita2;
float costante = 300.0;
//#define dGND5 'D5'
//#define dGND5 'D4'
//#define dGND5 'D3'
//#define dGND5 'D2'
void setup() {
Wire.begin(); //begin the Wire comunication
Wire.beginTransmission(0x68); //begin, Send the slave adress (in this case 68)
Wire.write(0x6B); //make the reset (place a 0 into the 6B register)
Wire.write(0x00);
Wire.endTransmission(true); //end the transmission
//Gyro config
Wire.beginTransmission(0x68); //begin, Send the slave adress (in this case 68)
Wire.write(0x1B); //We want to write to the GYRO_CONFIG register (1B hex)
Wire.write(0x10); //Set the register bits as 00010000 (1000dps full scale)
Wire.endTransmission(true); //End the transmission with the gyro
//Acc config
Wire.beginTransmission(0x68); //Start communication with the address found during search.
Wire.write(0x1C); //We want to write to the ACCEL_CONFIG register
Wire.write(0x10); //Set the register bits as 00010000 (+/- 8g full scale range)
Wire.endTransmission(true);
Serial.begin(115200); //Remember to set this same baud rate to the serial monitor
Serial.println("Sistema pronto");
// pinMode(dGND5, INPUT);
// digitalWrite(dGND5, LOW);
// pinMode(dGND4, INPUT);
// digitalWrite(dGND4, LOW);
// pinMode(dGND3, INPUT);
// digitalWrite(dGND3, LOW);
// pinMode(dGND2, INPUT);
// digitalWrite(dGND2, LOW);
//stessa cosa per 4 3 e 2
}
void loop() {
if (Serial.available() > 0) {
char tmpcaso = Serial.read();
if (tmpcaso == 'A' || tmpcaso == 'B' || tmpcaso == 'E') {
if (caso != tmpcaso) {
}
caso = tmpcaso;
}
}
switch (caso) {
case 'A': //Calcoliamo lo zero in qualsiasi posizione senza dover spegnere e accendere tutte le volte
if (count2 == 0) {
errore();
count2++;
}
break;
case 'B':
//startTimeA=micros();
while (count1 < 10) {
angoli();
count1++;
}
count1 = 0;
count2 = 0;
//Serial.print(micros()-startTimeA); //serve a controllare il ciclo poi va eliminato
Serial.print( Total_angle_x);
Serial.print(" ");
Serial.println( Total_angle_y);
//mettendo il dispositivo in verticale l'asse y trasla in z e z in y quindi l'angolo visto è attorno all'asse Z
break;
case 'E':
break;
}
}
void errore() {
timex = micros(); //Start counting time in milliseconds
/*Here we calculate the acc data error before we start the loop
* I make the mean of 200 values, that should be enough*/
//if(acc_error==0) Se attivi questo if il calcolo dell'errore lo fa una sola volta, ciò implica che tutte le volte
// { in cui il dispositivo viene acceso deve essere in quella posizione, levandolo siamo liberi da ciò
for (int a = 0; a < 200; a++) {
Wire.beginTransmission(0x68);
Wire.write(0x3B); //Ask for the 0x3B register- correspond to AcX
Wire.endTransmission(false);
Wire.requestFrom(0x68, 6, true);
Acc_rawX = (Wire.read() << 8 | Wire.read()) / 4096.0; //each value needs two registres
Acc_rawY = (Wire.read() << 8 | Wire.read()) / 4096.0;
Acc_rawZ = (Wire.read() << 8 | Wire.read()) / 4096.0;
/*---X---*/
Acc_angle_error_x = Acc_angle_error_x + ((atan((Acc_rawY) / sqrt(pow((Acc_rawX), 2) + pow((Acc_rawZ), 2))) * rad_to_deg));
/*---Y---*/
Acc_angle_error_y = Acc_angle_error_y + ((atan(-1 * (Acc_rawX) / sqrt(pow((Acc_rawY), 2) + pow((Acc_rawZ), 2))) * rad_to_deg));
if (a == 199) {
Acc_angle_error_x = Acc_angle_error_x / 200;
Acc_angle_error_y = Acc_angle_error_y / 200;
acc_error = 1;
}
}
//}//end of acc error calculation
/*Here we calculate the gyro data error before we start the loop
* I make the mean of 200 values, that should be enough*/
//if(gyro_error==0)
//{
for (int i = 0; i < 200; i++) {
Wire.beginTransmission(0x68); //begin, Send the slave adress (in this case 68)
Wire.write(0x43); //First adress of the Gyro data
Wire.endTransmission(false);
Wire.requestFrom(0x68, 4, true); //We ask for just 4 registers
Gyr_rawX = Wire.read() << 8 | Wire.read(); //Once again we shif and sum
Gyr_rawY = Wire.read() << 8 | Wire.read();
/*---X---*/
Gyro_raw_error_x = Gyro_raw_error_x + (Gyr_rawX / 32.8);
/*---Y---*/
Gyro_raw_error_y = Gyro_raw_error_y + (Gyr_rawY / 32.8);
if (i == 199) {
Gyro_raw_error_x = Gyro_raw_error_x / 200;
Gyro_raw_error_y = Gyro_raw_error_y / 200;
gyro_error = 1;
}
}
//}//end of gyro error calculation
//Serial.println("Calcolo errore effettuato");
} //end of setup void
void angoli() {
timePrev = timex; // the previous time is stored before the actual time read
timex = micros(); // actual time read
elapsedTime = (timex - timePrev) / 1000000; //divide by 1000 in order to obtain seconds
elapsedTime1 = elapsedTime1 + elapsedTime * 1000000;
//////////////////////////////////////Gyro read/////////////////////////////////////
Wire.beginTransmission(0x68); //begin, Send the slave adress (in this case 68)
Wire.write(0x43); //First adress of the Gyro data
Wire.endTransmission(false);
Wire.requestFrom(0x68, 4, true); //We ask for just 4 registers
Gyr_rawX = Wire.read() << 8 | Wire.read(); //Once again we shif and sum
Gyr_rawY = Wire.read() << 8 | Wire.read();
/*Now in order to obtain the gyro data in degrees/seconds we have to divide first
the raw value by 32.8 because that's the value that the datasheet gives us for a 1000dps range*/
/*---X---*/
Gyr_rawX = (Gyr_rawX / 32.8) - Gyro_raw_error_x;
/*---Y---*/
Gyr_rawY = (Gyr_rawY / 32.8) - Gyro_raw_error_y;
/*Now we integrate the raw value in degrees per seconds in order to obtain the angle
* If you multiply degrees/seconds by seconds you obtain degrees */
/*---X---*/
Gyro_angle_x = Gyr_rawX * elapsedTime;
/*---X---*/
Gyro_angle_y = Gyr_rawY * elapsedTime;
//////////////////////////////////////Acc read/////////////////////////////////////
Wire.beginTransmission(0x68); //begin, Send the slave adress (in this case 68)
Wire.write(0x3B); //Ask for the 0x3B register- correspond to AcX
Wire.endTransmission(false); //keep the transmission and next
Wire.requestFrom(0x68, 6, true); //We ask for next 6 registers starting withj the 3B
/*We have asked for the 0x3B register. The IMU will send a brust of register.
* The amount of register to read is specify in the requestFrom function.
* In this case we request 6 registers. Each value of acceleration is made out of
* two 8bits registers, low values and high values. For that we request the 6 of them
* and just make then sum of each pair. For that we shift to the left the high values
* register (<<) and make an or (|) operation to add the low values.
If we read the datasheet, for a range of+-8g, we have to divide the raw values by 4096*/
Acc_rawX = (Wire.read() << 8 | Wire.read()) / 4096.0; //each value needs two registres
Acc_rawY = (Wire.read() << 8 | Wire.read()) / 4096.0;
Acc_rawZ = (Wire.read() << 8 | Wire.read()) / 4096.0;
/*Now in order to obtain the Acc angles we use euler formula with acceleration values
after that we substract the error value found before*/
/*---X---*/
Acc_angle_x = (atan((Acc_rawY) / sqrt(Acc_rawX * Acc_rawX + Acc_rawZ * Acc_rawZ)) * rad_to_deg) - Acc_angle_error_x;
/*---Y---*/
Acc_angle_y = (atan(-1 * (Acc_rawX) / sqrt(Acc_rawY*Acc_rawY + Acc_rawZ*Acc_rawZ)) * rad_to_deg) - Acc_angle_error_y;
//////////////////////////////////////Total angle and filter/////////////////////////////////////
/*---X axis angle---*/
Total_angle_x = 0.98 * (Total_angle_x + Gyro_angle_x) + 0.02 * Acc_angle_x;
/*---Y axis angle---*/
Total_angle_y = 0.98 * (Total_angle_y + Gyro_angle_y) + 0.02 * Acc_angle_y;
Total_angle_r_gyro_x = Total_angle_r_gyro_x + Gyro_angle_x;
Total_angle_r_gyro_y = Total_angle_r_gyro_y + Gyro_angle_y;
Total_angle_r_acc_x = Acc_angle_x;
Total_angle_r_acc_y = Acc_angle_y;
/*Uncoment the rest of the serial prines
* I only print the Y angle value for this test */
/*
//Serial.print("Xº: ");
Serial.print(Total_angle_x);
//Serial.print(" | ");
// Serial.print("Yº: ");
Serial.print(" ");
Serial.println(Total_angle_y);*/
}