自己位置推定機能を追加
Dependencies: SBDBT arrc_mbed BNO055
PIDco.cpp
00001 #include "PIDco.hpp" 00002 00003 PIDco::PIDco(){ 00004 spd = 0.0; 00005 Pwm = 0.0; 00006 dt = 0.05; 00007 Integral = 0.0; 00008 Error_b = 0.0; 00009 Error_a = 0.0; 00010 diff = 0.0; 00011 pulse_b = 0.0; 00012 } 00013 00014 void PIDco::cal_spd(){ 00015 diff = pulse_a - pulse_b; 00016 spd = co * diff / dt; 00017 pulse_b = pulse_a; 00018 } 00019 00020 void PIDco::cal_Ival(){ 00021 Integral += (Error_a + Error_b) / 2.0 * dt; 00022 Ival = Ki * Integral; 00023 } 00024 00025 void PIDco::pass_val(double PULSE,double TARGET){ 00026 pulse_a = PULSE; 00027 Target = TARGET; 00028 } 00029 00030 void PIDco::wheel_ctl(PinName PIN_A,PinName PIN_B){ 00031 PwmOut v1p(PIN_A); 00032 PwmOut v1m(PIN_B); 00033 00034 v1p.period_us(1024); 00035 v1m.period_us(1024); 00036 00037 cal_spd(); 00038 cal_Error(); 00039 cal_Pval(); 00040 cal_Ival(); 00041 cal_Dval(); 00042 cal_pwm(); 00043 00044 if(Pwm > 0){ 00045 v1p = Pwm > 0.5 ? 0.5 : Pwm; 00046 v1m = 0; 00047 }else if(Pwm < 0){ 00048 v1p = 0; 00049 v1m = -Pwm > 0.5 ? 0.5 : -Pwm; 00050 }else{ 00051 v1p = 0; 00052 v1m = 0; 00053 } 00054 00055 renew_Error(); 00056 } 00057 00058 00059 void PIDco::cal_Error(){ Error_a = Target - spd; } 00060 00061 void PIDco::cal_Pval(){ Pval = Kp * Error_a; } 00062 00063 void PIDco::cal_Dval(){ Dval = Kd * (Error_a - Error_b) / dt; } 00064 00065 void PIDco::cal_pwm(){ Pwm += (Pval + Ival + Dval); } 00066 00067 void PIDco::renew_Error(){ Error_b = Error_a; } 00068 00069 double PIDco::obt_spd(){ return spd; } 00070 00071 00072 00073 00074 00075
Generated on Thu Jul 14 2022 19:42:28 by 1.7.2