ok, now both wheels are breaking but I can't get it balancing properly.
// PID Balance variables
double SPZero = -0.30;// Setpoint trim adjustment in Degrees
double SPAdjust = 0, Setpoint = 0, Input, Output;
double consKp = 25, consKi = 1, consKd = 0.8;
The robot stays still and balanced for one or two second then overshoots slowly, so things are going better...
I want to test it on a carpet but before that can you adjust the SPZero? When the robot stands still imu gives me -0.30 (using your program)... so double SPZero = -0.30; ?