Thinking about balancing robots, a lot of energy is spent to do this, both electrical energy and your energy. I would suggest using large wheels and ballasting the robot below the axle to lower the C/G. Perhaps the batteries could go there. then install one of the gyros sold for R/C helicopters to help stabalize the bot. I think then your code for the motors would be less critical, and robot would would make you smile sooner (even if the ballast is inadequate to allow it to stand without controller operating, it should help).