// FIXME this was never getting called. Try again sometime.

    //bootloader_unset();

    HAL_Delay(500);

    HAL_Delay(1000);

    HAL_Delay(1000);

#include "stm32f0xx_hal.h"

#include "states.h"

// PID implementation

// TODO: Make struct that has the last_temp and i_state in it, pass by ref. Make struct that has other input values maybe.

static int16_t last_pid_temp = 0;

static uint8_t last_pid_temp_frac = 0;

static int32_t i_state = 0;

int16_t pid_update(uint16_t k_p, uint16_t k_i, uint16_t k_d, int16_t temp, uint8_t temp_frac, int16_t setpoint, therm_settings_t* set, therm_status_t* status) 

{

  // Calculate instantaneous error

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

  // Proportional component

  int32_t p_term = k_p * error;

  // Error accumulator (integrator)

  i_state += error;

  // to prevent the iTerm getting huge from 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

  int32_t windup_guard_res = set->val.windup_guard / k_i;  

  // Calculate integral term with windup guard 

  if (i_state > windup_guard_res) 

    i_state = windup_guard_res;

  else if (i_state < -windup_guard_res) 

    i_state = -windup_guard_res;

  int32_t i_term = k_i * i_state;

  // Calculate differential term (slope since last iteration)

  int32_t d_term = (k_d * (status->temp - last_pid_temp));

  // Save temperature for next iteration

  last_pid_temp = status->temp;

  last_pid_temp_frac = status->temp_frac;

  int16_t result = p_term + i_term - d_term;

  // Put out tenths of percent, 0-1000. 

  if(result > 1000)

    result = 1000;

  else if(result < -1000)

    result = -1000;

  // Return feedback

  return result;

}

#ifndef PIDS_H

#define PIDS_H

#include "states.h"

int16_t pid_update(uint16_t k_p, uint16_t k_i, uint16_t k_d, int16_t temp, uint8_t temp_frac, int16_t setpoint, therm_settings_t* set, therm_status_t* status);

#endif