Improved scrappy stabilization

Changed relationship between angles and delta motor speeds (speed proportional to square of angle) and coefficients.

Added accelerometer smoothing, but not yet tested; Drive constructor now takes a smoothing parameter to specify how many frames to average over. Sort of a low-pass filter.

This was because motors started getting jerky around extreme angles (nearing 60 degree tilt)... but accelerometer smoothing probably won't actually fix this. Probably need to modify the calculations so they maintain more precision.