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 */


ethercat_hardware
Author(s): Rob Wheeler , Derek King
autogenerated on Fri Aug 28 2015 12:09:44