motor.h
Go to the documentation of this file.
00001 #ifndef CANOPEN_402_MOTOR_H
00002 #define CANOPEN_402_MOTOR_H
00003 
00004 #include <canopen_402/base.h>
00005 #include <canopen_master/canopen.h>
00006 #include <boost/function.hpp>
00007 #include <boost/container/flat_map.hpp>
00008 
00009 #include <boost/numeric/conversion/cast.hpp>
00010 #include <limits>
00011 #include <algorithm>
00012 
00013 namespace canopen
00014 {
00015 
00016 class State402{
00017 public:
00018     enum StatusWord
00019     {
00020         SW_Ready_To_Switch_On=0,
00021         SW_Switched_On=1,
00022         SW_Operation_enabled=2,
00023         SW_Fault=3,
00024         SW_Voltage_enabled=4,
00025         SW_Quick_stop=5,
00026         SW_Switch_on_disabled=6,
00027         SW_Warning=7,
00028         SW_Manufacturer_specific0=8,
00029         SW_Remote=9,
00030         SW_Target_reached=10,
00031         SW_Internal_limit=11,
00032         SW_Operation_mode_specific0=12,
00033         SW_Operation_mode_specific1=13,
00034         SW_Manufacturer_specific1=14,
00035         SW_Manufacturer_specific2=15
00036     };
00037     enum InternalState
00038     {
00039         Unknown = 0,
00040         Start = 0,
00041         Not_Ready_To_Switch_On = 1,
00042         Switch_On_Disabled = 2,
00043         Ready_To_Switch_On = 3,
00044         Switched_On = 4,
00045         Operation_Enable = 5,
00046         Quick_Stop_Active = 6,
00047         Fault_Reaction_Active = 7,
00048         Fault = 8,
00049     };
00050     InternalState getState();
00051     InternalState read(uint16_t sw);
00052     bool waitForNewState(const time_point &abstime, InternalState &state);
00053     State402() : state_(Unknown) {}
00054 private:
00055     boost::condition_variable cond_;
00056     boost::mutex mutex_;
00057     InternalState state_;
00058 };
00059 
00060 class Command402 {
00061     struct Op {
00062         uint16_t to_set_;
00063         uint16_t to_reset_;
00064         Op(uint16_t to_set ,uint16_t to_reset) : to_set_(to_set), to_reset_(to_reset) {}
00065         void operator()(uint16_t &val) const {
00066             val = (val & ~to_reset_) | to_set_;
00067         }
00068     };
00069     class TransitionTable {
00070         boost::container::flat_map< std::pair<State402::InternalState, State402::InternalState>, Op > transitions_;
00071         void add(const State402::InternalState &from, const State402::InternalState &to, Op op){
00072             transitions_.insert(std::make_pair(std::make_pair(from, to), op));
00073         }
00074     public:
00075         TransitionTable();
00076         const Op& get(const State402::InternalState &from, const State402::InternalState &to) const {
00077             return transitions_.at(std::make_pair(from, to));
00078         }
00079     };
00080     static const TransitionTable transitions_;
00081     static State402::InternalState nextStateForEnabling(State402::InternalState state);
00082     Command402();
00083 public:
00084     enum ControlWord
00085     {
00086         CW_Switch_On=0,
00087         CW_Enable_Voltage=1,
00088         CW_Quick_Stop=2,
00089         CW_Enable_Operation=3,
00090         CW_Operation_mode_specific0=4,
00091         CW_Operation_mode_specific1=5,
00092         CW_Operation_mode_specific2=6,
00093         CW_Fault_Reset=7,
00094         CW_Halt=8,
00095         CW_Operation_mode_specific3=9,
00096         // CW_Reserved1=10,
00097         CW_Manufacturer_specific0=11,
00098         CW_Manufacturer_specific1=12,
00099         CW_Manufacturer_specific2=13,
00100         CW_Manufacturer_specific3=14,
00101         CW_Manufacturer_specific4=15,
00102     };
00103     static bool setTransition(uint16_t &cw, const State402::InternalState &from, const State402::InternalState &to, State402::InternalState *next);
00104 };
00105 
00106 template<uint16_t MASK> class WordAccessor{
00107     uint16_t &word_;
00108 public:
00109     WordAccessor(uint16_t &word) : word_(word) {}
00110     bool set(uint8_t bit){
00111         uint16_t val = MASK & (1<<bit);
00112         word_ |= val;
00113         return val;
00114    }
00115     bool reset(uint8_t bit){
00116         uint16_t val = MASK & (1<<bit);
00117         word_ &= ~val;
00118         return val;
00119     }
00120     bool get(uint8_t bit) const { return word_ & (1<<bit); }
00121     uint16_t get() const { return word_ & MASK; }
00122     WordAccessor & operator=(const uint16_t &val){
00123         uint16_t was = word_;
00124         word_ = (word_ & ~MASK) | (val & MASK);
00125         return *this;
00126     }
00127 };
00128 
00129 class Mode {
00130 public:
00131     const uint16_t mode_id_;
00132     Mode(uint16_t id) : mode_id_(id) {}
00133     typedef 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;
00134     virtual bool start() = 0;
00135     virtual bool read(const uint16_t &sw) = 0;
00136     virtual bool write(OpModeAccesser& cw) = 0;
00137     virtual bool setTarget(const double &val) { LOG("not implemented"); return false; }
00138     virtual ~Mode() {}
00139 };
00140 
00141 
00142 template<typename T> class ModeTargetHelper : public Mode {
00143     T target_;
00144     boost::atomic<bool> has_target_;
00145 
00146 public:
00147     ModeTargetHelper(uint16_t mode) : Mode (mode) {}
00148     bool hasTarget() { return has_target_; }
00149     T getTarget() { return target_; }
00150     virtual bool setTarget(const double &val) {
00151         if(isnan(val)){
00152             LOG("target command is not a number");
00153             return false;
00154         }
00155 
00156         using boost::numeric_cast;
00157         using boost::numeric::positive_overflow;
00158         using boost::numeric::negative_overflow;
00159 
00160         try
00161         {
00162             target_= numeric_cast<T>(val);
00163         }
00164         catch(negative_overflow&) {
00165             LOG("Command " << val << " does not fit into target, clamping to min limit");
00166             target_= std::numeric_limits<T>::min();
00167         }
00168         catch(positive_overflow&) {
00169             LOG("Command " << val << " does not fit into target, clamping to max limit");
00170             target_= std::numeric_limits<T>::max();
00171         }
00172         catch(...){
00173             LOG("Was not able to cast command " << val);
00174             return false;
00175         }
00176 
00177         has_target_ = true;
00178         return true;
00179     }
00180     virtual bool start() { has_target_ = false; return true; }
00181 };
00182 
00183 template<uint16_t ID, typename TYPE, uint16_t OBJ, uint8_t SUB, uint16_t CW_MASK> class ModeForwardHelper : public ModeTargetHelper<TYPE> {
00184     canopen::ObjectStorage::Entry<TYPE> target_entry_;
00185 public:
00186     ModeForwardHelper(boost::shared_ptr<ObjectStorage> storage) : ModeTargetHelper<TYPE>(ID) {
00187         if(SUB) storage->entry(target_entry_, OBJ, SUB);
00188         else storage->entry(target_entry_, OBJ);
00189     }
00190     virtual bool read(const uint16_t &sw) { return true;}
00191     virtual bool write(Mode::OpModeAccesser& cw) {
00192         if(this->hasTarget()){
00193             cw = cw.get() | CW_MASK;
00194             target_entry_.set(this->getTarget());
00195             return true;
00196         }else{
00197             cw = cw.get() & ~CW_MASK;
00198             return false;
00199         }
00200     }
00201 };
00202 
00203 typedef ModeForwardHelper<MotorBase::Profiled_Velocity, int32_t, 0x60FF, 0, 0> ProfiledVelocityMode;
00204 typedef ModeForwardHelper<MotorBase::Profiled_Torque, int16_t, 0x6071, 0, 0> ProfiledTorqueMode;
00205 typedef ModeForwardHelper<MotorBase::Cyclic_Synchronous_Position, int32_t, 0x607A, 0, 0> CyclicSynchronousPositionMode;
00206 typedef ModeForwardHelper<MotorBase::Cyclic_Synchronous_Velocity, int32_t, 0x60FF, 0, 0> CyclicSynchronousVelocityMode;
00207 typedef ModeForwardHelper<MotorBase::Cyclic_Synchronous_Torque, int16_t, 0x6071, 0, 0> CyclicSynchronousTorqueMode;
00208 typedef 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;
00209 typedef ModeForwardHelper<MotorBase::Interpolated_Position, int32_t, 0x60C1, 0x01, (1<<Command402::CW_Operation_mode_specific0)> InterpolatedPositionMode;
00210 
00211 class ProfiledPositionMode : public ModeTargetHelper<int32_t> {
00212     canopen::ObjectStorage::Entry<int32_t> target_position_;
00213     int32_t last_target_;
00214     uint16_t sw_;
00215 public:
00216     enum SW_masks {
00217         MASK_Reached = (1<<State402::SW_Target_reached),
00218         MASK_Acknowledged = (1<<State402::SW_Operation_mode_specific0),
00219         MASK_Error = (1<<State402::SW_Operation_mode_specific1),
00220     };
00221     enum CW_bits {
00222         CW_NewPoint =  Command402::CW_Operation_mode_specific0,
00223         CW_Immediate =  Command402::CW_Operation_mode_specific1,
00224         CW_Blending =  Command402::CW_Operation_mode_specific3,
00225     };
00226     ProfiledPositionMode(boost::shared_ptr<ObjectStorage> storage) : ModeTargetHelper(MotorBase::Profiled_Position) {
00227         storage->entry(target_position_, 0x607A);
00228     }
00229     virtual bool start() { sw_ = 0; last_target_= std::numeric_limits<double>::quiet_NaN(); return ModeTargetHelper::start(); }
00230     virtual bool read(const uint16_t &sw) { sw_ = sw; return (sw & MASK_Error) == 0; }
00231     virtual bool write(OpModeAccesser& cw) {
00232         cw.set(CW_Immediate);
00233         if(hasTarget()){
00234             int32_t target = getTarget();
00235             if((sw_ & MASK_Acknowledged) == 0 && target != last_target_){
00236                 if(cw.get(CW_NewPoint)){
00237                     cw.reset(CW_NewPoint); // reset if needed
00238                 }else{
00239                     target_position_.set(target);
00240                     cw.set(CW_NewPoint);
00241                     last_target_ = target;
00242                 }
00243             } else if (sw_ & MASK_Acknowledged){
00244                 cw.reset(CW_NewPoint);
00245             }
00246             return true;
00247         }
00248         return false;
00249     }
00250 };
00251 
00252 class HomingMode: public Mode{
00253 protected:
00254     enum SW_bits {
00255         SW_Attained =  State402::SW_Operation_mode_specific0,
00256         SW_Error =  State402::SW_Operation_mode_specific1,
00257     };
00258     enum CW_bits {
00259         CW_StartHoming =  Command402::CW_Operation_mode_specific0,
00260     };
00261 public:
00262     HomingMode() : Mode(MotorBase::Homing) {}
00263     virtual bool executeHoming(canopen::LayerStatus &status) = 0;
00264 };
00265 
00266 class DefaultHomingMode: public HomingMode{
00267     canopen::ObjectStorage::Entry<int8_t> homing_method_;
00268     boost::atomic<bool> execute_;
00269 
00270     boost::mutex mutex_;
00271     boost::condition_variable cond_;
00272     uint16_t status_;
00273 
00274     enum SW_masks {
00275         MASK_Reached = (1<<State402::SW_Target_reached),
00276         MASK_Attained = (1<<SW_Attained),
00277         MASK_Error = (1<<SW_Error),
00278     };
00279     bool error(canopen::LayerStatus &status, const std::string& msg) { execute_= false; status.error(msg); return false; }
00280 public:
00281     DefaultHomingMode(boost::shared_ptr<ObjectStorage> storage) {
00282         storage->entry(homing_method_, 0x6098);
00283     }
00284     virtual bool start();
00285     virtual bool read(const uint16_t &sw);
00286     virtual bool write(OpModeAccesser& cw);
00287 
00288     virtual bool executeHoming(canopen::LayerStatus &status);
00289 };
00290 
00291 class Motor402 : public MotorBase
00292 {
00293 public:
00294 
00295     Motor402(const std::string &name, boost::shared_ptr<ObjectStorage> storage, const canopen::Settings &settings)
00296     : MotorBase(name), status_word_(0),control_word_(0),
00297       switching_state_(State402::InternalState(settings.get_optional<unsigned int>("switching_state", static_cast<unsigned int>(State402::Operation_Enable)))),
00298       monitor_mode_(settings.get_optional<bool>("monitor_mode", true))
00299     {
00300         storage->entry(status_word_entry_, 0x6041);
00301         storage->entry(control_word_entry_, 0x6040);
00302         storage->entry(op_mode_display_, 0x6061);
00303         storage->entry(op_mode_, 0x6060);
00304         try{
00305             storage->entry(supported_drive_modes_, 0x6502);
00306         }
00307         catch(...){
00308         }
00309     }
00310 
00311     virtual bool setTarget(double val);
00312     virtual bool enterModeAndWait(uint16_t mode);
00313     virtual bool isModeSupported(uint16_t mode);
00314     virtual uint16_t getMode();
00315 
00316     template<typename T> bool registerMode(uint16_t mode) {
00317         return mode_allocators_.insert(std::make_pair(mode, boost::bind(&Motor402::createAndRegister<T>, this, mode))).second;
00318     }
00319     template<typename T, typename T1> bool registerMode(uint16_t mode, const T1& t1) {
00320         return mode_allocators_.insert(std::make_pair(mode, boost::bind(&Motor402::createAndRegister<T,T1>, this, mode, t1))).second;
00321     }
00322     template<typename T, typename T1, typename T2> bool registerMode(uint16_t mode, const T1& t1, const T2& t2) {
00323         return mode_allocators_.insert(std::make_pair(mode, boost::bind(&Motor402::createAndRegister<T,T1,T2>, this, mode, t1, t2))).second;
00324     }
00325 
00326     virtual void registerDefaultModes(boost::shared_ptr<ObjectStorage> storage){
00327         registerMode<ProfiledPositionMode> (MotorBase::Profiled_Position, storage);
00328         registerMode<VelocityMode> (MotorBase::Velocity, storage);
00329         registerMode<ProfiledVelocityMode> (MotorBase::Profiled_Velocity, storage);
00330         registerMode<ProfiledTorqueMode> (MotorBase::Profiled_Torque, storage);
00331         registerMode<DefaultHomingMode> (MotorBase::Homing, storage);
00332         registerMode<InterpolatedPositionMode> (MotorBase::Interpolated_Position, storage);
00333         registerMode<CyclicSynchronousPositionMode> (MotorBase::Cyclic_Synchronous_Position, storage);
00334         registerMode<CyclicSynchronousVelocityMode> (MotorBase::Cyclic_Synchronous_Velocity, storage);
00335         registerMode<CyclicSynchronousTorqueMode> (MotorBase::Cyclic_Synchronous_Torque, storage);
00336     }
00337 
00338     class Allocator : public MotorBase::Allocator{
00339     public:
00340         virtual boost::shared_ptr<MotorBase> allocate(const std::string &name, boost::shared_ptr<ObjectStorage> storage, const canopen::Settings &settings);
00341     };
00342 protected:
00343     virtual void handleRead(LayerStatus &status, const LayerState &current_state);
00344     virtual void handleWrite(LayerStatus &status, const LayerState &current_state);
00345     virtual void handleDiag(LayerReport &report);
00346     virtual void handleInit(LayerStatus &status);
00347     virtual void handleShutdown(LayerStatus &status);
00348     virtual void handleHalt(LayerStatus &status);
00349     virtual void handleRecover(LayerStatus &status);
00350 
00351 private:
00352     template<typename T> void createAndRegister0(uint16_t mode){
00353         if(isModeSupportedByDevice(mode)) registerMode(mode, boost::shared_ptr<Mode>(new T()));
00354     }
00355     template<typename T, typename T1> void createAndRegister(uint16_t mode, const T1& t1){
00356         if(isModeSupportedByDevice(mode)) registerMode(mode, boost::shared_ptr<Mode>(new T(t1)));
00357     }
00358     template<typename T, typename T1, typename T2> void createAndRegister(uint16_t mode, const T1& t1, const T2& t2){
00359         if(isModeSupportedByDevice(mode)) registerMode(mode, boost::shared_ptr<Mode>(new T(t1,t2)));
00360     }
00361 
00362     virtual bool isModeSupportedByDevice(uint16_t mode);
00363     void registerMode(uint16_t id, const boost::shared_ptr<Mode> &m);
00364 
00365     boost::shared_ptr<Mode> allocMode(uint16_t mode);
00366 
00367     bool readState(LayerStatus &status, const LayerState &current_state);
00368     bool switchMode(LayerStatus &status, uint16_t mode);
00369     bool switchState(LayerStatus &status, const State402::InternalState &target);
00370 
00371     boost::atomic<uint16_t> status_word_;
00372     uint16_t control_word_;
00373     boost::mutex cw_mutex_;
00374     boost::atomic<bool> start_fault_reset_;
00375     boost::atomic<State402::InternalState> target_state_;
00376 
00377 
00378     State402 state_handler_;
00379 
00380     boost::mutex map_mutex_;
00381     boost::unordered_map<uint16_t, boost::shared_ptr<Mode> > modes_;
00382     boost::unordered_map<uint16_t, boost::function<void()> > mode_allocators_;
00383 
00384     boost::shared_ptr<Mode> selected_mode_;
00385     uint16_t mode_id_;
00386     boost::condition_variable mode_cond_;
00387     boost::mutex mode_mutex_;
00388     const State402::InternalState switching_state_;
00389     const bool monitor_mode_;
00390 
00391     canopen::ObjectStorage::Entry<uint16_t>  status_word_entry_;
00392     canopen::ObjectStorage::Entry<uint16_t >  control_word_entry_;
00393     canopen::ObjectStorage::Entry<int8_t>  op_mode_display_;
00394     canopen::ObjectStorage::Entry<int8_t>  op_mode_;
00395     canopen::ObjectStorage::Entry<uint32_t>  supported_drive_modes_;
00396 };
00397 
00398 }
00399 
00400 #endif


canopen_402
Author(s): Thiago de Freitas , Mathias L├╝dtke
autogenerated on Thu Jun 6 2019 20:44:04