kusano kiyoshige
ライブラリ化を行った後
Dependencies: QEI accelerator bit_test cyclic_io cyclic_var cylinder event_var limit mbed mecanum motor_drive pid pid_encoder rs422_put sbdbt servo
Fork of
17robo_Practice1
by 
kusano kiyoshige
Diff: pid.h
- Revision:
- 0:bf96e953cdb8
- Child:
- 7:c4ae1d001d09
- Child:
- 10:04f2a82cfd89
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/pid.h Mon Jun 26 09:59:14 2017 +0000
@@ -0,0 +1,64 @@
+class Position_pid {
+public :
+ void setup(float Kp, float Ki, float Kd) {
+ kp = Kp;
+ ki = Ki;
+ kd = Kd;
+ }
+
+ void cal(float target, float nowval, float dt) {
+ old = now;
+ now = nowval - target;
+ p = now;
+ i = i + (now + old)/2.0f * dt;
+ d = (now - old) / dt;
+ result = kp*p + ki*i + kd*d;
+ if (result > 1.0f) {
+ result = 1.0f;
+ } else if (result < -1.0f) {
+ result = -1.0f;
+ }
+ }
+
+ float duty() {
+ return result;
+ }
+
+private :
+ float kp, ki, kd,
+ old, now,
+ p, i, d, result;
+};
+
+class Speed_pid {
+public :
+ void setup(float Kp, float Ki, float Kd) {
+ kp = Kp;
+ ki = Ki;
+ kd = Kd;
+ }
+
+ void cal(float target, float nowval, float dt) {
+ e2 = e1;
+ e1 = e;
+ e = nowval - target;
+ p = e - e1;
+ i = e*dt;
+ d = (e-2*e1+e2)/dt;
+ result = result + (kp*p+ki*i+kd*d);
+ if (result > 1.0f) {
+ result = 1.0f;
+ } else if (result < -1.0f) {
+ result = -1.0f;
+ }
+ }
+
+ float duty() {
+ return result;
+ }
+
+private :
+ float kp, ki, kd,
+ e, e1, e2,
+ p, i, d, result;
+};
\ No newline at end of file