Ok bro, I probably found a solution, let's have a look
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps_V6_12.h"
#include "Wire.h"
#include <Servo.h>
/////////////////////////////////// CONFIGURATION /////////////////////////////
int buffersize = 2000; //Amount of readings used to average, make it higher to get more precision but sketch will be slower (default:1000)
int acel_deadzone = 8; //Acelerometer error allowed, make it lower to get more precision, but sketch may not converge (default:8)
int giro_deadzone = 1; //Giro error allowed, make it lower to get more precision, but sketch may not converge (default:1)
// the MPU
MPU6050 mpu;
// DMP buffer
uint8_t fifoBuffer[64];
Servo servo0;
Servo servo1;
Servo servo2;
// constants
const float rad_to_deg = 180 / M_PI;
// global variables
float min_roll = 0;
float max_roll = 0;
float yaw, pitch, rolld, correct;
int16_t ax, ay, az, gx, gy, gz;
int mean_ax, mean_ay, mean_az, mean_gx, mean_gy, mean_gz, j = 0;
int ax_offset, ay_offset, az_offset, gx_offset, gy_offset, gz_offset;
void setup()
{
Wire.begin();
Wire.setClock(400000);
Serial.begin(9600);
servo0.attach(3);
servo1.attach(5);
servo2.attach(6);
// initialize the MPU
Serial.println(F("Initializing MPU..."));
mpu.initialize();
Serial.println(F("Initializing DMP..."));
uint8_t mpuStatus = mpu.dmpInitialize();
if (mpuStatus == 0) {
Serial.println("\nMPU6050 Calibration Sketch");
delay(2000);
Serial.println("\nYour MPU6050 should be placed in horizontal position, with package letters facing up. \nDon't touch it until you see a finish message.\n");
delay(3000);
// verify connection
Serial.println(mpu.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
delay(1000);
// reset offsets
mpu.setXAccelOffset(0);
mpu.setYAccelOffset(0);
mpu.setZAccelOffset(0);
mpu.setXGyroOffset(0);
mpu.setYGyroOffset(0);
mpu.setZGyroOffset(0);
Serial.println("\nReading sensors for first time...");
meansensors();
mpuStatus++;
delay(1000);
}
if (mpuStatus == 1) {
Serial.println("\nCalculating offsets...");
calibration();
mpuStatus++;
delay(1000);
}
if (mpuStatus == 2) {
meansensors();
Serial.println("\nFINISHED!");
Serial.print("\nSensor readings with offsets:\t");
Serial.print(mean_ax);
Serial.print("\t");
Serial.print(mean_ay);
Serial.print("\t");
Serial.print(mean_az);
Serial.print("\t");
Serial.print(mean_gx);
Serial.print("\t");
Serial.print(mean_gy);
Serial.print("\t");
Serial.println(mean_gz);
Serial.print("Your offsets:\t");
Serial.print(ax_offset);
Serial.print("\t");
Serial.print(ay_offset);
Serial.print("\t");
Serial.print(az_offset);
Serial.print("\t");
Serial.print(gx_offset);
Serial.print("\t");
Serial.print(gy_offset);
Serial.print("\t");
Serial.println(gz_offset);
Serial.println("\nData is printed as: acelX acelY acelZ giroX giroY giroZ");
Serial.println("Calibrando con i valori ottenuti");
mpu.setXAccelOffset(ax_offset);
mpu.setYAccelOffset(ay_offset);
mpu.setZAccelOffset(az_offset);
mpu.setXGyroOffset(gx_offset);
mpu.setYGyroOffset(gy_offset);
mpu.setZGyroOffset(gz_offset);
mpu.setDMPEnabled(true);
loop();
}
}
void loop() {
if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) {
Quaternion q;
mpu.dmpGetQuaternion(&q, fifoBuffer);
VectorFloat gravity;
mpu.dmpGetGravity(&gravity, &q);
float ypr[3];
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
const float roll = ypr[2] * rad_to_deg;
/*min_roll = min(roll, min_roll);
max_roll = max(roll, max_roll);
*/
yaw = (ypr[0] * rad_to_deg);
pitch = (ypr[1] * rad_to_deg);
rolld = (roll);
if (j <= 500) {
correct = yaw; // Yaw starts at random value, so we capture last value after 300 readings
j++;
}
// After 500 readings
else {
yaw = yaw - correct; // Set the Yaw to 0 deg - subtract the last random Yaw value from the currrent value to make the Yaw 0 degrees
Serial.print(yaw , 1);
Serial.print(" ");
int servo0Value = map(yaw, -90, 90, 0, 180);
Serial.print(pitch, 1);
int servo1Value = map(pitch, -90, 90, 0, 180);
Serial.print(" ");
Serial.println(rolld, 1);
int servo2Value = map(rolld, -90, 90, 180, 0);
servo2.write(servo0Value);
servo1.write(servo1Value);
servo0.write(servo2Value);
}
}
}
void meansensors() {
long i = 0, buff_ax = 0, buff_ay = 0, buff_az = 0, buff_gx = 0, buff_gy = 0, buff_gz = 0;
while (i < (buffersize + 101)) {
// read raw accel/gyro measurements from device
mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
if (i > 100 && i <= (buffersize + 100)) { //First 100 measures are discarded
buff_ax = buff_ax + ax;
buff_ay = buff_ay + ay;
buff_az = buff_az + az;
buff_gx = buff_gx + gx;
buff_gy = buff_gy + gy;
buff_gz = buff_gz + gz;
}
if (i == (buffersize + 100)) {
mean_ax = buff_ax / buffersize;
mean_ay = buff_ay / buffersize;
mean_az = buff_az / buffersize;
mean_gx = buff_gx / buffersize;
mean_gy = buff_gy / buffersize;
mean_gz = buff_gz / buffersize;
}
i++;
delay(2); //Needed so we don't get repeated measures
}
}
void calibration() {
ax_offset = -mean_ax / 8;
ay_offset = -mean_ay / 8;
az_offset = (16384 - mean_az) / 8;
gx_offset = -mean_gx / 4;
gy_offset = -mean_gy / 4;
gz_offset = -mean_gz / 4;
while (1) {
int ready = 0;
mpu.setXAccelOffset(ax_offset);
mpu.setYAccelOffset(ay_offset);
mpu.setZAccelOffset(az_offset);
mpu.setXGyroOffset(gx_offset);
mpu.setYGyroOffset(gy_offset);
mpu.setZGyroOffset(gz_offset);
meansensors();
Serial.println("...");
if (abs(mean_ax) <= acel_deadzone) ready++;
else ax_offset = ax_offset - mean_ax / acel_deadzone;
if (abs(mean_ay) <= acel_deadzone) ready++;
else ay_offset = ay_offset - mean_ay / acel_deadzone;
if (abs(16384 - mean_az) <= acel_deadzone) ready++;
else az_offset = az_offset + (16384 - mean_az) / acel_deadzone;
if (abs(mean_gx) <= giro_deadzone) ready++;
else gx_offset = gx_offset - mean_gx / (giro_deadzone + 1);
if (abs(mean_gy) <= giro_deadzone) ready++;
else gy_offset = gy_offset - mean_gy / (giro_deadzone + 1);
if (abs(mean_gz) <= giro_deadzone) ready++;
else gz_offset = gz_offset - mean_gz / (giro_deadzone + 1);
if (ready == 6) break;
}
}
it works, i gots the offset values integrating the calibration ini found on this site.
if you have any advertisment it would be apreciated