Add motor controlling to rust_telemetry.py