Quadrotor

#include "pid_control.h"

#include "assert.h"

PID_control::PID_control(int p_term, int d_term, int i_term, float goal); 

{

  P_err = 0;

  I_err = 0;

  D_err = 0;

  k_p = p_term;

  k_d = d_term;

  k_i = i_term;

  set_config = goal;

}

PID_control::PID_control(int p_term, int d_term, int i_term)

{

  P_err = 0;

  I_err = 0;

  D_err = 0;

  k_p = p_term;

  k_d = d_term;

  k_i = i_term;

}

void PID_control::change_goal(float goal)

{

  set_config == goal;

  I_err = 0;

}

float PID_control::pid(float input)

{

  prev_error = error;

  error = set_config - input;

  P_err = error;

  I_err += prev_error;

  D_err = error - prev_error;

  return k_p*P_err + k_i*I_err + k_d * D_err;

}