Test code ported to main foxlatch program.

Servo code is a rough estimate, it needs actual testing on
a door to see how far it actually needs to turn. Also the
lock and unlock may be backwards currently...