00001 #ifndef ETHERCAT_HARDWARE_MOTOR_MODEL_H 00002 #define ETHERCAT_HARDWARE_MOTOR_MODEL_H 00003 00004 #include <string> 00005 #include <vector> 00006 00007 #include <realtime_tools/realtime_publisher.h> 00008 #include <ethercat_hardware/MotorTraceSample.h> 00009 #include <ethercat_hardware/MotorTrace.h> 00010 #include <ethercat_hardware/ActuatorInfo.h> 00011 #include <ethercat_hardware/BoardInfo.h> 00012 00013 #include <diagnostic_updater/DiagnosticStatusWrapper.h> 00014 00015 #include <boost/utility.hpp> 00016 #include <boost/thread/mutex.hpp> 00017 00018 class MotorModel : private boost::noncopyable 00019 { 00020 public: 00021 MotorModel(unsigned trace_size); 00022 bool initialize(const ethercat_hardware::ActuatorInfo &actuator_info, 00023 const ethercat_hardware::BoardInfo &board_info); 00024 void flagPublish(const std::string &reason, int level, int delay); 00025 void checkPublish(); 00026 void diagnostics(diagnostic_updater::DiagnosticStatusWrapper &d); 00027 void sample(const ethercat_hardware::MotorTraceSample &s); 00028 bool verify(); 00029 void reset(); 00030 protected: 00031 unsigned trace_size_; 00032 unsigned trace_index_; /* index of most recent element in trace vector */ 00033 unsigned published_traces_; 00034 ethercat_hardware::ActuatorInfo actuator_info_; 00035 ethercat_hardware::BoardInfo board_info_; 00036 double backemf_constant_; 00037 bool previous_pwm_saturated_; 00038 std::vector<ethercat_hardware::MotorTraceSample> trace_buffer_; 00039 realtime_tools::RealtimePublisher<ethercat_hardware::MotorTrace> *publisher_; 00040 double current_error_limit_; 00041 int publish_delay_; 00042 int publish_level_; 00043 std::string publish_reason_; 00044 //bool new_max_current_error_; 00045 //bool new_max_voltage_error_; 00046 int diagnostics_level_; 00047 std::string diagnostics_reason_; 00048 00049 class SimpleFilter 00050 { 00051 public: 00052 SimpleFilter(); 00053 void sample(double value, double filter_coefficient); 00054 void reset(); 00055 double filter() const { return filtered_value_; } 00056 protected: 00057 double filtered_value_; 00058 }; 00059 00060 // Filters and keeps a record of max values 00061 class Filter : public SimpleFilter 00062 { 00063 public: 00064 Filter(double filter_coefficient); 00065 bool sample(double value); 00066 void reset(); 00067 double filter_max() const { return max_filtered_value_; } 00068 protected: 00069 double filter_coefficient_; 00070 double max_filtered_value_; 00071 }; 00072 00073 // Lock around values used for diagnostics, 00074 // filter updates and diagnostic publishing are called from same different threads 00075 boost::mutex diagnostics_mutex_; 00076 Filter motor_voltage_error_; 00077 Filter abs_motor_voltage_error_; 00078 Filter measured_voltage_error_; 00079 Filter abs_measured_voltage_error_; 00080 Filter current_error_; 00081 Filter abs_current_error_; 00082 SimpleFilter motor_resistance_; 00083 00084 // These filter is for indentifing source of errors not for calculations 00085 Filter abs_velocity_; 00086 Filter abs_measured_current_; 00087 Filter abs_board_voltage_; 00088 Filter abs_position_delta_; 00089 00090 }; 00091 00092 #endif /* ETHERCAT_HARDWARE_MOTOR_MODEL_H */