Miniature Balancing Robot: first test

My miniature balancing robot is up and balancing! The new IMU sensor board implementation was easy and painless, and now I'm in the long (and probably never ending) of playing around with P, I, and D coefficients for the control loop. I've achieved balancing on carpets and temporary balance on hard surfaces so far. The small wheels, short distance between axis of rotation and center of mass, and poor vibration isolation of the IMU is making more optimized balance very difficult. Any proportional gain high enough to keep the bot upright on a hard surface is enough to generate high frequency oscillations that I can't easily address. Thus far, I've experimented with low-pass filtering the derivative output and filtering of the total output with little benefit. I've also attempted mapping the proportional gain exponentially to allow for higher gains at the extremes without jittering at low angles, but without much success. Next up, I may try optimizing the program to eliminate trig through a lookup table and certain floating point operations to speed it up. Problems aside, I am very pleased with how well the first iteration has worked!: