2022_Ateam_MOTORprogramをscrp_slaveでメインマイコンからコントローラー状況を読み取れるように改良。 また、モータに0以外のpwmが送られている場合に基盤付属のledが点灯するようにした。
Dependencies: SBDBT arrc_mbed BNO055
PIDco.cpp
- Committer:
- guesta
- Date:
- 2022-03-11
- Revision:
- 10:ad8fced7d6b6
- Parent:
- 9:569c0f55dd9b
- Child:
- 11:264f992664b0
File content as of revision 10:ad8fced7d6b6:
#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; 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::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; }