Hi Sancho. It's possible that your Kp value may be too large. At the moment, you have a value of 700 for Kp.
Thanks again Kas!I added encoders but don't use them for stability, only speed control.Then I added a ultrasonic sensor and made it autonomous. Hope I get my 2.4Ghz modules soon!http://www.youtube.com/watch?v=SxB7yf4_Tj0--------------------------Some details:I used two cascading PI loops. The global input to the system is the velocity of the Centre Of Gravity (Vcog_target). To have a stationary robot it's Vcog_target = 0 etc.The IMU raw readings are fed to a Kalman filter to get the robot tilt angle (theta) and tilt velocity:theta_dot = angle - last_angleEncoders read from each motors (928 counts / turn each) then I get wheel velocity:Vwheel = count - last_countThe actual cog velocity Vcog is calculated from wheel velocity and tilt velocity:Vcog = K x theta_dot + VwheelThe factor K is to account for various things such as difference in units and height of the cog.This then feeds the first PI loop which output is the desired tilt angle of the robot:theta_target = PI (Vcog_target, Vcog)The target tilt angle is fed into the second PI loop, the "classic" one used for balance:motor_PWM = PI (theta_target, theta)and this is sent to the motors.The autonomous mode works by setting a desired speed. Then it reads the ultrasound sensor, and when an obstacle is detected, sets target speed to 0, waits a small delay, then spins the robot until it finds a path. Then sets speed again, etc.With the two PI loops the robot is very stable to perturbation (pushes) but doesn't maintain a stationary position very well. I think it's due to my very crappy tuning of the stability PI parameters I still can't get proper, clean balance when robot stays stationary. I looked at the Balanduino and it's very still when stationary, it uses the same motors as me so I know it's doable.I still have the same problem, either the robot is too "soft" (low P and I values) and doesn't balance properly, or when I increase P and I it starts jittering a lot.I think the root of the problem is backlash, and I'm looking at ways to deal with it. Then I will have better stability. I'm thinking of adding a 3rd PID loop to control motor speed: the 2nd PID output (desired motor speed) is fed to a 3rd PID which takes desired speed and actual speed (from encoders) to drive PWM. this loop would have to run very fast though to achieve anything. I don't know if that could work, gonna try it now!I'm turning this topic into a "project progress" kind of post, hope it's OK.