D-O Selftest Demo

I've implemented a self test routine for (my) D-O (dubbed D.O.D.O. by my everloving wife), first because I'm too damn lazy to do a proper pre-flight check every single time, and second because I think it's good practice for a system that can go completely haywire under PID control if a motor is plugged in wrong.
The self test uses D.O.D.O.'s voltage/current sensor to check whether we have power at all (or run under USB control from a PC). Then it checks for IMU availability, because without the IMU D-O doesn't make a lot of sense. And then it runs both motors individually, checking the droid's motion around Z (the "up" axis) and its acceleration along X and Y, plus the motor encoders, to see if everything is connected correctly.
So this can diagnose
battery over- and undervoltage
IMU availability
IMU orientation
servo availability and proper angle (wouldn't want to start driving with the head dragging on the ground)
motor blockage (draws too much current and doesn't move)
encoder and motor polarity.