<commit_before>
#include "pid.h"

/*
 * This code is based on <NAME>'s Improved Beginner PID series of
 * blog posts [1] with additions for self-tuning behavior based on the paper
 * "Self-Tuning of PID Controllers by Adaptive Interaction." by <NAME>,
 * <NAME>, and <NAME> [2]
 *
 * [1] http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-introduction/
 * [2] http://www.ece.eng.wayne.edu/~flin/Conference/AI-PID.pdf
 */

void
pid_init(pid_t* pid)
{
  pid->sample_time = MS2ST(1000);
  pid->last_time   = (chTimeNow() - pid->sample_time);
  pid->enabled     = false;

  pid_set_gains(pid, 10, 0, 0);
}

void
pid_exec(pid_t* pid, float setpoint, float sample)
{
  if (!pid->enabled)
    return;

  systime_t now = chTimeNow();
  systime_t time_diff = (now - pid->last_time);

  if (time_diff >= pid->sample_time) {
    float err_p = (setpoint - sample);

    pid->err_i += (pid->ki * err_p);
    pid->err_i = LIMIT(pid->err_i, pid->out_min, pid->out_max);

    float err_d = (sample - pid->last_sample);

    tune_gains(pid, err_p, err_d);

    pid->out = (pid->kp * err_p) + (pid->err_i - pid->kd) * err_d;
    pid->out = LIMIT(pid->out, pid->out_min, pid->out_max);

    pid->last_err  = err_p;
    pid->last_sample = sample;
    pid->last_time = now;
  }
}

#define GAMMA 0.03
void
tune_gains(pid_t* pid, float err_p, float err_d)
{
  /* Recalculate gains using Lin et al algorithm */
  pid->kp += -GAMMA * err_p;
  //pid->kp = LIMIT(pid->kp, 0, 20);
<commit_msg>Fix order of output pid operations<commit_after>
#include "pid.h"

/*
 * This code is based on <NAME>'s Improved Beginner PID series of
 * blog posts [1] with additions for self-tuning behavior based on the paper
 * "Self-Tuning of PID Controllers by Adaptive Interaction." by <NAME>,
 * <NAME>, and <NAME> [2]
 *
 * [1] http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-introduction/
 * [2] http://www.ece.eng.wayne.edu/~flin/Conference/AI-PID.pdf
 */

void
pid_init(pid_t* pid)
{
  pid->sample_time = MS2ST(1000);
  pid->last_time   = (chTimeNow() - pid->sample_time);
  pid->enabled     = false;

  pid_set_gains(pid, 10, 0, 0);
}

void
pid_exec(pid_t* pid, float setpoint, float sample)
{
  if (!pid->enabled)
    return;

  systime_t now = chTimeNow();
  systime_t time_diff = (now - pid->last_time);

  if (time_diff >= pid->sample_time) {
    float err_p = (setpoint - sample);
    float err_d = (sample - pid->last_sample);

    pid->err_i += (pid->ki * err_p);
    pid->err_i = LIMIT(pid->err_i, pid->out_min, pid->out_max);

    tune_gains(pid, err_p, err_d);

    pid->out = (pid->kp * err_p) + pid->err_i - (pid->kd * err_d);
    pid->out = LIMIT(pid->out, pid->out_min, pid->out_max);

    pid->last_err  = err_p;
    pid->last_sample = sample;
    pid->last_time = now;
  }
}

#define GAMMA 0.03
void
tune_gains(pid_t* pid, float err_p, float err_d)
{
  pid->kp += -GAMMA * err_p;

{'fraction_non_alphanumeric': 0.16580310880829016, 'fraction_numerical': 0.01623488773747841, 'mean_word_length': 3.341829085457271, 'pattern_counts': {'":': 0, '<': 11, '<?xml version=': 0, '>': 67, 'https://': 0, 'lorem ipsum': 0, 'www.': 2, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '459975', 'n_tokens_mistral': 1215, 'n_tokens_neox': 1159, 'n_words': 264}