2022_gorobo_Ateam / 2022_Ateam_MOTORprogram

足回り用プログラム(修正版)

Dependencies:   SBDBT arrc_mbed BNO055

PIDco.cpp@5:e189d55ef292, 2022-01-28 (annotated)

Committer:
kazumayamanaka
Date:
Fri Jan 28 15:12:04 2022 +0000
Revision:
5:e189d55ef292
Parent:
1:ea880e226e5a 
program  of wheel;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kazumayamanaka 0:a1238c4cd105 1 #include "PIDco.hpp"
kazumayamanaka 0:a1238c4cd105 2
kazumayamanaka 0:a1238c4cd105 3 PIDco::PIDco(){
kazumayamanaka 5:e189d55ef292 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 5:e189d55ef292 34 v1p.period_us(1024);
kazumayamanaka 5:e189d55ef292 35 v1m.period_us(1024);
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
kazumayamanaka 0:a1238c4cd105 55 renew_Error();
kazumayamanaka 0:a1238c4cd105 56 }
kazumayamanaka 0:a1238c4cd105 57
kazumayamanaka 0:a1238c4cd105 58
kazumayamanaka 0:a1238c4cd105 59 void PIDco::cal_Error(){ Error_a = Target - spd; }
kazumayamanaka 0:a1238c4cd105 60
kazumayamanaka 0:a1238c4cd105 61 void PIDco::cal_Pval(){ Pval = Kp * Error_a; }
kazumayamanaka 0:a1238c4cd105 62
kazumayamanaka 0:a1238c4cd105 63 void PIDco::cal_Dval(){ Dval = Kd * (Error_a - Error_b) / dt; }
kazumayamanaka 0:a1238c4cd105 64
kazumayamanaka 0:a1238c4cd105 65 void PIDco::cal_pwm(){ Pwm += (Pval + Ival + Dval); }
kazumayamanaka 0:a1238c4cd105 66
kazumayamanaka 0:a1238c4cd105 67 void PIDco::renew_Error(){ Error_b = Error_a; }
kazumayamanaka 0:a1238c4cd105 68
kazumayamanaka 0:a1238c4cd105 69 double PIDco::obt_spd(){ return spd; }
kazumayamanaka 0:a1238c4cd105 70
kazumayamanaka 0:a1238c4cd105 71
kazumayamanaka 0:a1238c4cd105 72
kazumayamanaka 0:a1238c4cd105 73
kazumayamanaka 0:a1238c4cd105 74
kazumayamanaka 0:a1238c4cd105 75