Redo teleop

[RCoder01]

The code is old and very hard to read. Also we now have new features that could use teleop integrations.

• PID control for all the relevant axes (Yaw, Heave; pitch and roll don't need to be controlled)
• Optional world-relative control (surge/sway go relative to zero yaw, not current yaw)
• Teleop record/replay (should this be done at a lower level?)
• State machine integration (same as current, zeroing, resetting, soft stopping)
• New controller?
• Estop

[gaoshenghan1130]

Hi @RCoder01,

I’ve refactored the teleop node into a new file joystick_teleopn.py in my fork. This improves readability and roughly halves the original code length. B

I didn’t find a suitable branch in the main repo for a PR (I started this on the default branch as suggested by @muskaan-mittal) and I don’t have authorization to create one there. You can check out my branch here: teleop/reconstruct.

I currently don’t have a joystick to test it with, so I haven’t been able to fully verify the functionality.

[michiganrobosub]

Old bindings:

[gaoshenghan1130]

Fixed estop logic now: once pressed, never change back to normal