Adding stop/switch motor/servo code