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;

}

#ifndef _PID_CONTROL_H_

#define _PID_CONTROL_H_

class PID_control

{

  public:

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

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

    void change_goal(float goal);

    float pid(float input)

  private:

    int k_p;

    int k_i;

    int k_d;

    float set_config;

    float error;

    float prev_error;

    float P_err;

    float I_err;

    float D_err;

};

#endif