Implement PID controller

[AbBaSaMo]

Overview

Originally electrical handled PID control on the MCU chip but now that is being delegated to us. It would reduce the amount of overhead their chips will use. PID from what I understand provides some sort of homeostasis between desired wheel velocity and actual wheel velocity.

Some existing implementations exist but they need to fulfil our interfaces i.e. take in the command velocity, wheel velocity as measured by encoders and then output wheel acceleration. This needs to be done per wheel so PID control for wheel 1 will be done separately to wheel 2.

Relevant resources

• PID controller from ROS2_control
• Control toolbox package
• ros2 pid library

Acceptance criteria

• accepts cmd vel and wheel velocity as inputs
• outputs wheel acceleration values
• wheels spin appropriately when run with the MCU interface node

Todo

• research potential packages we could use
• the package should ideally take 2 inputs: encoder velocity (meaning the velocity of the wheel the encoders measured) and the desirred velocity which is from cmd_vel
• the package should have 2 outputs: acceleration for left wheel and also for right wheel
• if not such package is found, find any other package and speak with elec and @AbBaSaMo to see if it is viable
• if still no package is found, we implement our own
• at the very end update the MCU interface node