Wataru Shibazaki
赤崎PID プログラム
Revision 0:52ac0ff7ac4d, committed 2020-11-27
- Comitter:
- shibazakiwataru
- Date:
- Fri Nov 27 07:38:19 2020 +0000
- Commit message:
- AKSK
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/QEI2.lib Fri Nov 27 07:38:19 2020 +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/main.cpp Fri Nov 27 07:38:19 2020 +0000
@@ -0,0 +1,80 @@
+#include "mbed.h"
+#include "QEI.h"
+
+#define Limit 100
+#define PI 3.14159265359
+
+int count_r;
+float rev;
+float rev_back = 0.0;
+float an_vel;
+float powe_element = 0.0;
+float erorr;
+float erorr_back = 0.0;
+float goal = 2.0 * PI;
+float k_p = 20.0;
+float k_i = 3.0;
+float k_d = 2.5;
+float integ;
+float deriva;
+int power;
+
+Ticker warikomi;
+
+int dir;
+//int vel = 130;
+
+Serial pc(USBTX, USBRX, 115200);
+RawSerial saber(p13,p14,115200);
+Timer t;
+QEI motor(p30, p29, NC, 100, &t, QEI::X4_ENCODING);
+
+
+void func()
+{
+ count_r = motor.getPulses();
+ rev = (count_r / 400.0) * 2.0* PI;
+ an_vel = (rev - rev_back) / 0.01f;
+ rev_back = rev;
+
+ //////////////////////////////////
+
+ erorr = goal - rev;
+ integ += (erorr + erorr_back) * (1/2) * 0.01f;
+ deriva = (erorr - erorr_back) / 0.01f;
+ powe_element = k_p * erorr + k_i * integ + k_d * deriva;
+ erorr_back = erorr;
+
+ /////////////////////////////////////////////////////////
+
+ if (31.0 >= powe_element && powe_element >= 0.0) {
+ dir = 0;
+ } else if (powe_element < 0.0) {
+ dir = 1;
+ }
+
+ power = powe_element / 31.0f * Limit;
+ power = (int)power;
+
+ if (power > 100){
+ power = 100;
+ }
+
+ /////////////////////////////////////
+
+ saber.putc(130);//アドレス
+ saber.putc(dir);//回転方向
+ saber.putc(abs(power));//出力0~127
+ saber.putc((abs(power) + dir + 130) &0b1111111);
+}
+
+int main()
+{
+ warikomi.attach(&func, 0.01);
+ saber.baud(115200);
+ while(1) {
+ pc.printf("count: %d count(float): %f rev: %f an_vel: %f powe_element: %f power: %d dir: %d erorr: %f\n"
+ , count_r, (float)count_r, rev, an_vel, powe_element, power, dir, erorr);
+ wait(0.01);
+ }
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Fri Nov 27 07:38:19 2020 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400 \ No newline at end of file