State Machine Lab Initial Code

Must Add LEDs, proofread comments and code, add all sensor thresholds,
and add servo drop code