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