自己位置推定機能を追加
Dependencies: SBDBT arrc_mbed BNO055
PIDco.cpp@3:612efbe94f42, 2022-01-21 (annotated)
- Committer:
- guesta
- Date:
- Fri Jan 21 08:57:07 2022 +0000
- Revision:
- 3:612efbe94f42
- Parent:
- 1:ea880e226e5a
first_commit;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
kazumayamanaka | 0:a1238c4cd105 | 1 | #include "PIDco.hpp" |
kazumayamanaka | 0:a1238c4cd105 | 2 | |
kazumayamanaka | 0:a1238c4cd105 | 3 | PIDco::PIDco(){ |
kazumayamanaka | 1:ea880e226e5a | 4 | spd = 0.0; |
kazumayamanaka | 0:a1238c4cd105 | 5 | Pwm = 0.0; |
kazumayamanaka | 0:a1238c4cd105 | 6 | dt = 0.05; |
kazumayamanaka | 0:a1238c4cd105 | 7 | Integral = 0.0; |
kazumayamanaka | 0:a1238c4cd105 | 8 | Error_b = 0.0; |
kazumayamanaka | 0:a1238c4cd105 | 9 | Error_a = 0.0; |
kazumayamanaka | 0:a1238c4cd105 | 10 | diff = 0.0; |
kazumayamanaka | 0:a1238c4cd105 | 11 | pulse_b = 0.0; |
kazumayamanaka | 0:a1238c4cd105 | 12 | } |
kazumayamanaka | 0:a1238c4cd105 | 13 | |
kazumayamanaka | 0:a1238c4cd105 | 14 | void PIDco::cal_spd(){ |
kazumayamanaka | 0:a1238c4cd105 | 15 | diff = pulse_a - pulse_b; |
kazumayamanaka | 0:a1238c4cd105 | 16 | spd = co * diff / dt; |
kazumayamanaka | 0:a1238c4cd105 | 17 | pulse_b = pulse_a; |
kazumayamanaka | 0:a1238c4cd105 | 18 | } |
kazumayamanaka | 0:a1238c4cd105 | 19 | |
kazumayamanaka | 0:a1238c4cd105 | 20 | void PIDco::cal_Ival(){ |
kazumayamanaka | 0:a1238c4cd105 | 21 | Integral += (Error_a + Error_b) / 2.0 * dt; |
kazumayamanaka | 0:a1238c4cd105 | 22 | Ival = Ki * Integral; |
kazumayamanaka | 0:a1238c4cd105 | 23 | } |
kazumayamanaka | 0:a1238c4cd105 | 24 | |
kazumayamanaka | 0:a1238c4cd105 | 25 | void PIDco::pass_val(double PULSE,double TARGET){ |
kazumayamanaka | 0:a1238c4cd105 | 26 | pulse_a = PULSE; |
kazumayamanaka | 0:a1238c4cd105 | 27 | Target = TARGET; |
kazumayamanaka | 0:a1238c4cd105 | 28 | } |
kazumayamanaka | 0:a1238c4cd105 | 29 | |
kazumayamanaka | 0:a1238c4cd105 | 30 | void PIDco::wheel_ctl(PinName PIN_A,PinName PIN_B){ |
kazumayamanaka | 0:a1238c4cd105 | 31 | PwmOut v1p(PIN_A); |
kazumayamanaka | 0:a1238c4cd105 | 32 | PwmOut v1m(PIN_B); |
kazumayamanaka | 0:a1238c4cd105 | 33 | |
kazumayamanaka | 0:a1238c4cd105 | 34 | v1p.period_us(2048); |
kazumayamanaka | 0:a1238c4cd105 | 35 | v1m.period_us(2048); |
kazumayamanaka | 0:a1238c4cd105 | 36 | |
kazumayamanaka | 0:a1238c4cd105 | 37 | cal_spd(); |
kazumayamanaka | 0:a1238c4cd105 | 38 | cal_Error(); |
kazumayamanaka | 0:a1238c4cd105 | 39 | cal_Pval(); |
kazumayamanaka | 0:a1238c4cd105 | 40 | cal_Ival(); |
kazumayamanaka | 0:a1238c4cd105 | 41 | cal_Dval(); |
kazumayamanaka | 0:a1238c4cd105 | 42 | cal_pwm(); |
kazumayamanaka | 0:a1238c4cd105 | 43 | |
kazumayamanaka | 0:a1238c4cd105 | 44 | if(Pwm > 0){ |
kazumayamanaka | 0:a1238c4cd105 | 45 | v1p = Pwm > 0.5 ? 0.5 : Pwm; |
kazumayamanaka | 0:a1238c4cd105 | 46 | v1m = 0; |
kazumayamanaka | 0:a1238c4cd105 | 47 | }else if(Pwm < 0){ |
kazumayamanaka | 0:a1238c4cd105 | 48 | v1p = 0; |
kazumayamanaka | 0:a1238c4cd105 | 49 | v1m = -Pwm > 0.5 ? 0.5 : -Pwm; |
kazumayamanaka | 0:a1238c4cd105 | 50 | }else{ |
kazumayamanaka | 0:a1238c4cd105 | 51 | v1p = 0; |
kazumayamanaka | 0:a1238c4cd105 | 52 | v1m = 0; |
kazumayamanaka | 0:a1238c4cd105 | 53 | } |
kazumayamanaka | 0:a1238c4cd105 | 54 | renew_Error(); |
kazumayamanaka | 0:a1238c4cd105 | 55 | } |
kazumayamanaka | 0:a1238c4cd105 | 56 | |
kazumayamanaka | 0:a1238c4cd105 | 57 | |
kazumayamanaka | 0:a1238c4cd105 | 58 | void PIDco::cal_Error(){ Error_a = Target - spd; } |
kazumayamanaka | 0:a1238c4cd105 | 59 | |
kazumayamanaka | 0:a1238c4cd105 | 60 | void PIDco::cal_Pval(){ Pval = Kp * Error_a; } |
kazumayamanaka | 0:a1238c4cd105 | 61 | |
kazumayamanaka | 0:a1238c4cd105 | 62 | void PIDco::cal_Dval(){ Dval = Kd * (Error_a - Error_b) / dt; } |
kazumayamanaka | 0:a1238c4cd105 | 63 | |
kazumayamanaka | 0:a1238c4cd105 | 64 | void PIDco::cal_pwm(){ Pwm += (Pval + Ival + Dval); } |
kazumayamanaka | 0:a1238c4cd105 | 65 | |
kazumayamanaka | 0:a1238c4cd105 | 66 | void PIDco::renew_Error(){ Error_b = Error_a; } |
kazumayamanaka | 0:a1238c4cd105 | 67 | |
kazumayamanaka | 0:a1238c4cd105 | 68 | double PIDco::obt_spd(){ return spd; } |
kazumayamanaka | 0:a1238c4cd105 | 69 | |
kazumayamanaka | 0:a1238c4cd105 | 70 | |
kazumayamanaka | 0:a1238c4cd105 | 71 | |
kazumayamanaka | 0:a1238c4cd105 | 72 | |
kazumayamanaka | 0:a1238c4cd105 | 73 | |
kazumayamanaka | 0:a1238c4cd105 | 74 |