Proj4/lib4.c
void pidStep(uint16_t vtk) {
    etk_1 = etk;
    etk = ((int16_t)vtk) - PID_SET_POINT;
    esum += etk;
    esum -= pushPop(esumQueue, etk);
    int16_t p = PID_KP*etk;
    int16_t i = PID_KI*esum*PID_DT / PID_QSIZE; // damping
    int16_t d = PID_KD*(etk-etk_1)/PID_DT;
    irobprintf("etk_1: %d\netk: %d\nesum: %d\nutk: %d\np: %d\ni: %d\nd: %d\n",
            etk_1, etk, esum, utk, p, i, d);
    utk = p + i + d;
}
/**
 * Takes the next input for the pid controler
 * utilizes constants:
 * PID_SET_POINT - the set point or goal
 * PID_KP - the multiplier for the current distance
 * PID_KI - the multiplier for the "integral" term
 * PID_KD - the multiplier for the "derivative" term
 * This then sets utk which is essentially the result
 * of this calculation
 * Important notes:
 * The derivative terms only look at this and the previous value
 * the integral term currently has no time mitigation and uses
 * a fixed size queue (now has damping based on size)
 * 
 * @param vtk the current value for the pid controller
 */
Takes the next input for the pid controler
utilizes constants:
PID_SET_POINT - the set point or goal
PID_KP - the multiplier for the current distance
PID_KI - the multiplier for the "integral" term
PID_KD - the multiplier for the "derivative" term
This then sets utk which is essentially the result
of this calculation
Important notes:
The derivative terms only look at this and the previous value
the integral term currently has no time mitigation and uses
a fixed size queue (now has damping based on size)

@param vtk the current value for the pid controller
Takes the next input for the pid controler utilizes constants : PID_SET_POINT - the set point or goal PID_KP - the multiplier for the current distance PID_KI - the multiplier for the " integral " term PID_KD - the multiplier for the " derivative " term This then sets utk which is essentially the result of this calculation Important notes : The derivative terms only look at this and the previous value the integral term currently has no time mitigation and uses a fixed size queue ( now has damping based on size ) @param vtk the current value for the pid controller
void pidStep(uint16_t vtk) {
    etk_1 = etk;
    etk = ((int16_t)vtk) - PID_SET_POINT;
    esum += etk;
    esum -= pushPop(esumQueue, etk);
    int16_t p = PID_KP*etk;
    int16_t i = PID_KI*esum*PID_DT / PID_QSIZE; 
    int16_t d = PID_KD*(etk-etk_1)/PID_DT;
    irobprintf("etk_1: %d\netk: %d\nesum: %d\nutk: %d\np: %d\ni: %d\nd: %d\n",
            etk_1, etk, esum, utk, p, i, d);
    utk = p + i + d;
}
void pidStep ( uint16_t vtk ) { etk_1 = etk ; etk = ( ( int16_t ) vtk ) - PID_SET_POINT ; esum += etk ; esum -= pushPop ( esumQueue , etk ) ; int16_t p = PID_KP * etk ; int16_t i = PID_KI * esum * PID_DT / PID_QSIZE ; int16_t d = PID_KD * ( etk - etk_1 ) / PID_DT ; irobprintf ( " \n \n \n \n \n \n \n " , etk_1 , etk , esum , utk , p , i , d ) ; utk = p + i + d ; }
Takes the next input for the pid controler
 utilizes constants:
 PID_SET_POINT - the set point or goal
 PID_KP - the multiplier for the current distance
 PID_KI - the multiplier for the "integral" term
 PID_KD - the multiplier for the "derivative" term
 This then sets utk which is essentially the result
 of this calculation
 Important notes:
 The derivative terms only look at this and the previous value
 the integral term currently has no time mitigation and uses
 a fixed size queue (now has damping based on size)
Takes the next input for the pid controler utilizes constants : PID_SET_POINT - the set point or goal PID_KP - the multiplier for the current distance PID_KI - the multiplier for the " integral " term PID_KD - the multiplier for the " derivative " term This then sets utk which is essentially the result of this calculation Important notes : The derivative terms only look at this and the previous value the integral term currently has no time mitigation and uses a fixed size queue ( now has damping based on size )
// damping