The IMU MPU6050, that I am using, is not providing the same value for the holding position of the actuated object. I started by keeping the object stationary, then pressurized it, and kept it pressurized for 4 sec. and collected the angle position value of sensor. I saw that the sensor was providing gradually decreasing values when it was hold for 4 sec. in pressurized position. So, I wrote the following code to keep its angle measurement the same for holding position.
void MPU6050::update(){
// retrieve raw data
this->fetchData();
// estimate tilt angles: this is an approximation for small angles!
float sgZ = (accZ>=0)-(accZ<0); // allow one angle to go from -180 to +180 degrees
angleAccX = atan2(sgZ*accY, sqrt(accZ*accZ + accX*accX)) * RAD_2_DEG; // [-180,+180] deg
angleAccY = atan2(accX, sqrt(accZ*accZ + accY*accY)) * RAD_2_DEG; // [- 90,+ 90] deg
unsigned long Tnew = millis();
float dt = (Tnew - preInterval) * 1e-3;
preInterval = Tnew;
// Correctly wrap X and Y angles (special thanks to Edgar Bonet!)
// https://github.com/gabriel-milan/TinyMPU6050/issues/6
angleX = wrap(filterGyroCoef*(angleAccX + wrap(angleX + gyroX*dt - angleAccX,180)) + (1.0-filterGyroCoef)*angleAccX,180);
angleY = wrap(filterGyroCoef*(angleAccY + wrap(angleY + sgZ*gyroY*dt - angleAccY, 90)) + (1.0-filterGyroCoef)*angleAccY, 90);
angleZ += gyroZ*dt; // not wrapped (to do???)
float previousangleX = 0;
float currentangleX = wrap(filterGyroCoef*(angleAccX + wrap(angleX + gyroX*dt angleAccX,180)) + (1.0-filterGyroCoef)*angleAccX,180);
if (gyroX < 0.25) {currentangleX = previousangleX;}
previousangleX = currentangleX;
}
Now, I am reading currentangleX from the sensor instead of the angleX. But the code is not working. I am getting all 0 angle for all position. What change should I make?
This is the source code that I am editing: GitHub - rfetick/MPU6050_light: Lightweight, fast and simple library to communicate with the MPU6050
The following figure will be helpful to understand my problem. This diagram is obtained before updating the source code.
A zero result of a multiplication means that one of your inputs is a zero, which usually sneaks in by an undefined value. Make sure you can see each individual value to verify it is not zero. I think it is the 1e-3 which might be "too small" and rounds to zero.
I feel like the problem is defining the float previousangleX = 0; in the loop. How can I set previousangleX's initial value as 0 and keep it outside of the loop?
Please note that everything works fine with the source code and I only added last 4 lines in the code (from float previousangleX = 0; to previousangleX = currentangleX;)
Unfortunately, it is still the same when I moved the float previousangleX = 0 outside of the loop and defined it in the private variable section of the MPU6050_light.h file of the source code.
For stationary tilt angle measurements, use only the accelerometer. The gyro contributes nothing but noise and drift errors, which can be quite large if you don't calibrate the gyro properly.
I tried and it didn't work, too! You can see, this is graph I got for the angle measured by the accelerometer. In the holding part, it returns to zero and does not keep its position at -40 degrees (and sometimes provides more angle that the actual angle). Accelerometer uses this equation to measure the angle:
