2022_Ateam_MOTORprogramをscrp_slaveでメインマイコンからコントローラー状況を読み取れるように改良。 また、モータに0以外のpwmが送られている場合に基盤付属のledが点灯するようにした。
Dependencies: SBDBT arrc_mbed BNO055
PIDco.cpp@11:264f992664b0, 2022-03-22 (annotated)
- Committer:
- guesta
- Date:
- Tue Mar 22 00:35:37 2022 +0000
- Revision:
- 11:264f992664b0
- Parent:
- 10:ad8fced7d6b6
- Child:
- 12:894e5ac49810
new
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
kazumayamanaka | 0:a1238c4cd105 | 1 | #include "PIDco.hpp" |
guesta | 7:a0375e6dc8ca | 2 | #include "mbed.h" |
guesta | 7:a0375e6dc8ca | 3 | |
guesta | 7:a0375e6dc8ca | 4 | Serial pc(USBTX,USBRX); |
kazumayamanaka | 0:a1238c4cd105 | 5 | |
kazumayamanaka | 0:a1238c4cd105 | 6 | PIDco::PIDco(){ |
kazumayamanaka | 1:ea880e226e5a | 7 | spd = 0.0; |
kazumayamanaka | 0:a1238c4cd105 | 8 | Pwm = 0.0; |
kazumayamanaka | 0:a1238c4cd105 | 9 | dt = 0.05; |
kazumayamanaka | 0:a1238c4cd105 | 10 | Integral = 0.0; |
kazumayamanaka | 0:a1238c4cd105 | 11 | Error_b = 0.0; |
kazumayamanaka | 0:a1238c4cd105 | 12 | Error_a = 0.0; |
kazumayamanaka | 0:a1238c4cd105 | 13 | diff = 0.0; |
kazumayamanaka | 0:a1238c4cd105 | 14 | pulse_b = 0.0; |
kazumayamanaka | 0:a1238c4cd105 | 15 | } |
kazumayamanaka | 0:a1238c4cd105 | 16 | |
kazumayamanaka | 0:a1238c4cd105 | 17 | void PIDco::cal_spd(){ |
kazumayamanaka | 0:a1238c4cd105 | 18 | diff = pulse_a - pulse_b; |
kazumayamanaka | 0:a1238c4cd105 | 19 | spd = co * diff / dt; |
kazumayamanaka | 0:a1238c4cd105 | 20 | pulse_b = pulse_a; |
kazumayamanaka | 0:a1238c4cd105 | 21 | } |
kazumayamanaka | 0:a1238c4cd105 | 22 | |
kazumayamanaka | 0:a1238c4cd105 | 23 | void PIDco::cal_Ival(){ |
kazumayamanaka | 0:a1238c4cd105 | 24 | Integral += (Error_a + Error_b) / 2.0 * dt; |
kazumayamanaka | 0:a1238c4cd105 | 25 | Ival = Ki * Integral; |
kazumayamanaka | 0:a1238c4cd105 | 26 | } |
kazumayamanaka | 0:a1238c4cd105 | 27 | |
guesta | 5:1a850f68a06c | 28 | void PIDco::pass_val(double PULSE,double TARGET,double p_gain,double i_gain,double d_gain){ |
kazumayamanaka | 0:a1238c4cd105 | 29 | pulse_a = PULSE; |
kazumayamanaka | 0:a1238c4cd105 | 30 | Target = TARGET; |
guesta | 5:1a850f68a06c | 31 | Kp = p_gain; |
guesta | 5:1a850f68a06c | 32 | Ki = i_gain; |
guesta | 5:1a850f68a06c | 33 | Kd = d_gain; |
kazumayamanaka | 0:a1238c4cd105 | 34 | } |
kazumayamanaka | 0:a1238c4cd105 | 35 | |
guesta | 10:ad8fced7d6b6 | 36 | void PIDco::wheel_ctl(PinName PIN_A,PinName PIN_B,double Regulation){ |
kazumayamanaka | 0:a1238c4cd105 | 37 | PwmOut v1p(PIN_A); |
kazumayamanaka | 0:a1238c4cd105 | 38 | PwmOut v1m(PIN_B); |
kazumayamanaka | 0:a1238c4cd105 | 39 | |
guesta | 6:e089fda81b74 | 40 | v1p.period_us(1024); |
guesta | 6:e089fda81b74 | 41 | v1m.period_us(1024); |
kazumayamanaka | 0:a1238c4cd105 | 42 | |
kazumayamanaka | 0:a1238c4cd105 | 43 | cal_spd(); |
kazumayamanaka | 0:a1238c4cd105 | 44 | cal_Error(); |
kazumayamanaka | 0:a1238c4cd105 | 45 | cal_Pval(); |
kazumayamanaka | 0:a1238c4cd105 | 46 | cal_Ival(); |
kazumayamanaka | 0:a1238c4cd105 | 47 | cal_Dval(); |
kazumayamanaka | 0:a1238c4cd105 | 48 | cal_pwm(); |
guesta | 10:ad8fced7d6b6 | 49 | Pwm = Pwm * Regulation; |
kazumayamanaka | 0:a1238c4cd105 | 50 | |
guesta | 7:a0375e6dc8ca | 51 | if(Pwm > 0){ |
guesta | 11:264f992664b0 | 52 | v1p = Pwm > 0.6 ? 0.6 : Pwm; |
kazumayamanaka | 0:a1238c4cd105 | 53 | v1m = 0; |
kazumayamanaka | 0:a1238c4cd105 | 54 | }else if(Pwm < 0){ |
kazumayamanaka | 0:a1238c4cd105 | 55 | v1p = 0; |
guesta | 11:264f992664b0 | 56 | v1m = -Pwm > 0.6 ? 0.6 : -Pwm; |
kazumayamanaka | 0:a1238c4cd105 | 57 | }else{ |
kazumayamanaka | 0:a1238c4cd105 | 58 | v1p = 0; |
kazumayamanaka | 0:a1238c4cd105 | 59 | v1m = 0; |
kazumayamanaka | 0:a1238c4cd105 | 60 | } |
kazumayamanaka | 0:a1238c4cd105 | 61 | renew_Error(); |
kazumayamanaka | 0:a1238c4cd105 | 62 | } |
kazumayamanaka | 0:a1238c4cd105 | 63 | |
guesta | 10:ad8fced7d6b6 | 64 | void PIDco::pid_ctl(double now_angle,double TARGET,double p_gain,double i_gain,double d_gain){ |
guesta | 7:a0375e6dc8ca | 65 | Target = TARGET; |
guesta | 7:a0375e6dc8ca | 66 | Kp = p_gain; |
guesta | 7:a0375e6dc8ca | 67 | Ki = i_gain; |
guesta | 7:a0375e6dc8ca | 68 | Kd = d_gain; |
guesta | 10:ad8fced7d6b6 | 69 | |
guesta | 10:ad8fced7d6b6 | 70 | Error_a = Target - now_angle; |
guesta | 7:a0375e6dc8ca | 71 | cal_Pval(); |
guesta | 7:a0375e6dc8ca | 72 | cal_Ival(); |
guesta | 7:a0375e6dc8ca | 73 | cal_Dval(); |
guesta | 10:ad8fced7d6b6 | 74 | Pwm = (Pval + Ival + Dval); |
guesta | 7:a0375e6dc8ca | 75 | output = Pwm; |
guesta | 10:ad8fced7d6b6 | 76 | //printf("%lf\n",Error_a); |
guesta | 10:ad8fced7d6b6 | 77 | renew_Error(); |
guesta | 7:a0375e6dc8ca | 78 | } |
kazumayamanaka | 0:a1238c4cd105 | 79 | |
kazumayamanaka | 0:a1238c4cd105 | 80 | void PIDco::cal_Error(){ Error_a = Target - spd; } |
kazumayamanaka | 0:a1238c4cd105 | 81 | |
kazumayamanaka | 0:a1238c4cd105 | 82 | void PIDco::cal_Pval(){ Pval = Kp * Error_a; } |
kazumayamanaka | 0:a1238c4cd105 | 83 | |
kazumayamanaka | 0:a1238c4cd105 | 84 | void PIDco::cal_Dval(){ Dval = Kd * (Error_a - Error_b) / dt; } |
kazumayamanaka | 0:a1238c4cd105 | 85 | |
guesta | 7:a0375e6dc8ca | 86 | void PIDco::cal_pwm(){ |
guesta | 7:a0375e6dc8ca | 87 | Pwm += (Pval + Ival + Dval); |
guesta | 9:569c0f55dd9b | 88 | if(Pwm <= 0.0001 && Pwm >= -0.0001){ |
guesta | 7:a0375e6dc8ca | 89 | Pwm = 0.0; |
guesta | 7:a0375e6dc8ca | 90 | } |
guesta | 7:a0375e6dc8ca | 91 | //printf("%lf\n",Pwm); |
guesta | 7:a0375e6dc8ca | 92 | } |
kazumayamanaka | 0:a1238c4cd105 | 93 | |
kazumayamanaka | 0:a1238c4cd105 | 94 | void PIDco::renew_Error(){ Error_b = Error_a; } |
kazumayamanaka | 0:a1238c4cd105 | 95 | |
kazumayamanaka | 0:a1238c4cd105 | 96 | double PIDco::obt_spd(){ return spd; } |