自己位置推定機能を追加
Dependencies: SBDBT arrc_mbed BNO055
Diff: PIDco.hpp
- Revision:
- 0:a1238c4cd105
diff -r 000000000000 -r a1238c4cd105 PIDco.hpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PIDco.hpp Wed Jan 19 12:43:26 2022 +0000 @@ -0,0 +1,42 @@ +#ifndef PID_CO_H +#define PID_CO_H +#include "mbed.h" + +#define Kp 0.0001 +#define Ki 0.0000014 +#define Kd 0.0000001 +#define co (108 * 3.1415926535897) / 500 + + +class PIDco{ + + public://use in main.cpp + PIDco(); + double obt_spd(); + void pass_val(double PULSE,double TARGET); + void wheel_ctl(PinName PIN_A,PinName PIN_B); + + private: + void cal_spd(); + void cal_Error(); + void renew_Error(); + void cal_Pval(); + void cal_Ival(); + void cal_Dval(); + void cal_pwm(); + + double spd; + double Target,Error_a,Error_b; + double Pval,Ival,Dval,Pwm; + double Integral,dt; + double diff,pulse_a,pulse_b; + double v1p_set,v1m_set; +}; + +#endif + + + + + + \ No newline at end of file