uint16_t PID (int16_t currentError, int16_t *previousError, int16_t *integral, uint8_t pGain, uint8_t iGain, uint8_t dGain) {
// proportional
int24_t output = pGain * currentError;
// integral
output += iGain * *integral;
// differential
output += dGain * (currentError - *previousError);
// adjust for int gains
output = output / 100;
// add midpoint
output += 512;
// no negatives
if (output < 0) {
output = 0;
}
// limit to 10 bit
output &= 1023;
// saves
*integral = *integral + currentError;
*previousError = currentError;
return (uint16_t) output;
}