ケンタ ミヤザキ
Dependents: nhk_2018_undercarry_test04 nhk_2018_undercarry_test08 nhk_2018_undercarry_test09 nhk_2018_undercarry_test10 ... more
Diff: PID.cpp
- Revision:
- 0:d8b5fa590ca2
- Child:
- 1:90e1008a1bf9
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/PID.cpp Tue Jan 16 14:27:31 2018 +0000
@@ -0,0 +1,29 @@
+#include "PID.h"
+#include "mbed.h"
+
+PID::PID(double kp, double ki, double kd, double setpoint){
+ KP = kp;
+ KI = ki;
+ KD = kd;
+ Setpoint = setpoint;
+ init();
+}
+
+void PID::init(){
+ timer.reset();
+ timer.start();
+ preTime = 0;
+ P = 0;
+ I = 0;
+ D = 0;
+}
+
+double PID::compute(double input){
+ dt = (timer.read() - preTime) / 10000;
+ preTime = timer.read();
+ P = Setpoint - input;
+ I += P * dt;
+ D = (P - preP) / dt;
+ preP = P;
+ return (KP * P + KI * I + KD * D);
+}