00001 #ifndef INCLUDE_EVAROBOT_CONTROLLER_H_ 00002 #define INCLUDE_EVAROBOT_CONTROLLER_H_ 00003 00004 #include "ros/ros.h" 00005 #include <realtime_tools/realtime_publisher.h> 00006 00007 #include "geometry_msgs/PointStamped.h" 00008 #include "geometry_msgs/Twist.h" 00009 00010 #include <diagnostic_updater/diagnostic_updater.h> 00011 #include <diagnostic_updater/publisher.h> 00012 00013 #include "std_srvs/Empty.h" 00014 #include "im_msgs/WheelVel.h" 00015 00016 #include <stdio.h> 00017 #include <stdlib.h> 00018 00019 #include <string> 00020 #include <sstream> 00021 00022 #include <ros/console.h> 00023 #include <ErrorCodes.h> 00024 00025 float g_f_left_desired = 0.0; 00026 float g_f_right_desired = 0.0; 00027 00028 float g_f_left_measured = 0.0; 00029 float g_f_right_measured = 0.0; 00030 00031 double g_d_wheel_separation; 00032 bool b_is_received_params = false; 00033 00034 bool b_reset_controller = false; 00035 00036 using namespace std; 00037 00038 double g_d_p_left; 00039 double g_d_i_left; 00040 double g_d_d_left; 00041 00042 double g_d_p_right; 00043 double g_d_i_right; 00044 double g_d_d_right; 00045 00046 double g_d_dt = 0; 00047 00048 class PIDController 00049 { 00050 public: 00051 PIDController(); 00052 PIDController(double d_proportional_constant, 00053 double d_integral_constant, 00054 double d_derivative_constant, 00055 double _d_max_vel, 00056 string _str_name); 00057 00058 void UpdateParams(double d_proportional_constant, 00059 double d_integral_constant, 00060 double d_derivative_constant); 00061 00062 void ProduceDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat); 00063 00064 ~PIDController(); 00065 00066 float RunController(float f_desired, float f_measured); 00067 void Reset(); 00068 00069 private: 00070 double d_integral_error; 00071 double d_derivative_error; 00072 double d_proportional_error; 00073 double d_pre_error; 00074 00075 double d_integral_constant; 00076 double d_derivative_constant; 00077 double d_proportional_constant; 00078 00079 string str_name; 00080 00081 double d_max_vel; 00082 00083 ros::Time read_time; 00084 ros::Duration dur_time; 00085 00086 }; 00087 00088 #endif