pid.cpp

00001 #include "pid.h"
00002 
00003 void PositionPid::setup(float Kp, float Ki, float Kd, float dt){
00004     kp = Kp;
00005     ki = Ki;
00006     kd = Kd;
00007     time = dt;
00008     frequency = 1/dt;
00009 }
00010 
00011 void PositionPid::calculate(float target, float nowValue){
00012     old = now;
00013     now = nowValue - target;
00014     p = now;
00015     i = i + (now + old) * 0.5f * time;
00016     d = (now - old) * frequency;
00017     result = kp*p + ki*i + kd*d;
00018     if (result > 1.0f) {
00019         result = 1.0f;
00020     } else if (result < -1.0f) {
00021         result = -1.0f;
00022     }
00023 }
00024 
00025 float PositionPid::duty(){
00026     return result;
00027 }
00028 
00029 void SpeedPid::setup(float Kp, float Ki, float Kd, float dt){
00030     kp = Kp;
00031     ki = Ki;
00032     kd = Kd;
00033     time = dt;
00034     frequency = 1/dt;
00035 }
00036 
00037 void SpeedPid::calculate(float target, float nowValue){
00038     e2 = e1;
00039     e1 = e;
00040     e = nowValue - target;
00041     p = e - e1;
00042     i = e*time;
00043     d = (e-2*e1+e2)*frequency;
00044     result = result + (kp*p+ki*i+kd*d);
00045     if (result > 1.0f) {
00046         result = 1.0f;
00047     } else if (result < -1.0f) {
00048         result = -1.0f;
00049     }
00050 }
00051 
00052 float SpeedPid::duty(){
00053     return result;
00054 }