I am building a project that is a self-balancing platform, much like this
one, however I'm not using the same components. Here's the list:
- Accelerometer: MMA7361 (datasheet
- Gyroscope: L3G4200D (datasheet
** And for further information:
- Arduino UNO R3
- A pair of Servomotors: S3003 (datasheet
** Libraries I'm using:
- Accelerometer: AcceleroMMA7361.h (source code
- Gyroscope: L3G4200D.h (source code
- Kalman filter: Kalman.h (source code
The code is the following:
short accX, accY, accZ;
short gyroX, gyroY, gyroZ;
double accXangle, accYangle;
double kalman_x, kalman_y;
double map_x, map_y, p_map_x, p_map_y;
unsigned long int timer;
int set_default_servo = 0;
accelerometer.begin(13, 12, 11, 10, A0, A1, A2);
timer = micros();
accX = accelerometer.getXAccel(); // Why are you so stubborn?
accY = accelerometer.getYAccel();
//accZ = accelerometer.getZAccel();
gyroX = gyro.g.x;
gyroY = gyro.g.y;
//gyroZ = gyro.g.z;
if (set_default_servo == 0)
Serial.print("\nWriting default servo angulation...\n");
set_default_servo = 1;
double p_map_x = (double)map_x;
double p_map_y = (double)map_y;
map_x = map(round(accX), -97, 105, 0, 180);
map_y = map(round(accY), -75, 118, 0, 180);
double gyro_x_rate = (double)gyroX/131.0; // is this right? i couldn't find how to get the rate
double gyro_y_rate = -((double)gyroY/131.0);
kalman_x = kalman_x.getAngle(map_x, gyro_x_rate, (double)(micros()-timer)/1000000);
kalman_y = kalman_y.getAngle(map_y, gyro_y_rate, (double)(micros()-timer)/1000000);
if ((kalman_x > 0 && kalman_x < 181) && abs(map_x - p_map_x) >= 5) // last condition because the values vary a lot
if ((kalman_y > 0 && kalman_y < 181) && abs(map_y - p_map_y) >= 5)
timer = micros();
The problem I'm having is that the accX
outputs strangely spiking values for a single loop and then go back to normal (or what they should be outputting). I have tried a lot of things, but the problem still happens when I use analogRead()
to get the output from the accelerometer.
My gyro also does this, with the platform standing still, parallel to its referential (in theory the Earth), it varies its output values so much, but it is not giving me any problem, because I'm just using it for the Kalman filter. But the core problem seems to be pointing at the accelerometer, but I'm not quite sure where to look, I've tried so much, different methods, none worked.
If you know something that might help me, I will be very grateful; and since this project was built with pieces of code from other projects, I do plan to make a full tutorial once I everything is sorted out. And, if you have any suggestion to better this code, even if apart from the accelerometer matter, please reply.
And if there is any information I may have forgotten or someting that you would like to know, please also reply.
Thank you very much.