Hi, I am currently doing a project where I use 2 gyro sensors. The problem is whenever I start the code, the gyro sensors set its relative position to 0 even when at a tilt. I want to have the code start at the gyro sensor's absolute position. If anyone could let me know how to do this, I would be grateful. Thanks! Here is my first part of the code
#include <Wire.h>
#include <SoftwareSerial.h>
SoftwareSerial espSerial(5,6);
//Gyro1 Variables
float elapsedTime, time, timePrev; //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;
// Gyro2 Variables
int gyro_error2=0; //We use this variable to only calculate once the gyro data error
float Gyr_rawX2, Gyr_rawY2, Gyr_rawZ2; //Here we store the raw data read
float Gyro_angle_x2, Gyro_angle_y2; //Here we store the angle value obtained with Gyro data
float Gyro_raw_error_x2, Gyro_raw_error_y2; //Here we store the initial gyro data error
//Acc Variables
int acc_error2=0; //We use this variable to only calculate once the Acc data error
float Acc_rawX2, Acc_rawY2, Acc_rawZ2; //Here we store the raw data read
float Acc_angle_x2, Acc_angle_y2; //Here we store the angle value obtained with Acc data
float Acc_angle_error_x2, Acc_angle_error_y2; //Here we store the initial Acc data error
float Total_angle_x2, Total_angle_y2;
String out1;
String out2;
void setup() {
// Gyro 1 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);
// Gyro 2 setup
Wire.begin(); //begin the wire comunication
Wire.beginTransmission(0x69); //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(0x69); //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(0x69); //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(9600); //Remember to set this same baud rate to the serial monitor
// espSerial.begin(9600);
time = millis(); //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)
{
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
// Gyro 2 Error calculation
if(acc_error2==0)
{
for(int a=0; a<200; a++)
{
Wire.beginTransmission(0x69);
Wire.write(0x3B); //Ask for the 0x3B register- correspond to AcX
Wire.endTransmission(false);
Wire.requestFrom(0x69,6,true);
Acc_rawX2=(Wire.read()<<8|Wire.read())/4096.0 ; //each value needs two registres
Acc_rawY2=(Wire.read()<<8|Wire.read())/4096.0 ;
Acc_rawZ2=(Wire.read()<<8|Wire.read())/4096.0 ;
/*---X---*/
Acc_angle_error_x2 = Acc_angle_error_x2 + ((atan((Acc_rawY2)/sqrt(pow((Acc_rawX2),2) + pow((Acc_rawZ2),2)))*rad_to_deg));
/*---Y---*/
Acc_angle_error_y2 = Acc_angle_error_y2 + ((atan(-1*(Acc_rawX2)/sqrt(pow((Acc_rawY2),2) + pow((Acc_rawZ2),2)))*rad_to_deg));
if(a==199)
{
Acc_angle_error_x2 = Acc_angle_error_x2/200;
Acc_angle_error_y2 = Acc_angle_error_y2/200;
acc_error2=1;
}
}
}//end of acc error calculation
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
if(gyro_error2==0)
{
for(int i=0; i<200; i++)
{
Wire.beginTransmission(0x69); //begin, Send the slave adress (in this case 68)
Wire.write(0x43); //First adress of the Gyro data
Wire.endTransmission(false);
Wire.requestFrom(0x69,4,true); //We ask for just 4 registers
Gyr_rawX2=Wire.read()<<8|Wire.read(); //Once again we shif and sum
Gyr_rawY2=Wire.read()<<8|Wire.read();
/*---X---*/
Gyro_raw_error_x2 = Gyro_raw_error_x2 + (Gyr_rawX2/32.8);
/*---Y---*/
Gyro_raw_error_y2 = Gyro_raw_error_y2 + (Gyr_rawY2/32.8);
if(i==199)
{
Gyro_raw_error_x2 = Gyro_raw_error_x2/200;
Gyro_raw_error_y2 = Gyro_raw_error_y2/200;
gyro_error2=1;
}
}
}//end of gyro error calculation
}//end of setup void