Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035 #ifndef ETHERCAT_HARDWARE__MOTOR_HEATING_MODEL_H
00036 #define ETHERCAT_HARDWARE__MOTOR_HEATING_MODEL_H
00037
00038 #include "ethercat_hardware/MotorTemperature.h"
00039 #include "ethercat_hardware/MotorTraceSample.h"
00040 #include "ethercat_hardware/ActuatorInfo.h"
00041
00042 #include "realtime_tools/realtime_publisher.h"
00043 #include "diagnostic_updater/DiagnosticStatusWrapper.h"
00044
00045 #include <boost/utility.hpp>
00046 #include <boost/thread/mutex.hpp>
00047 #include <boost/shared_ptr.hpp>
00048
00049 #include <vector>
00050 #include <string>
00051
00052 namespace ethercat_hardware
00053 {
00054
00055
00061 struct MotorHeatingModelParameters
00062 {
00064 double housing_to_ambient_thermal_resistance_;
00066 double winding_to_housing_thermal_resistance_;
00068 double winding_thermal_time_constant_;
00070 double housing_thermal_time_constant_;
00072 double max_winding_temperature_;
00073 } __attribute__ ((__packed__));
00074
00075
00086 struct MotorHeatingModelParametersEepromConfig
00087 {
00088 uint16_t major_;
00089 uint16_t minor_;
00090 bool enforce_;
00091 uint8_t pad1[3];
00092 MotorHeatingModelParameters params_;
00093 uint8_t pad2[204];
00094 uint32_t crc32_;
00095
00096 static const unsigned EEPROM_PAGE = 4093;
00097
00098 bool verifyCRC(void) const;
00099 void generateCRC(void);
00100 } __attribute__ ((__packed__));
00101
00102
00103 class MotorHeatingModel;
00104
00105 class MotorHeatingModelCommon : private boost::noncopyable
00106 {
00107 public:
00110 MotorHeatingModelCommon(ros::NodeHandle nh);
00111
00114 MotorHeatingModelCommon();
00115
00116
00117 bool initialize();
00118
00121 void attach(boost::shared_ptr<MotorHeatingModel> model);
00122
00125 bool update_save_files_;
00127 std::string save_directory_;
00129 bool load_save_files_;
00131 bool disable_halt_;
00133 bool enable_model_;
00135 bool publish_temperature_;
00136
00137 protected:
00138 bool createSaveDirectory();
00139
00141 boost::thread save_thread_;
00142 void saveThreadFunc();
00143
00145 std::vector< boost::shared_ptr<MotorHeatingModel> > models_;
00146
00148 boost::mutex mutex_;
00149 };
00150
00151
00152
00153 class MotorHeatingModel : private boost::noncopyable
00154 {
00155 public:
00156
00157 MotorHeatingModel(const MotorHeatingModelParameters &motor_params,
00158 const std::string &actuator_name,
00159 const std::string &hwid,
00160 const std::string &save_directory
00161 );
00162
00163 bool startTemperaturePublisher();
00164
00175 bool update(double heating_power, double ambient_temperature, double duration);
00176 bool update(const ethercat_hardware::MotorTraceSample &sample,
00177 const ethercat_hardware::ActuatorInfo &actuator_info,
00178 double ambient_temperature, double duration)
00179 {
00180 double heating_power = calculateMotorHeatPower(sample, actuator_info);
00181 return update(heating_power, ambient_temperature, duration);
00182 }
00183
00185 bool hasOverheated() const {return overheat_;}
00187 double getWindingTemperature() {return winding_temperature_;}
00189 double getHousingTemperature() {return housing_temperature_;}
00190
00191
00193 void reset();
00194
00195
00207 void updateFromDowntime(double downtime, double saved_ambient_temperature);
00208
00209
00217 double updateFromDowntimeWithInterval(double downtime,
00218 double saved_ambient_temperature,
00219 double interval,
00220 unsigned cycles);
00221
00222
00225 double calculateMotorHeatPower(const ethercat_hardware::MotorTraceSample &sample,
00226 const ethercat_hardware::ActuatorInfo &actuator_info);
00227
00228
00230 bool loadTemperatureState();
00231
00232
00238 bool saveTemperatureState();
00239
00240
00242 void diagnostics(diagnostic_updater::DiagnosticStatusWrapper &d);
00243
00244
00245
00246
00247 protected:
00248
00249
00250
00251
00252
00253
00254
00255
00256
00257
00258
00259
00260
00262 double winding_to_housing_thermal_conductance_;
00264 double housing_to_ambient_thermal_conductance_;
00266 double winding_thermal_mass_inverse_;
00268 double housing_thermal_mass_inverse_;
00269
00270
00272 double winding_temperature_;
00274 double housing_temperature_;
00276 double ambient_temperature_;
00277
00279 boost::mutex mutex_;
00280
00282 bool overheat_;
00283
00285 double heating_energy_sum_;
00287 double ambient_temperature_sum_;
00289 double duration_since_last_sample_;
00291
00293 realtime_tools::RealtimePublisher<ethercat_hardware::MotorTemperature> *publisher_;
00294
00295 MotorHeatingModelParameters motor_params_;
00296 std::string actuator_name_;
00297 std::string save_filename_;
00298 std::string hwid_;
00299 };
00300
00301
00302 };
00303
00304 #endif //ETHERCAT_HARDWARE__MOTOR_HEATING_MODEL_H