Code now works on teensy. About to try and implement with old docking station code