![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
足回りプログラム
Dependencies: mbed SBDBT arrc_mbed
PIDcontrol.cpp@0:4f39632d7a42, 2022-01-17 (annotated)
- Committer:
- kazumayamanaka
- Date:
- Mon Jan 17 02:40:16 2022 +0000
- Revision:
- 0:4f39632d7a42
this program is to motor control;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
kazumayamanaka | 0:4f39632d7a42 | 1 | #include "PIDcontrol.hpp" |
kazumayamanaka | 0:4f39632d7a42 | 2 | |
kazumayamanaka | 0:4f39632d7a42 | 3 | //コンストラクタ |
kazumayamanaka | 0:4f39632d7a42 | 4 | PIDcontrol::PIDcontrol(){ |
kazumayamanaka | 0:4f39632d7a42 | 5 | Kp=0.0001; |
kazumayamanaka | 0:4f39632d7a42 | 6 | Ki=0.0000014; |
kazumayamanaka | 0:4f39632d7a42 | 7 | Kd=0.0000001; |
kazumayamanaka | 0:4f39632d7a42 | 8 | spd=0.0; |
kazumayamanaka | 0:4f39632d7a42 | 9 | pwm=0.0; |
kazumayamanaka | 0:4f39632d7a42 | 10 | dt=0.05; |
kazumayamanaka | 0:4f39632d7a42 | 11 | integral=0.0; |
kazumayamanaka | 0:4f39632d7a42 | 12 | finish=0; |
kazumayamanaka | 0:4f39632d7a42 | 13 | pal=0; |
kazumayamanaka | 0:4f39632d7a42 | 14 | } |
kazumayamanaka | 0:4f39632d7a42 | 15 | |
kazumayamanaka | 0:4f39632d7a42 | 16 | |
kazumayamanaka | 0:4f39632d7a42 | 17 | //spd計算 |
kazumayamanaka | 0:4f39632d7a42 | 18 | void PIDcontrol::speed(){ |
kazumayamanaka | 0:4f39632d7a42 | 19 | finish = pulse - pal; |
kazumayamanaka | 0:4f39632d7a42 | 20 | spd = co*finish/dt; |
kazumayamanaka | 0:4f39632d7a42 | 21 | pal = pulse; |
kazumayamanaka | 0:4f39632d7a42 | 22 | } |
kazumayamanaka | 0:4f39632d7a42 | 23 | |
kazumayamanaka | 0:4f39632d7a42 | 24 | |
kazumayamanaka | 0:4f39632d7a42 | 25 | //pwm計算 |
kazumayamanaka | 0:4f39632d7a42 | 26 | void PIDcontrol::control(){ |
kazumayamanaka | 0:4f39632d7a42 | 27 | error_n = target-spd; |
kazumayamanaka | 0:4f39632d7a42 | 28 | integral += (error_n+error_b) / 2.0 * dt; |
kazumayamanaka | 0:4f39632d7a42 | 29 | P = Kp * error_n; |
kazumayamanaka | 0:4f39632d7a42 | 30 | I = Ki * integral; |
kazumayamanaka | 0:4f39632d7a42 | 31 | D = Kd * (error_n-error_b) / dt; |
kazumayamanaka | 0:4f39632d7a42 | 32 | pwm += (P+I+D); |
kazumayamanaka | 0:4f39632d7a42 | 33 | error_b=error_n; |
kazumayamanaka | 0:4f39632d7a42 | 34 | } |
kazumayamanaka | 0:4f39632d7a42 | 35 | |
kazumayamanaka | 0:4f39632d7a42 | 36 | //モーター制御 |
kazumayamanaka | 0:4f39632d7a42 | 37 | void PIDcontrol::motor_control(double var_1,double var_2,PinName pin_a,PinName pin_b){ |
kazumayamanaka | 0:4f39632d7a42 | 38 | PwmOut v1p(pin_a); |
kazumayamanaka | 0:4f39632d7a42 | 39 | PwmOut v1m(pin_b); |
kazumayamanaka | 0:4f39632d7a42 | 40 | v1p.period_us(2048); |
kazumayamanaka | 0:4f39632d7a42 | 41 | v1m.period_us(2048); |
kazumayamanaka | 0:4f39632d7a42 | 42 | target=var_1; |
kazumayamanaka | 0:4f39632d7a42 | 43 | pulse=var_2; |
kazumayamanaka | 0:4f39632d7a42 | 44 | speed(); |
kazumayamanaka | 0:4f39632d7a42 | 45 | control(); |
kazumayamanaka | 0:4f39632d7a42 | 46 | if(pwm>0){ |
kazumayamanaka | 0:4f39632d7a42 | 47 | v1p = pwm > 0.5 ? 0.5 : pwm; |
kazumayamanaka | 0:4f39632d7a42 | 48 | v1m=0; |
kazumayamanaka | 0:4f39632d7a42 | 49 | }else if(pwm<0){ |
kazumayamanaka | 0:4f39632d7a42 | 50 | v1p = 0; |
kazumayamanaka | 0:4f39632d7a42 | 51 | v1m = -pwm > 0.5 ? 0.5 : -pwm; |
kazumayamanaka | 0:4f39632d7a42 | 52 | }else{ |
kazumayamanaka | 0:4f39632d7a42 | 53 | v1p = 0; |
kazumayamanaka | 0:4f39632d7a42 | 54 | v1m = 0; |
kazumayamanaka | 0:4f39632d7a42 | 55 | } |
kazumayamanaka | 0:4f39632d7a42 | 56 | } |
kazumayamanaka | 0:4f39632d7a42 | 57 | |
kazumayamanaka | 0:4f39632d7a42 | 58 | //spdをreturnする |
kazumayamanaka | 0:4f39632d7a42 | 59 | double PIDcontrol::get_spd(){ |
kazumayamanaka | 0:4f39632d7a42 | 60 | return spd; |
kazumayamanaka | 0:4f39632d7a42 | 61 | } |
kazumayamanaka | 0:4f39632d7a42 | 62 | |
kazumayamanaka | 0:4f39632d7a42 | 63 |