Sure, the principle is the following:It reads the inclination angle data given by the accelerometer and gyro, applies Kalman Filter to best predict the real inclination and write to the servo motors the inverse angle to compensate the inclination.For my luck, I found an already made Kalman Filter algorithm for arduino in this post: http://arduino.cc/forum/index.php/topic,58048.msg417140.html#msg417140Then I basically used the same code, just adapting it to write the angles to the servos: // added in the beginning:#include <Servo.h> Servo servo1;Servo servo2;// added in setup: servo1.attach(6); servo2.attach(9);// added in the end of loop() function: servo1.writeMicroseconds(1560-yAngle*9.55); servo2.writeMicroseconds(1510+xAngle*9.55);Where 1560 and 1510 are the values corresponding to the center position of my servos and yAngle and xAngle varies from -90 to +90 and are scaled to fit the observed servo limits.Any question, please comment!
Well it is your sketch, and your free to show what parts of it you wish.
Please enter a valid email to subscribe
We need to confirm your email address.
To complete the subscription, please click the link in the
email we just sent you.
Thank you for subscribing!
via Egeo 16