Feature/ctrl pendulum

Implement PID pendulum control to control the pitch angle of the Robot

Includes:

• generic PID controller
• angle limit
• modifiable setpoint
• AHRS corrections