Source file: /~heha/basteln/Haus/Lüfter/my_avr449.zip/pid.cpp

#include "pid.h"
#include <stdint.h>

void PID_DATA::Init(int16_t p_factor, int16_t i_factor, int16_t d_factor) {
// Set up PID controller parameters
  // Start values for PID controller
 sumError = 0;
 lastProcessValue = 0;
  // Tuning constants for PID loop
 P_Factor = p_factor;
 I_Factor = i_factor;
 D_Factor = d_factor;
  // Limits to avoid overflow
 maxError = 0x7FFF / (P_Factor + 1);
 maxSumError = 0x3FFFFFFFL / (I_Factor + 1);
}


int16_t PID_DATA::Controller(int16_t setPoint, int16_t processValue) {
 int16_t error, p_term, d_term;
 int32_t i_term, ret, temp;

 error = setPoint - processValue;

  // Calculate Pterm and limit error overflow
 if (error > maxError) p_term = 0x7FFF;
 else if (error < -maxError) p_term = -0x7FFF;
 else p_term = P_Factor * error;

  // Calculate Iterm and limit integral runaway
 temp = sumError + error;
 if (temp > maxSumError) {
  i_term = 0x3FFFFFFFL;
  sumError = maxSumError;
 }else if (temp < -maxSumError) {
  i_term = -0x3FFFFFFFL;
  sumError = -maxSumError;
 }else{
  sumError = temp;
  i_term = I_Factor * sumError;
 }
  // Calculate Dterm
 d_term = D_Factor * (lastProcessValue - processValue);

 lastProcessValue = processValue;

 ret = (p_term + i_term + d_term) / SCALING_FACTOR;
 if (ret > 0x7FFF) ret = 0x7FFF;
 else if (ret < -0x7FFF) ret = -0x7FFF;
 return (int16_t)ret;
}

void PID_DATA::Reset_Integrator() {
 sumError = 0;
}