Keiji Akasaki / Mbed 2 deprecated RoboClaw_PID_control

私が書いた最新のマニュアルPID制御(PID位置制御)のプログラムです

Dependencies:   mbed QEI2 RoboClaw Filter

main.cpp@0:0e3b9c903282, 2021-04-01 (annotated)

Committer:
keiji0604
Date:
Thu Apr 01 05:57:40 2021 +0000
Revision:
0:0e3b9c903282
last

Who changed what in which revision?

UserRevisionLine numberNew contents of line
keiji0604 0:0e3b9c903282 1 #include "mbed.h"
keiji0604 0:0e3b9c903282 2 #include "RoboClaw.h"
keiji0604 0:0e3b9c903282 3 #include "QEI.h"
keiji0604 0:0e3b9c903282 4 #include "Filter.h"
keiji0604 0:0e3b9c903282 5
keiji0604 0:0e3b9c903282 6 Filter filter(0.01);
keiji0604 0:0e3b9c903282 7 Timer t;
keiji0604 0:0e3b9c903282 8 QEI motor(p21, p22, NC, 500, &t, QEI::X4_ENCODING);
keiji0604 0:0e3b9c903282 9 RoboClaw robo(128, 115200, p9, NC);
keiji0604 0:0e3b9c903282 10 Serial pc(USBTX, USBRX, 115200);
keiji0604 0:0e3b9c903282 11 Ticker flipper;
keiji0604 0:0e3b9c903282 12
keiji0604 0:0e3b9c903282 13 int count_r;
keiji0604 0:0e3b9c903282 14 int pre_count = 0;
keiji0604 0:0e3b9c903282 15 int error_PID;
keiji0604 0:0e3b9c903282 16 int goal = 3000;
keiji0604 0:0e3b9c903282 17
keiji0604 0:0e3b9c903282 18 double k_p = 0.00035;
keiji0604 0:0e3b9c903282 19 double k_i = 0.00075;
keiji0604 0:0e3b9c903282 20 double k_d = 0.0003;
keiji0604 0:0e3b9c903282 21
keiji0604 0:0e3b9c903282 22 double pro;
keiji0604 0:0e3b9c903282 23 double integ;
keiji0604 0:0e3b9c903282 24 double diff;
keiji0604 0:0e3b9c903282 25 double s_t = 0.01;
keiji0604 0:0e3b9c903282 26
keiji0604 0:0e3b9c903282 27 double def;
keiji0604 0:0e3b9c903282 28 int s_d = 0.0;
keiji0604 0:0e3b9c903282 29
keiji0604 0:0e3b9c903282 30 bool flag_check = false;
keiji0604 0:0e3b9c903282 31 bool flag_printf = false;
keiji0604 0:0e3b9c903282 32
keiji0604 0:0e3b9c903282 33 int count_t = 0;
keiji0604 0:0e3b9c903282 34
keiji0604 0:0e3b9c903282 35 void func()
keiji0604 0:0e3b9c903282 36 {
keiji0604 0:0e3b9c903282 37 //goal = (int)filter.SecondOrderLag(3000);
keiji0604 0:0e3b9c903282 38 count_t++;
keiji0604 0:0e3b9c903282 39 count_r = motor.getPulses();
keiji0604 0:0e3b9c903282 40 error_PID = (double)(goal - count_r);
keiji0604 0:0e3b9c903282 41 integ += (double)(count_r + pre_count) / 2.0 * s_t;
keiji0604 0:0e3b9c903282 42 diff = (count_r - pre_count) / s_t;
keiji0604 0:0e3b9c903282 43 def = k_p * error_PID + k_i * integ + k_d * diff;
keiji0604 0:0e3b9c903282 44
keiji0604 0:0e3b9c903282 45 s_d = (int)def;
keiji0604 0:0e3b9c903282 46 if(s_d > 30) {
keiji0604 0:0e3b9c903282 47 s_d = 30;
keiji0604 0:0e3b9c903282 48 }
keiji0604 0:0e3b9c903282 49
keiji0604 0:0e3b9c903282 50 if(s_d < -30) {
keiji0604 0:0e3b9c903282 51 s_d = -30;
keiji0604 0:0e3b9c903282 52 }
keiji0604 0:0e3b9c903282 53
keiji0604 0:0e3b9c903282 54 flag_check = true;
keiji0604 0:0e3b9c903282 55
keiji0604 0:0e3b9c903282 56 if(count_t >= 10) flag_printf = true;
keiji0604 0:0e3b9c903282 57
keiji0604 0:0e3b9c903282 58 pre_count = count_r;
keiji0604 0:0e3b9c903282 59 }
keiji0604 0:0e3b9c903282 60
keiji0604 0:0e3b9c903282 61 int main()
keiji0604 0:0e3b9c903282 62 {
keiji0604 0:0e3b9c903282 63 filter.setSecondOrderPara(1.0, 1.0, 0.0);
keiji0604 0:0e3b9c903282 64 flipper.attach(&func, 0.01);
keiji0604 0:0e3b9c903282 65
keiji0604 0:0e3b9c903282 66 while(1) {
keiji0604 0:0e3b9c903282 67
keiji0604 0:0e3b9c903282 68 if (flag_check == true) {
keiji0604 0:0e3b9c903282 69 if (error_PID <= 0) {
keiji0604 0:0e3b9c903282 70 robo.ForwardM2(s_d);
keiji0604 0:0e3b9c903282 71 flag_check = false;
keiji0604 0:0e3b9c903282 72 } else {
keiji0604 0:0e3b9c903282 73 robo.BackwardM2(-1 * s_d);
keiji0604 0:0e3b9c903282 74 flag_check = false;
keiji0604 0:0e3b9c903282 75 }
keiji0604 0:0e3b9c903282 76 }
keiji0604 0:0e3b9c903282 77 if(flag_printf == true) {
keiji0604 0:0e3b9c903282 78 pc.printf("count_r : %d\t\terror : %d\t\ts_d : %d\t\tgoal : %d\n", count_r, error_PID, s_d, goal);
keiji0604 0:0e3b9c903282 79 flag_printf = false;
keiji0604 0:0e3b9c903282 80 count_r = 0;
keiji0604 0:0e3b9c903282 81 }
keiji0604 0:0e3b9c903282 82
keiji0604 0:0e3b9c903282 83 }
keiji0604 0:0e3b9c903282 84 }