2022_gorobo_Ateam / Mbed 2 deprecated PIDcontrol_4

足回りプログラム

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?

UserRevisionLine numberNew 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