Dedicated controller at a fixed frequency

[kushal-a]

Add a dedicated controller method triggered at a fixed frequency (1kHz). Update the setpoint as and when you get goals.

Bonus feature.

Update the controller and send messages to the robot at 1kHz
Update the setpoint at a fixed frequency 50 or 20 Hz and if you do not receive meesages for a second or two, you can start winding down the integral and sending it to a home position.