struct pid{
    // Controller gains
    float kP;
    float kI;
    float kD;

    // State variables
    float lastError;
    float integral;
};

float pid_step(volatile struct pid* controller, float dt, float error);
void init_pid(volatile struct pid* controller, float p, float i, float d);