File content as of revision 2:e340bdcb9bca:

#include "mbed.h"
#include "cal_PID.hpp"

cal_pid::cal_pid(){
    past_error=0;
    integral_error=0;
    p_gain=1.0;
    i_gain=0.0;
    d_gain=0.0;
    control_period=0.001;
    output_min=-128;
    output_max=127;
}

cal_pid::~cal_pid(){    
}

void cal_pid::param(float _p_gain,float _i_gain,float _d_gain){
    p_gain=_p_gain;
    i_gain=_i_gain;
    d_gain=_d_gain;
}
void cal_pid::period(float _control_period){
    if(_control_period<=0)return;
    control_period=_control_period;
}
void cal_pid::output(signed long _min,signed long _max){
    output_min=_min;
    output_max=_max;
}
void cal_pid::reset(){
    past_error=0;
    integral_error=0;
}
signed long cal_pid::get_pid(signed long position,signed long targ, int out_dir){
    error = targ - position;
    integral_error+=error*control_period;
    differential_error=(error-past_error)/control_period;
    past_error=error;
    p_part=error*p_gain;
    i_part=integral_error*i_gain;
    d_part=differential_error*d_gain;
    operate=p_part+i_part+d_part;
    operate *= out_dir;
    if(operate<output_min)operate=output_min;
    else if(operate>output_max)operate=output_max;
    
    return operate;
    
}