Improve ArduControl task scheduling

The new scheduler can handle multiple tasks with the same priority, so
other things won't get blocked by the priority command queue for too
long.  It also handles sleeping at the scheduler rather than individual
tasks.