File content as of revision 0:4f39632d7a42:

#include "PIDcontrol.hpp"

//コンストラクタ
PIDcontrol::PIDcontrol(){
    Kp=0.0001;
    Ki=0.0000014;
    Kd=0.0000001;
    spd=0.0;
    pwm=0.0;
    dt=0.05;
    integral=0.0;
    finish=0;
    pal=0;
}


//spd計算
void PIDcontrol::speed(){
     finish = pulse - pal;
        spd = co*finish/dt;
        pal = pulse;
}


//pwm計算
void PIDcontrol::control(){
        error_n = target-spd;
        integral += (error_n+error_b) / 2.0 * dt;
        P = Kp * error_n;
        I = Ki * integral;
        D = Kd * (error_n-error_b) / dt;
        pwm += (P+I+D);
        error_b=error_n;
}

//モーター制御
void PIDcontrol::motor_control(double var_1,double var_2,PinName pin_a,PinName pin_b){
    PwmOut v1p(pin_a);
    PwmOut v1m(pin_b);
    v1p.period_us(2048);
    v1m.period_us(2048);
    target=var_1;
    pulse=var_2;
    speed();
    control();
    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;
        }
}

//spdをreturnする
double PIDcontrol::get_spd(){
    return spd;
}