File content as of revision 5:e189d55ef292:

#include "PIDco.hpp"

PIDco::PIDco(){
    spd = 0.0;
    Pwm = 0.0;
    dt = 0.05;
    Integral = 0.0;
    Error_b = 0.0;
    Error_a = 0.0;
    diff = 0.0;
    pulse_b = 0.0;
    }
    
void PIDco::cal_spd(){
    diff = pulse_a - pulse_b;
    spd = co * diff / dt;
    pulse_b = pulse_a;
}

void PIDco::cal_Ival(){
    Integral += (Error_a + Error_b) / 2.0 * dt;
    Ival = Ki * Integral;
}

void PIDco::pass_val(double PULSE,double TARGET){
    pulse_a = PULSE;
    Target = TARGET;
}

void PIDco::wheel_ctl(PinName PIN_A,PinName PIN_B){
    PwmOut v1p(PIN_A);
    PwmOut v1m(PIN_B);
    
    v1p.period_us(1024);
    v1m.period_us(1024);
    
    cal_spd();
    cal_Error();
    cal_Pval();
    cal_Ival();
    cal_Dval();
    cal_pwm();
    
    if(Pwm > 0){
            v1p = Pwm > 0.5 ? 0.5 : Pwm;
            v1m = 0;
        }else if(Pwm < 0){
            v1p = 0;
            v1m = -Pwm > 0.5 ? 0.5 : -Pwm;
        }else{
            v1p = 0;
            v1m = 0;
        }
    
    renew_Error();
}
    

void PIDco::cal_Error(){ Error_a = Target - spd; }

void PIDco::cal_Pval(){ Pval = Kp * Error_a; }

void PIDco::cal_Dval(){ Dval = Kd * (Error_a - Error_b) / dt; }

void PIDco::cal_pwm(){ Pwm += (Pval + Ival + Dval); }

void PIDco::renew_Error(){ Error_b = Error_a; }

double PIDco::obt_spd(){ return spd; }