Keiji Akasaki
私が書いた最新のマニュアルPID制御(PID位置制御)のプログラムです
Dependencies: mbed QEI2 RoboClaw Filter
Revision 0:0e3b9c903282, committed 2021-04-01
- Comitter:
- keiji0604
- Date:
- Thu Apr 01 05:57:40 2021 +0000
- Commit message:
- last
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Filter.lib Thu Apr 01 05:57:40 2021 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/keiji0604/code/Filter/#1c0c3d33900f
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/QEI2.lib Thu Apr 01 05:57:40 2021 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/kikoaac/code/QEI2/#49fa8827718d
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/RoboClaw.lib Thu Apr 01 05:57:40 2021 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/teams/ARES/code/RoboClaw/#f82bc24e3bd4
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Thu Apr 01 05:57:40 2021 +0000
@@ -0,0 +1,84 @@
+#include "mbed.h"
+#include "RoboClaw.h"
+#include "QEI.h"
+#include "Filter.h"
+
+Filter filter(0.01);
+Timer t;
+QEI motor(p21, p22, NC, 500, &t, QEI::X4_ENCODING);
+RoboClaw robo(128, 115200, p9, NC);
+Serial pc(USBTX, USBRX, 115200);
+Ticker flipper;
+
+int count_r;
+int pre_count = 0;
+int error_PID;
+int goal = 3000;
+
+double k_p = 0.00035;
+double k_i = 0.00075;
+double k_d = 0.0003;
+
+double pro;
+double integ;
+double diff;
+double s_t = 0.01;
+
+double def;
+int s_d = 0.0;
+
+bool flag_check = false;
+bool flag_printf = false;
+
+int count_t = 0;
+
+void func()
+{
+ //goal = (int)filter.SecondOrderLag(3000);
+ count_t++;
+ count_r = motor.getPulses();
+ error_PID = (double)(goal - count_r);
+ integ += (double)(count_r + pre_count) / 2.0 * s_t;
+ diff = (count_r - pre_count) / s_t;
+ def = k_p * error_PID + k_i * integ + k_d * diff;
+
+ s_d = (int)def;
+ if(s_d > 30) {
+ s_d = 30;
+ }
+
+ if(s_d < -30) {
+ s_d = -30;
+ }
+
+ flag_check = true;
+
+ if(count_t >= 10) flag_printf = true;
+
+ pre_count = count_r;
+}
+
+int main()
+{
+ filter.setSecondOrderPara(1.0, 1.0, 0.0);
+ flipper.attach(&func, 0.01);
+
+ while(1) {
+
+ if (flag_check == true) {
+ if (error_PID <= 0) {
+ robo.ForwardM2(s_d);
+ flag_check = false;
+ } else {
+ robo.BackwardM2(-1 * s_d);
+ flag_check = false;
+ }
+ }
+ if(flag_printf == true) {
+ pc.printf("count_r : %d\t\terror : %d\t\ts_d : %d\t\tgoal : %d\n", count_r, error_PID, s_d, goal);
+ flag_printf = false;
+ count_r = 0;
+ }
+
+ }
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Thu Apr 01 05:57:40 2021 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400 \ No newline at end of file