Motor controller code working. No support for Reed switch yet. Increased speed of Serial Comms which should help speed up sonar, too.