Massive commit

Fixed heaps of bugs with servos, where they would pass tests but still
be wrong.
Added error checking so this is very strict, if it would cause an error
in the Arduino, for example invoking digitalWrite on a pin before
setting that pins' pinMode to output