...

Although the inputs may be angles or rates, the output of a PID should be a motor % that can be understood by each actuator.

Implementation

A new PID will be will be designed to solve the issues with the previous iteration and saved in the PID.cpp file. The goal is to have multiple PID controllers at the same time each with their own integral & derivative terms being calculated independently, also the PID’s should update at the same fixed frequency, so that derivative and integral calculations work properly. The structure of the PID and its methods are below: