MD23 problem was solved by increasing my timings for the I2C bus
ended up with:
#define iic_bittime 250
#define iic_scl_high_time 600
#define iic_scl_low_time 600
#define iic_bus_free_time 470
Here's what I have so far:
I did initially ignore that line you wrote, but then found that it wouldn't work at all. Theory says that the motors can't provide as much torque if they're already moving, and so the speed must be increased relative to the previous speed. In the end I have:
float motors = 128 - ( pid + oldSpeed * smulti);
smulti in my case is 0.6 (trial and error).
The result is it balancing if it's very still, but once it gets passed about 2 degrees it goes crazy and falls down.