src/temperature/pid.c

Go to the documentation of this file.

/*
 * pid.c
 *
 * Copyright 2011 Pieter Agten
 *
 * This file is part of Yarf.
 *
 * Yarf is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * Yarf is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with Yarf.  If not, see <http://www.gnu.org/licenses/>.
 *
 */

#include "pid.h"
#include "yarf.h"

#include "util/math.h"

#include <stdint.h>

static inline void
reset_state(pid_t *pid) {
  pid->i_state = 0;
}

void 
pid_init(pid_t *pid, uint8_t envelope, uint8_t p, uint8_t i, uint8_t d)
{
  pid->envelope = envelope;
  pid->p_gain = p;
  pid->i_gain = i;
  pid->d_gain = d;
  pid->i_state = 0;
  pid->d_term = 0;
  pid->target_temp = 0;
  pid->last_temp = 0;
}


void
pid_set_target(pid_t *pid, float target_temp)
{
  pid->target_temp = target_temp;
}



uint8_t
pid_next(pid_t *pid, float current_temp)
{
  float error = pid->target_temp - current_temp;
  if (error > pid->envelope) {
    reset_state(pid);
    return 255;
  } else if (error < -pid->envelope) {
    reset_state(pid);
    return 0;
  } else {
    float result = 0;

    // Proportional:
    result = error * pid->p_gain;

    // Integral:
    pid->i_state += error;
    result += pid->i_state * pid->i_gain;

    // Derivative
    pid->d_term = ((pid->last_temp - current_temp)*0.2 + pid->d_term * 0.8) * pid->d_gain;
    pid->last_temp = current_temp;
    result += pid->d_term;

    return (uint8_t)MIN(MAX(lround(result),0),255);
  }
}