// PID implementation

int16_t last_pid_temp = 0;

uint8_t last_pid_temp_frac = 0;

int16_t i_state = 0;

#define WINDUP_GUARD_GAIN 100

int16_t update_pid(uint16_t k_p, uint16_t k_i, uint16_t k_d, int16_t temp, uint8_t temp_frac, int16_t setpoint) 

{

  // Calculate instantaneous error

  int16_t error = (int16_t)setpoint - (int16_t)temp; // TODO: Use fixed point fraction

  // to prevent the iTerm getting huge despite lots of 

  //  error, we use a "windup guard" 

  // (this happens when the machine is first turned on and

  // it cant help be cold despite its best efforts)

  // not necessary, but this makes windup guard values 

  // relative to the current iGain

  // Calculate integral term with windup guard 

  int16_t i_term = k_i * i_state;

  // Calculate differential term (slope since last iteration)

  int16_t d_term = (k_d * (temp - last_pid_temp));

  // Save temperature for next iteration

uint32_t last_ssr_on = 0;

uint32_t last_led = 0;

int32_t setpoint = 0;

uint16_t k_p = 1;

uint16_t k_i = 1;

uint16_t k_d = 1;

int16_t ssr_output = 0; // Duty cycle of ssr, 0 to SSR_PERIOD 

uint8_t pid_enabled = 0;

// Process things

void process()

{