2022_gorobo_Ateam / gourobo2022_undercarriage_new

2022_Ateam_MOTORprogramをscrp_slaveでメインマイコンからコントローラー状況を読み取れるように改良。 また、モータに0以外のpwmが送られている場合に基盤付属のledが点灯するようにした。

Dependencies:   SBDBT arrc_mbed BNO055

PIDco.cpp@12:894e5ac49810, 2022-04-08 (annotated)

Committer:
guesta
Date:
Fri Apr 08 07:35:55 2022 +0000
Revision:
12:894e5ac49810
Parent:
11:264f992664b0
Child:
13:369f4abc1f36
2022/04/08;

Who changed what in which revision?

UserRevisionLine numberNew 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 12:894e5ac49810 52 v1p = Pwm > 0.68 ? 0.68 : Pwm;
kazumayamanaka 0:a1238c4cd105 53 v1m = 0;
kazumayamanaka 0:a1238c4cd105 54 }else if(Pwm < 0){
kazumayamanaka 0:a1238c4cd105 55 v1p = 0;
guesta 12:894e5ac49810 56 v1m = -Pwm > 0.68 ? 0.68 : -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; }