pid.hpp
Go to the documentation of this file.
00001 #pragma once
00002 
00003 #include <ros/ros.h>
00004 
00005 class PID
00006 {
00007 public:
00008     PID(
00009         float kp,
00010         float kd,
00011         float ki,
00012         float minOutput,
00013         float maxOutput,
00014         float integratorMin,
00015         float integratorMax,
00016         const std::string& name)
00017         : m_kp(kp)
00018         , m_kd(kd)
00019         , m_ki(ki)
00020         , m_minOutput(minOutput)
00021         , m_maxOutput(maxOutput)
00022         , m_integratorMin(integratorMin)
00023         , m_integratorMax(integratorMax)
00024         , m_integral(0)
00025         , m_previousError(0)
00026         , m_previousTime(ros::Time::now())
00027     {
00028     }
00029 
00030     void reset()
00031     {
00032         m_integral = 0;
00033         m_previousError = 0;
00034         m_previousTime = ros::Time::now();
00035     }
00036 
00037     void setIntegral(float integral)
00038     {
00039         m_integral = integral;
00040     }
00041 
00042     float ki() const
00043     {
00044         return m_ki;
00045     }
00046 
00047     float update(float value, float targetValue)
00048     {
00049         ros::Time time = ros::Time::now();
00050         float dt = time.toSec() - m_previousTime.toSec();
00051         float error = targetValue - value;
00052         m_integral += error * dt;
00053         m_integral = std::max(std::min(m_integral, m_integratorMax), m_integratorMin);
00054         float p = m_kp * error;
00055         float d = 0;
00056         if (dt > 0)
00057         {
00058             d = m_kd * (error - m_previousError) / dt;
00059         }
00060         float i = m_ki * m_integral;
00061         float output = p + d + i;
00062         m_previousError = error;
00063         m_previousTime = time;
00064         // self.pubOutput.publish(output)
00065         // self.pubError.publish(error)
00066         // self.pubP.publish(p)
00067         // self.pubD.publish(d)
00068         // self.pubI.publish(i)
00069         return std::max(std::min(output, m_maxOutput), m_minOutput);
00070     }
00071 
00072 private:
00073     float m_kp;
00074     float m_kd;
00075     float m_ki;
00076     float m_minOutput;
00077     float m_maxOutput;
00078     float m_integratorMin;
00079     float m_integratorMax;
00080     float m_integral;
00081     float m_previousError;
00082     ros::Time m_previousTime;
00083 };


crazyflie_controller
Author(s): Wolfgang Hoenig
autogenerated on Wed Jun 12 2019 19:20:42