2022_gorobo_Ateam / gourobo2022_undercarriage_new

2022_Ateam_MOTORprogramをscrp_slaveでメインマイコンからコントローラー状況を読み取れるように改良。 また、モータに0以外のpwmが送られている場合に基盤付属のledが点灯するようにした。

Dependencies:   SBDBT arrc_mbed BNO055

PIDco.cpp

Committer:
guesta
Date:
2022-04-08
Revision:
13:369f4abc1f36
Parent:
12:894e5ac49810

File content as of revision 13:369f4abc1f36:

#include "PIDco.hpp"
#include "mbed.h"

Serial pc(USBTX,USBRX);

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,double p_gain,double i_gain,double d_gain){
    pulse_a = PULSE;
    Target = TARGET;
    Kp = p_gain;
    Ki = i_gain;
    Kd = d_gain;
}

void PIDco::wheel_ctl(PinName PIN_A,PinName PIN_B,double Regulation){
    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();
    Pwm = Pwm * Regulation;
    //printf("%lf\n",Pwm);
    
        if(Pwm > 0){
            v1p = Pwm > 0.68 ? 0.68 : Pwm;
            v1m = 0;
        }else if(Pwm < 0){
            v1p = 0;
            v1m = -Pwm > 0.68 ? 0.68 : -Pwm;
        }else{
            v1p = 0;
            v1m = 0;
        }
    renew_Error();
}
    
void PIDco::pid_ctl(double now_angle,double TARGET,double p_gain,double i_gain,double d_gain){
    Target = TARGET;
    Kp = p_gain;
    Ki = i_gain;
    Kd = d_gain;
    
    Error_a = Target - now_angle;
    cal_Pval();
    cal_Ival();
    cal_Dval();
    Pwm = (Pval + Ival + Dval);
    output = Pwm;
    //printf("%lf\n",Error_a);
    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); 
    if(Pwm <= 0.0001 && Pwm >= -0.0001){
        Pwm = 0.0;
    }
    //printf("%lf\n",Pwm);
}

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

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