自己位置推定機能を追加
Dependencies: SBDBT arrc_mbed BNO055
Diff: PIDco.cpp
- Revision:
- 0:a1238c4cd105
- Child:
- 1:ea880e226e5a
diff -r 000000000000 -r a1238c4cd105 PIDco.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PIDco.cpp Wed Jan 19 12:43:26 2022 +0000 @@ -0,0 +1,75 @@ +#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(2048); + v1m.period_us(2048); + + 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; } + + + + + + \ No newline at end of file