motor.h
Go to the documentation of this file.
1 #ifndef CANOPEN_402_MOTOR_H
2 #define CANOPEN_402_MOTOR_H
3 
4 #include <canopen_402/base.h>
6 #include <boost/function.hpp>
7 #include <boost/container/flat_map.hpp>
8 
9 #include <boost/numeric/conversion/cast.hpp>
10 #include <limits>
11 #include <algorithm>
12 
13 namespace canopen
14 {
15 
16 class State402{
17 public:
19  {
36  };
38  {
39  Unknown = 0,
40  Start = 0,
48  Fault = 8,
49  };
51  InternalState read(uint16_t sw);
52  bool waitForNewState(const time_point &abstime, InternalState &state);
54 private:
55  boost::condition_variable cond_;
56  boost::mutex mutex_;
58 };
59 
60 class Command402 {
61  struct Op {
62  uint16_t to_set_;
63  uint16_t to_reset_;
64  Op(uint16_t to_set ,uint16_t to_reset) : to_set_(to_set), to_reset_(to_reset) {}
65  void operator()(uint16_t &val) const {
66  val = (val & ~to_reset_) | to_set_;
67  }
68  };
70  boost::container::flat_map< std::pair<State402::InternalState, State402::InternalState>, Op > transitions_;
71  void add(const State402::InternalState &from, const State402::InternalState &to, Op op){
72  transitions_.insert(std::make_pair(std::make_pair(from, to), op));
73  }
74  public:
76  const Op& get(const State402::InternalState &from, const State402::InternalState &to) const {
77  return transitions_.at(std::make_pair(from, to));
78  }
79  };
81  static State402::InternalState nextStateForEnabling(State402::InternalState state);
82  Command402();
83 public:
85  {
86  CW_Switch_On=0,
87  CW_Enable_Voltage=1,
88  CW_Quick_Stop=2,
89  CW_Enable_Operation=3,
90  CW_Operation_mode_specific0=4,
91  CW_Operation_mode_specific1=5,
92  CW_Operation_mode_specific2=6,
93  CW_Fault_Reset=7,
94  CW_Halt=8,
95  CW_Operation_mode_specific3=9,
96  // CW_Reserved1=10,
97  CW_Manufacturer_specific0=11,
98  CW_Manufacturer_specific1=12,
99  CW_Manufacturer_specific2=13,
100  CW_Manufacturer_specific3=14,
101  CW_Manufacturer_specific4=15,
102  };
103  static bool setTransition(uint16_t &cw, const State402::InternalState &from, const State402::InternalState &to, State402::InternalState *next);
104 };
105 
106 template<uint16_t MASK> class WordAccessor{
107  uint16_t &word_;
108 public:
109  WordAccessor(uint16_t &word) : word_(word) {}
110  bool set(uint8_t bit){
111  uint16_t val = MASK & (1<<bit);
112  word_ |= val;
113  return val;
114  }
115  bool reset(uint8_t bit){
116  uint16_t val = MASK & (1<<bit);
117  word_ &= ~val;
118  return val;
119  }
120  bool get(uint8_t bit) const { return word_ & (1<<bit); }
121  uint16_t get() const { return word_ & MASK; }
122  WordAccessor & operator=(const uint16_t &val){
123  uint16_t was = word_;
124  word_ = (word_ & ~MASK) | (val & MASK);
125  return *this;
126  }
127 };
128 
129 class Mode {
130 public:
131  const uint16_t mode_id_;
132  Mode(uint16_t id) : mode_id_(id) {}
134  virtual bool start() = 0;
135  virtual bool read(const uint16_t &sw) = 0;
136  virtual bool write(OpModeAccesser& cw) = 0;
137  virtual bool setTarget(const double &val) { LOG("not implemented"); return false; }
138  virtual ~Mode() {}
139 };
140 typedef boost::shared_ptr<Mode> ModeSharedPtr;
141 
142 template<typename T> class ModeTargetHelper : public Mode {
144  boost::atomic<bool> has_target_;
145 
146 public:
147  ModeTargetHelper(uint16_t mode) : Mode (mode) {}
148  bool hasTarget() { return has_target_; }
149  T getTarget() { return target_; }
150  virtual bool setTarget(const double &val) {
151  if(boost::math::isnan(val)){
152  LOG("target command is not a number");
153  return false;
154  }
155 
156  using boost::numeric_cast;
157  using boost::numeric::positive_overflow;
158  using boost::numeric::negative_overflow;
159 
160  try
161  {
162  target_= numeric_cast<T>(val);
163  }
164  catch(negative_overflow&) {
165  LOG("Command " << val << " does not fit into target, clamping to min limit");
166  target_= std::numeric_limits<T>::min();
167  }
168  catch(positive_overflow&) {
169  LOG("Command " << val << " does not fit into target, clamping to max limit");
170  target_= std::numeric_limits<T>::max();
171  }
172  catch(...){
173  LOG("Was not able to cast command " << val);
174  return false;
175  }
176 
177  has_target_ = true;
178  return true;
179  }
180  virtual bool start() { has_target_ = false; return true; }
181 };
182 
183 template<uint16_t ID, typename TYPE, uint16_t OBJ, uint8_t SUB, uint16_t CW_MASK> class ModeForwardHelper : public ModeTargetHelper<TYPE> {
185 public:
187  if(SUB) storage->entry(target_entry_, OBJ, SUB);
188  else storage->entry(target_entry_, OBJ);
189  }
190  virtual bool read(const uint16_t &sw) { return true;}
191  virtual bool write(Mode::OpModeAccesser& cw) {
192  if(this->hasTarget()){
193  cw = cw.get() | CW_MASK;
194  target_entry_.set(this->getTarget());
195  return true;
196  }else{
197  cw = cw.get() & ~CW_MASK;
198  return false;
199  }
200  }
201 };
202 
210 
211 class ProfiledPositionMode : public ModeTargetHelper<int32_t> {
213  double last_target_;
214  uint16_t sw_;
215 public:
216  enum SW_masks {
217  MASK_Reached = (1<<State402::SW_Target_reached),
218  MASK_Acknowledged = (1<<State402::SW_Operation_mode_specific0),
220  };
221  enum CW_bits {
225  };
227  storage->entry(target_position_, 0x607A);
228  }
229  virtual bool start() { sw_ = 0; last_target_= std::numeric_limits<double>::quiet_NaN(); return ModeTargetHelper::start(); }
230  virtual bool read(const uint16_t &sw) { sw_ = sw; return (sw & MASK_Error) == 0; }
231  virtual bool write(OpModeAccesser& cw) {
232  cw.set(CW_Immediate);
233  if(hasTarget()){
234  int32_t target = getTarget();
235  if((sw_ & MASK_Acknowledged) == 0 && target != last_target_){
236  if(cw.get(CW_NewPoint)){
237  cw.reset(CW_NewPoint); // reset if needed
238  }else{
239  target_position_.set(target);
240  cw.set(CW_NewPoint);
241  last_target_ = target;
242  }
243  } else if (sw_ & MASK_Acknowledged){
244  cw.reset(CW_NewPoint);
245  }
246  return true;
247  }
248  return false;
249  }
250 };
251 
252 class HomingMode: public Mode{
253 protected:
254  enum SW_bits {
257  };
258  enum CW_bits {
260  };
261 public:
262  HomingMode() : Mode(MotorBase::Homing) {}
263  virtual bool executeHoming(canopen::LayerStatus &status) = 0;
264 };
265 
268  boost::atomic<bool> execute_;
269 
270  boost::mutex mutex_;
271  boost::condition_variable cond_;
272  uint16_t status_;
273 
274  enum SW_masks {
275  MASK_Reached = (1<<State402::SW_Target_reached),
276  MASK_Attained = (1<<SW_Attained),
277  MASK_Error = (1<<SW_Error),
278  };
279  bool error(canopen::LayerStatus &status, const std::string& msg) { execute_= false; status.error(msg); return false; }
280 public:
282  storage->entry(homing_method_, 0x6098);
283  }
284  virtual bool start();
285  virtual bool read(const uint16_t &sw);
286  virtual bool write(OpModeAccesser& cw);
287 
288  virtual bool executeHoming(canopen::LayerStatus &status);
289 };
290 
291 class Motor402 : public MotorBase
292 {
293 public:
294 
295  Motor402(const std::string &name, ObjectStorageSharedPtr storage, const canopen::Settings &settings)
296  : MotorBase(name), status_word_(0),control_word_(0),
297  switching_state_(State402::InternalState(settings.get_optional<unsigned int>("switching_state", static_cast<unsigned int>(State402::Operation_Enable)))),
298  monitor_mode_(settings.get_optional<bool>("monitor_mode", true)),
299  state_switch_timeout_(settings.get_optional<unsigned int>("state_switch_timeout", 5))
300  {
301  storage->entry(status_word_entry_, 0x6041);
302  storage->entry(control_word_entry_, 0x6040);
303  storage->entry(op_mode_display_, 0x6061);
304  storage->entry(op_mode_, 0x6060);
305  try{
306  storage->entry(supported_drive_modes_, 0x6502);
307  }
308  catch(...){
309  }
310  }
311 
312  virtual bool setTarget(double val);
313  virtual bool enterModeAndWait(uint16_t mode);
314  virtual bool isModeSupported(uint16_t mode);
315  virtual uint16_t getMode();
316 
317  template<typename T> bool registerMode(uint16_t mode) {
318  return mode_allocators_.insert(std::make_pair(mode, boost::bind(&Motor402::createAndRegister<T>, this, mode))).second;
319  }
320  template<typename T, typename T1> bool registerMode(uint16_t mode, const T1& t1) {
321  return mode_allocators_.insert(std::make_pair(mode, boost::bind(&Motor402::createAndRegister<T,T1>, this, mode, t1))).second;
322  }
323  template<typename T, typename T1, typename T2> bool registerMode(uint16_t mode, const T1& t1, const T2& t2) {
324  return mode_allocators_.insert(std::make_pair(mode, boost::bind(&Motor402::createAndRegister<T,T1,T2>, this, mode, t1, t2))).second;
325  }
326 
328  registerMode<ProfiledPositionMode> (MotorBase::Profiled_Position, storage);
329  registerMode<VelocityMode> (MotorBase::Velocity, storage);
330  registerMode<ProfiledVelocityMode> (MotorBase::Profiled_Velocity, storage);
331  registerMode<ProfiledTorqueMode> (MotorBase::Profiled_Torque, storage);
332  registerMode<DefaultHomingMode> (MotorBase::Homing, storage);
333  registerMode<InterpolatedPositionMode> (MotorBase::Interpolated_Position, storage);
334  registerMode<CyclicSynchronousPositionMode> (MotorBase::Cyclic_Synchronous_Position, storage);
335  registerMode<CyclicSynchronousVelocityMode> (MotorBase::Cyclic_Synchronous_Velocity, storage);
336  registerMode<CyclicSynchronousTorqueMode> (MotorBase::Cyclic_Synchronous_Torque, storage);
337  }
338 
340  public:
341  virtual MotorBaseSharedPtr allocate(const std::string &name, ObjectStorageSharedPtr storage, const canopen::Settings &settings);
342  };
343 protected:
344  virtual void handleRead(LayerStatus &status, const LayerState &current_state);
345  virtual void handleWrite(LayerStatus &status, const LayerState &current_state);
346  virtual void handleDiag(LayerReport &report);
347  virtual void handleInit(LayerStatus &status);
348  virtual void handleShutdown(LayerStatus &status);
349  virtual void handleHalt(LayerStatus &status);
350  virtual void handleRecover(LayerStatus &status);
351 
352 private:
353  template<typename T> void createAndRegister0(uint16_t mode){
354  if(isModeSupportedByDevice(mode)) registerMode(mode, ModeSharedPtr(new T()));
355  }
356  template<typename T, typename T1> void createAndRegister(uint16_t mode, const T1& t1){
357  if(isModeSupportedByDevice(mode)) registerMode(mode, ModeSharedPtr(new T(t1)));
358  }
359  template<typename T, typename T1, typename T2> void createAndRegister(uint16_t mode, const T1& t1, const T2& t2){
360  if(isModeSupportedByDevice(mode)) registerMode(mode, ModeSharedPtr(new T(t1,t2)));
361  }
362 
363  virtual bool isModeSupportedByDevice(uint16_t mode);
364  void registerMode(uint16_t id, const ModeSharedPtr &m);
365 
366  ModeSharedPtr allocMode(uint16_t mode);
367 
368  bool readState(LayerStatus &status, const LayerState &current_state);
369  bool switchMode(LayerStatus &status, uint16_t mode);
370  bool switchState(LayerStatus &status, const State402::InternalState &target);
371 
372  boost::atomic<uint16_t> status_word_;
373  uint16_t control_word_;
374  boost::mutex cw_mutex_;
375  boost::atomic<bool> start_fault_reset_;
376  boost::atomic<State402::InternalState> target_state_;
377 
378 
380 
381  boost::mutex map_mutex_;
382  boost::unordered_map<uint16_t, ModeSharedPtr > modes_;
383  typedef boost::function<void()> AllocFuncType;
384  boost::unordered_map<uint16_t, AllocFuncType> mode_allocators_;
385 
386  ModeSharedPtr selected_mode_;
387  uint16_t mode_id_;
388  boost::condition_variable mode_cond_;
389  boost::mutex mode_mutex_;
391  const bool monitor_mode_;
392  const boost::chrono::seconds state_switch_timeout_;
393 
399 };
400 
401 }
402 
403 #endif
canopen::ObjectStorage::Entry< TYPE > target_entry_
Definition: motor.h:184
virtual void registerDefaultModes(ObjectStorageSharedPtr storage)
Definition: motor.h:327
ProfiledPositionMode(ObjectStorageSharedPtr storage)
Definition: motor.h:226
ModeForwardHelper< MotorBase::Velocity, int16_t, 0x6042, 0,(1<< Command402::CW_Operation_mode_specific0)|(1<< Command402::CW_Operation_mode_specific1)|(1<< Command402::CW_Operation_mode_specific2)> VelocityMode
Definition: motor.h:208
boost::atomic< State402::InternalState > target_state_
Definition: motor.h:376
InternalState state_
Definition: motor.h:57
boost::atomic< bool > start_fault_reset_
Definition: motor.h:375
bool registerMode(uint16_t mode, const T1 &t1, const T2 &t2)
Definition: motor.h:323
bool get(uint8_t bit) const
Definition: motor.h:120
boost::atomic< uint16_t > status_word_
Definition: motor.h:372
canopen::ObjectStorage::Entry< uint32_t > supported_drive_modes_
Definition: motor.h:398
ModeTargetHelper(uint16_t mode)
Definition: motor.h:147
static const TransitionTable transitions_
Definition: motor.h:80
virtual bool start()
Definition: motor.h:229
const uint16_t mode_id_
Definition: motor.h:131
bool set(uint8_t bit)
Definition: motor.h:110
ModeForwardHelper< MotorBase::Profiled_Torque, int16_t, 0x6071, 0, 0 > ProfiledTorqueMode
Definition: motor.h:204
boost::shared_ptr< MotorBase > MotorBaseSharedPtr
Definition: base.h:33
ModeForwardHelper< MotorBase::Cyclic_Synchronous_Position, int32_t, 0x607A, 0, 0 > CyclicSynchronousPositionMode
Definition: motor.h:205
boost::mutex mutex_
Definition: motor.h:56
canopen::ObjectStorage::Entry< int32_t > target_position_
Definition: motor.h:212
boost::unordered_map< uint16_t, AllocFuncType > mode_allocators_
Definition: motor.h:384
virtual bool write(OpModeAccesser &cw)
Definition: motor.h:231
WordAccessor & operator=(const uint16_t &val)
Definition: motor.h:122
ModeSharedPtr selected_mode_
Definition: motor.h:386
boost::condition_variable cond_
Definition: motor.h:55
virtual bool read(const uint16_t &sw)
Definition: motor.h:190
State402 state_handler_
Definition: motor.h:379
bool error(canopen::LayerStatus &status, const std::string &msg)
Definition: motor.h:279
ModeForwardHelper< MotorBase::Profiled_Velocity, int32_t, 0x60FF, 0, 0 > ProfiledVelocityMode
Definition: motor.h:203
uint16_t & word_
Definition: motor.h:107
void createAndRegister0(uint16_t mode)
Definition: motor.h:353
boost::condition_variable cond_
Definition: motor.h:271
void createAndRegister(uint16_t mode, const T1 &t1, const T2 &t2)
Definition: motor.h:359
boost::mutex cw_mutex_
Definition: motor.h:374
boost::chrono::high_resolution_clock::time_point time_point
canopen::ObjectStorage::Entry< uint16_t > status_word_entry_
Definition: motor.h:394
ModeForwardHelper(ObjectStorageSharedPtr storage)
Definition: motor.h:186
bool waitForNewState(const time_point &abstime, InternalState &state)
Definition: motor.cpp:72
boost::mutex mutex_
Definition: motor.h:270
uint16_t to_set_
Definition: motor.h:62
boost::function< void()> AllocFuncType
Definition: motor.h:383
virtual bool read(const uint16_t &sw)
Definition: motor.h:230
const boost::chrono::seconds state_switch_timeout_
Definition: motor.h:392
uint16_t control_word_
Definition: motor.h:373
canopen::ObjectStorage::Entry< int8_t > op_mode_
Definition: motor.h:397
bool registerMode(uint16_t mode)
Definition: motor.h:317
#define LOG(log)
uint16_t mode_id_
Definition: motor.h:387
canopen::ObjectStorage::Entry< uint16_t > control_word_entry_
Definition: motor.h:395
WordAccessor<(1<< Command402::CW_Operation_mode_specific0)|(1<< Command402::CW_Operation_mode_specific1)|(1<< Command402::CW_Operation_mode_specific2)|(1<< Command402::CW_Operation_mode_specific3)> OpModeAccesser
Definition: motor.h:133
void operator()(uint16_t &val) const
Definition: motor.h:65
boost::mutex map_mutex_
Definition: motor.h:381
ModeForwardHelper< MotorBase::Cyclic_Synchronous_Torque, int16_t, 0x6071, 0, 0 > CyclicSynchronousTorqueMode
Definition: motor.h:207
DefaultHomingMode(ObjectStorageSharedPtr storage)
Definition: motor.h:281
boost::condition_variable mode_cond_
Definition: motor.h:388
Mode(uint16_t id)
Definition: motor.h:132
ModeForwardHelper< MotorBase::Interpolated_Position, int32_t, 0x60C1, 0x01,(1<< Command402::CW_Operation_mode_specific0)> InterpolatedPositionMode
Definition: motor.h:209
canopen::ObjectStorage::Entry< int8_t > op_mode_display_
Definition: motor.h:396
bool reset(uint8_t bit)
Definition: motor.h:115
bool registerMode(uint16_t mode, const T1 &t1)
Definition: motor.h:320
WordAccessor(uint16_t &word)
Definition: motor.h:109
Op(uint16_t to_set, uint16_t to_reset)
Definition: motor.h:64
virtual ~Mode()
Definition: motor.h:138
canopen::ObjectStorage::Entry< int8_t > homing_method_
Definition: motor.h:267
InternalState read(uint16_t sw)
Definition: motor.cpp:12
Motor402(const std::string &name, ObjectStorageSharedPtr storage, const canopen::Settings &settings)
Definition: motor.h:295
void add(const State402::InternalState &from, const State402::InternalState &to, Op op)
Definition: motor.h:71
InternalState getState()
Definition: motor.cpp:7
boost::container::flat_map< std::pair< State402::InternalState, State402::InternalState >, Op > transitions_
Definition: motor.h:70
uint16_t to_reset_
Definition: motor.h:63
boost::atomic< bool > has_target_
Definition: motor.h:144
virtual bool start()
Definition: motor.h:180
virtual bool write(Mode::OpModeAccesser &cw)
Definition: motor.h:191
const void error(const std::string &r)
boost::mutex mode_mutex_
Definition: motor.h:389
void createAndRegister(uint16_t mode, const T1 &t1)
Definition: motor.h:356
boost::shared_ptr< Mode > ModeSharedPtr
Definition: motor.h:140
ModeForwardHelper< MotorBase::Cyclic_Synchronous_Velocity, int32_t, 0x60FF, 0, 0 > CyclicSynchronousVelocityMode
Definition: motor.h:206
virtual bool setTarget(const double &val)
Definition: motor.h:150
ObjectStorage::ObjectStorageSharedPtr ObjectStorageSharedPtr
const bool monitor_mode_
Definition: motor.h:391
boost::atomic< bool > execute_
Definition: motor.h:268
const State402::InternalState switching_state_
Definition: motor.h:390
boost::unordered_map< uint16_t, ModeSharedPtr > modes_
Definition: motor.h:382
virtual bool setTarget(const double &val)
Definition: motor.h:137


canopen_402
Author(s): Thiago de Freitas , Mathias L├╝dtke
autogenerated on Sat May 4 2019 02:40:44