OK - mine's going well now I've sorted problems with the MD23.
One thing though - in the PID controller I can't understand this line:
float motors = 128.0 - (pid+ ( sgn(pid) * sqr (pid/18) ) );
the sgn() function just returns 1 or -1 depending on pid (if it's less or greater than zero).
but i don't udnerstand where you get pid/18 from. Anyone know?