2022_gorobo_Ateam / gourobo2022_undercarriage_new

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

Dependencies:   SBDBT arrc_mbed BNO055

PIDco.cpp@13:369f4abc1f36, 2022-04-08 (annotated)

Committer:
guesta
Date:
Fri Apr 08 09:14:20 2022 +0000
Revision:
13:369f4abc1f36
Parent:
12:894e5ac49810 
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;
guesta 13:369f4abc1f36 50 //printf("%lf\n",Pwm);
kazumayamanaka 0:a1238c4cd105 51
guesta 7:a0375e6dc8ca 52 if(Pwm > 0){
guesta 12:894e5ac49810 53 v1p = Pwm > 0.68 ? 0.68 : Pwm;
kazumayamanaka 0:a1238c4cd105 54 v1m = 0;
kazumayamanaka 0:a1238c4cd105 55 }else if(Pwm < 0){
kazumayamanaka 0:a1238c4cd105 56 v1p = 0;
guesta 12:894e5ac49810 57 v1m = -Pwm > 0.68 ? 0.68 : -Pwm;
kazumayamanaka 0:a1238c4cd105 58 }else{
kazumayamanaka 0:a1238c4cd105 59 v1p = 0;
kazumayamanaka 0:a1238c4cd105 60 v1m = 0;
kazumayamanaka 0:a1238c4cd105 61 }
kazumayamanaka 0:a1238c4cd105 62 renew_Error();
kazumayamanaka 0:a1238c4cd105 63 }
kazumayamanaka 0:a1238c4cd105 64
guesta 10:ad8fced7d6b6 65 void PIDco::pid_ctl(double now_angle,double TARGET,double p_gain,double i_gain,double d_gain){
guesta 7:a0375e6dc8ca 66 Target = TARGET;
guesta 7:a0375e6dc8ca 67 Kp = p_gain;
guesta 7:a0375e6dc8ca 68 Ki = i_gain;
guesta 7:a0375e6dc8ca 69 Kd = d_gain;
guesta 10:ad8fced7d6b6 70
guesta 10:ad8fced7d6b6 71 Error_a = Target - now_angle;
guesta 7:a0375e6dc8ca 72 cal_Pval();
guesta 7:a0375e6dc8ca 73 cal_Ival();
guesta 7:a0375e6dc8ca 74 cal_Dval();
guesta 10:ad8fced7d6b6 75 Pwm = (Pval + Ival + Dval);
guesta 7:a0375e6dc8ca 76 output = Pwm;
guesta 10:ad8fced7d6b6 77 //printf("%lf\n",Error_a);
guesta 10:ad8fced7d6b6 78 renew_Error();
guesta 7:a0375e6dc8ca 79 }
kazumayamanaka 0:a1238c4cd105 80
kazumayamanaka 0:a1238c4cd105 81 void PIDco::cal_Error(){ Error_a = Target - spd; }
kazumayamanaka 0:a1238c4cd105 82
kazumayamanaka 0:a1238c4cd105 83 void PIDco::cal_Pval(){ Pval = Kp * Error_a; }
kazumayamanaka 0:a1238c4cd105 84
kazumayamanaka 0:a1238c4cd105 85 void PIDco::cal_Dval(){ Dval = Kd * (Error_a - Error_b) / dt; }
kazumayamanaka 0:a1238c4cd105 86
guesta 7:a0375e6dc8ca 87 void PIDco::cal_pwm(){
guesta 7:a0375e6dc8ca 88 Pwm += (Pval + Ival + Dval);
guesta 9:569c0f55dd9b 89 if(Pwm <= 0.0001 && Pwm >= -0.0001){
guesta 7:a0375e6dc8ca 90 Pwm = 0.0;
guesta 7:a0375e6dc8ca 91 }
guesta 7:a0375e6dc8ca 92 //printf("%lf\n",Pwm);
guesta 7:a0375e6dc8ca 93 }
kazumayamanaka 0:a1238c4cd105 94
kazumayamanaka 0:a1238c4cd105 95 void PIDco::renew_Error(){ Error_b = Error_a; }
kazumayamanaka 0:a1238c4cd105 96
kazumayamanaka 0:a1238c4cd105 97 double PIDco::obt_spd(){ return spd; }