IFW_HL/fcf/motorConfig_8hpp_source.html

#ifndef FCF_DEVMGR_DEVICE_MOTOR_CONFIG_HPP

#define FCF_DEVMGR_DEVICE_MOTOR_CONFIG_HPP


// System headers

#include <string>

#include <unordered_map>


// Third party headers

#include <yaml-cpp/yaml.h>


#include <utils/bat/logger.hpp>

#include <fcf/common/iComm.hpp>

#include <fcf/devmgr/common/deviceConfig.hpp>

#include <fcf/devmgr/common/configSet.hpp>

#include <fcf/devmgr/common/dataContext.hpp>

#include <fcf/devmgr/devices/motorRpcErrors.hpp>

#include <fcf/devmgr/devices/motorHwErrors.hpp>


namespace fcf::devmgr::motor {


  constexpr auto LOGGER_NAME = "motor";

  // Constants for internal mapping

  constexpr auto CI_BRAKE                 = "brake";

  constexpr auto CI_BACKLASH              = "backlash";

  constexpr auto CI_AXIS_TYPE             = "axis_type";

  constexpr auto CI_DEF_VEL               = "velocity";

  constexpr auto CI_MAX_POS               = "max_pos";

  constexpr auto CI_MIN_POS               = "min_pos";


  constexpr auto CI_CHECK_INPOS           = "check_inpos";

  constexpr auto CI_LOCK                  = "lock";

  constexpr auto CI_LOCK_POS              = "lock_pos";

  constexpr auto CI_LOCK_TOLERANCE        = "lock_tolerance";

  constexpr auto CI_DISABLE               = "disable";


  constexpr auto CI_TIMEOUT_INIT          = "tout_init";

  constexpr auto CI_TIMEOUT_MOVE          = "tout_move";

  constexpr auto CI_TIMEOUT_SWITCH        = "tout_switch";


  constexpr auto CI_ACTIVE_LOW_BRAKE      = "low_brake";

  constexpr auto CI_ACTIVE_LOW_INPOS      = "low_inpos";

  constexpr auto CI_ACTIVE_LOW_STOP       = "active_low_lstop";

  constexpr auto CI_ACTIVE_LOW_LHW        = "active_low_lhw";

  constexpr auto CI_ACTIVE_LOW_REF        = "active_low_ref";

  constexpr auto CI_ACTIVE_LOW_INDEX      = "active_low_index";

  constexpr auto CI_ACTIVE_LOW_UHW        = "active_low_uhw";

  constexpr auto CI_ACTIVE_LOW_USTOP      = "active_low_ustop";


  constexpr auto CI_EXEC_PRE_INIT         = "exec_pre_init";

  constexpr auto CI_EXEC_POST_INIT        = "exec_post_init";

  constexpr auto CI_EXEC_PRE_MOVE         = "exec_pre_move";

  constexpr auto CI_EXEC_POST_MOVE        = "exec_post_move";


  constexpr auto CI_INITIALISATION        = "initialisation";

  constexpr auto CI_INIT_SEQUENCE         = "sequence";

  constexpr auto CI_INIT_ACTION           = "step";

  constexpr auto CI_INIT_VALUE_1          = "value1";

  constexpr auto CI_INIT_VALUE_2          = "value2";


  // WS configuration keywords

  // These configuration only matters for WS level

  constexpr auto CI_POSITIONS             = "positions";

  constexpr auto CI_POS_NAME              = "name";

  constexpr auto CI_POS_VALUE             = "value";

  constexpr auto CI_NAME_POS_TOLERANCE    = "tolerance";

  constexpr auto CI_SCALE_FACTOR          = "scale_factor";


  constexpr int CI_AXIS_TYPE_LINEAR              = 1;

  constexpr int CI_AXIS_TYPE_CIRCULAR            = 2;

  constexpr int CI_AXIS_TYPE_CIRCULAR_OPT        = 3;


  constexpr auto CI_AXIS_TYPE_LINEAR_STR         = "LINEAR";

  constexpr auto CI_AXIS_TYPE_CIRCULAR_STR       = "CIRCULAR";

  constexpr auto CI_AXIS_TYPE_CIRCULAR_OPT_STR   = "CIRCULAR_OPT";


  constexpr int CI_AXIS_MODE_INPOS               = 0;

  constexpr int CI_AXIS_MODE_INVEL               = 1;


  constexpr auto CI_AXIS_MODE_INPOS_STR   = "InPosition";

  constexpr auto CI_AXIS_MODE_INVEL_STR   = "InVelocity";


  const std::unordered_map<int, std::string>  AxisModeMap = {

    {CI_AXIS_MODE_INPOS, CI_AXIS_MODE_INPOS_STR},

    {CI_AXIS_MODE_INVEL, CI_AXIS_MODE_INVEL_STR}

  };


  constexpr int CI_MAX_INIT_STEPS                     = 10;


  constexpr auto CI_INIT_ACTION_END            = "END";

  constexpr auto CI_INIT_ACTION_FIND_INDEX     = "FIND_INDEX";

  constexpr auto CI_INIT_ACTION_FIND_REF_LE    = "FIND_REF_LE";

  constexpr auto CI_INIT_ACTION_FIND_REF_UE    = "FIND_REF_UE";

  constexpr auto CI_INIT_ACTION_FIND_LHW       = "FIND_LHW";

  constexpr auto CI_INIT_ACTION_FIND_UHW       = "FIND_UHW";

  constexpr auto CI_INIT_ACTION_DELAY          = "DELAY";

  constexpr auto CI_INIT_ACTION_MOVE_ABS       = "MOVE_ABS";

  constexpr auto CI_INIT_ACTION_MOVE_REL       = "MOVE_REL";

  constexpr auto CI_INIT_ACTION_CALIB_ABS      = "CALIB_ABS";

  constexpr auto CI_INIT_ACTION_CALIB_REL      = "CALIB_REL";

  constexpr auto CI_INIT_ACTION_CALIB_SWITCH   = "CALIB_SWITCH";


  constexpr auto RPC_MOVE_ABS                  = "rpcMoveAbs";

  constexpr auto RPC_MOVE_REL                  = "rpcMoveRel";

  constexpr auto RPC_MOVE_VEL                  = "rpcMoveVel";


  constexpr int SUBSTATE_ABORTING              =  107;

  constexpr int SUBSTATE_OP_STANDSTILL         =  216;

  constexpr int SUBSTATE_OP_MOVING             =  217;

  constexpr int SUBSTATE_OP_SETTING_POS        =  218;

  constexpr int SUBSTATE_OP_STOPPING           =  219;


  constexpr auto SUBSTATE_READY_STR            =  "Ready";

  constexpr auto SUBSTATE_ABORTING_STR         =  "Aborting";

  constexpr auto SUBSTATE_OP_STANDSTILL_STR    =  "Standstill";

  constexpr auto SUBSTATE_OP_MOVING_STR        =  "Moving";

  constexpr auto SUBSTATE_OP_SETTING_POS_STR   =  "SettingPos";

  constexpr auto SUBSTATE_OP_STOPPING_STR      =  "Stopping";


  const std::unordered_map<short, std::string>  SubstateMap = {

    {fcf::devmgr::common::SUBSTATE_NOTREADY,       fcf::devmgr::common::SUBSTATE_NOTREADY_STR},

    {fcf::devmgr::common::SUBSTATE_INITIALISING,   fcf::devmgr::common::SUBSTATE_INITIALISING_STR},

    {fcf::devmgr::common::SUBSTATE_READY,          fcf::devmgr::common::SUBSTATE_READY_STR},

    {fcf::devmgr::common::SUBSTATE_ERROR,          fcf::devmgr::common::SUBSTATE_ERROR_STR},

    {SUBSTATE_ABORTING,       SUBSTATE_ABORTING_STR},

    {SUBSTATE_OP_STANDSTILL,  SUBSTATE_OP_STANDSTILL_STR},

    {SUBSTATE_OP_MOVING,      SUBSTATE_OP_MOVING_STR},

    {SUBSTATE_OP_SETTING_POS, SUBSTATE_OP_SETTING_POS_STR},

    {SUBSTATE_OP_STOPPING,    SUBSTATE_OP_STOPPING_STR},

    {fcf::devmgr::common::SUBSTATE_OP_ERROR,       fcf::devmgr::common::SUBSTATE_OP_ERROR_STR},

  };


  // encoders are used only at the WS level

  constexpr auto CI_STAT_ENC_POS              = "pos_enc";

  constexpr auto CI_STAT_TARGET_ENC           = "target_enc";

  constexpr auto CI_STAT_TARGET_POSNAME       = "pos_target_name";

  constexpr auto CI_STAT_ACTUAL_POSNAME       = "pos_actual_name";

  constexpr auto CI_STAT_TARGET_POS           = "pos_target";

  constexpr auto CI_STAT_ACTUAL_POS           = "pos_actual";

  constexpr auto CI_STAT_POS_ERROR            = "pos_error";

  constexpr auto CI_STAT_ACTUAL_VEL           = "vel_actual";

  constexpr auto CI_STAT_TARGET_VEL           = "vel_target";

  constexpr auto CI_STAT_SCALE_FACTOR         = "scale_factor";

  constexpr auto CI_STAT_BACKLASH_STEP        = "backlash_step";

  constexpr auto CI_STAT_MODE                 = "mode";

  constexpr auto CI_STAT_INITIALISED          = "initialised";

  constexpr auto CI_STAT_INIT_STEP            = "init_step";

  constexpr auto CI_STAT_INIT_ACTION          = "init_action";

  constexpr auto CI_STAT_AXIS_READY           = "axis_ready";

  constexpr auto CI_STAT_AXIS_ENABLE          = "axis_enable";

  constexpr auto CI_STAT_AXIS_INPOS           = "axis_inposition";

  constexpr auto CI_STAT_AXIS_LOCK            = "axis_lock";

  constexpr auto CI_STAT_AXIS_BRAKE           = "axis_brake";

  constexpr auto CI_STAT_AXIS_STOP_NEG        = "axis_stop_pos";

  constexpr auto CI_STAT_AXIS_STOP_POS        = "axis_stop_neg";

  constexpr auto CI_STAT_AXIS_INFO_DATA_1     = "axis_info_data1";

  constexpr auto CI_STAT_AXIS_INFO_DATA_2     = "axis_info_data2";


  constexpr auto CI_STAT_SIGNAL_LSTOP         = "signal_lstop";

  constexpr auto CI_STAT_SIGNAL_LHW           = "signal_lhw";

  constexpr auto CI_STAT_SIGNAL_REF           = "signal_ref";

  constexpr auto CI_STAT_SIGNAL_INDEX         = "signal_index";

  constexpr auto CI_STAT_SIGNAL_UHW           = "signal_uhw";

  constexpr auto CI_STAT_SIGNAL_USTOP         = "signal_ustop";


  enum class Signal : short {LSTOP = 0,

                             LHW   = 1,

                             REF   = 2,

                             INDEX = 3,

                             UHW   = 4,

                             USTOP = 5};


  constexpr auto FITS_PARAM_POSNAME      = "POSNAME";

  constexpr auto FITS_PARAM_POS          = "POS";

  constexpr auto FITS_PARAM_BRAKE        = "BRAKE";

  constexpr auto FITS_PARAM_LOCK         = "LOCK";


  constexpr auto UndefinedNamedPos       = "undefined";


  // Map of Axis type to do the converstion between strings to numbers

  const std::unordered_map<std::string, int> AxisTypeMap = {

    {CI_AXIS_TYPE_LINEAR_STR,       CI_AXIS_TYPE_LINEAR},

    {CI_AXIS_TYPE_CIRCULAR_STR,     CI_AXIS_TYPE_CIRCULAR},

    {CI_AXIS_TYPE_CIRCULAR_OPT_STR,  CI_AXIS_TYPE_CIRCULAR_OPT}

  };


  enum {

    INIT_ACTION_END          = 0,

    INIT_ACTION_FIND_INDEX   = 1,

    INIT_ACTION_FIND_REF_LE  = 2,

    INIT_ACTION_FIND_REF_UE  = 3,

    INIT_ACTION_FIND_LHW     = 4,

    INIT_ACTION_FIND_UHW     = 5,

    INIT_ACTION_DELAY        = 6,

    INIT_ACTION_MOVE_ABS     = 7,

    INIT_ACTION_MOVE_REL     = 8,

    INIT_ACTION_CALIB_ABS    = 9,

    INIT_ACTION_CALIB_REL    = 10,

    INIT_ACTION_CALIB_SWITCH = 11

  };


  // @TODO: replaced by boost bimaps

  // Tried with boost bimap but I've got hundred of errors.

  // I added temporaly two maps.


  // Map of motor initialisation steps

  const std::unordered_map<std::string, int> InitActionsMap = {

    {CI_INIT_ACTION_END,        INIT_ACTION_END},

    {CI_INIT_ACTION_FIND_INDEX,  INIT_ACTION_FIND_INDEX},

    {CI_INIT_ACTION_FIND_REF_LE,  INIT_ACTION_FIND_REF_LE},

    {CI_INIT_ACTION_FIND_REF_UE,  INIT_ACTION_FIND_REF_UE},

    {CI_INIT_ACTION_FIND_LHW,    INIT_ACTION_FIND_LHW},

    {CI_INIT_ACTION_FIND_UHW,    INIT_ACTION_FIND_UHW},

    {CI_INIT_ACTION_DELAY,      INIT_ACTION_DELAY},

    {CI_INIT_ACTION_MOVE_ABS,    INIT_ACTION_MOVE_ABS},

    {CI_INIT_ACTION_MOVE_REL,    INIT_ACTION_MOVE_REL},

    {CI_INIT_ACTION_CALIB_ABS,   INIT_ACTION_CALIB_ABS},

    {CI_INIT_ACTION_CALIB_REL,   INIT_ACTION_CALIB_REL},

    {CI_INIT_ACTION_CALIB_SWITCH,INIT_ACTION_CALIB_SWITCH}

  };


  // Map of motor initialisation steps

  const std::unordered_map<int, std::string> InitActions2Map = {

    {INIT_ACTION_END,         CI_INIT_ACTION_END},

    {INIT_ACTION_FIND_INDEX,  CI_INIT_ACTION_FIND_INDEX},

    {INIT_ACTION_FIND_REF_LE, CI_INIT_ACTION_FIND_REF_LE},

    {INIT_ACTION_FIND_REF_UE, CI_INIT_ACTION_FIND_REF_UE},

    {INIT_ACTION_FIND_LHW,    CI_INIT_ACTION_FIND_LHW},

    {INIT_ACTION_FIND_UHW,    CI_INIT_ACTION_FIND_UHW},

    {INIT_ACTION_DELAY,       CI_INIT_ACTION_DELAY},

    {INIT_ACTION_MOVE_ABS,    CI_INIT_ACTION_MOVE_ABS},

    {INIT_ACTION_MOVE_REL,    CI_INIT_ACTION_MOVE_REL},

    {INIT_ACTION_CALIB_ABS,   CI_INIT_ACTION_CALIB_ABS},

    {INIT_ACTION_CALIB_REL,   CI_INIT_ACTION_CALIB_REL},

    {INIT_ACTION_CALIB_SWITCH, CI_INIT_ACTION_CALIB_SWITCH}

  };


  struct InitAction {

    int id;

    double value1;

    double value2;


    InitAction():id(0), value1(0.0), value2(0.0) {}

  };


  class Sensor;

  class MotorConfig : public fcf::devmgr::common::DeviceConfig

  {

    friend class Sensor;


  public:

    enum {

      CFG_BRAKE,

      CFG_BACKLASH,

      CFG_AXISTYPE,

      CFG_DEFVEL,

      CFG_MAXPOS,

      CFG_MINPOS,

      CFG_CHECKINPOS,

      CFG_LOCK,

      CFG_LOCKPOS,

      CFG_LOCKTOL,

      CFG_AUTODISABLE,

      CFG_INITTOUT,

      CFG_MOVETOUT,

      CFG_SWITCHTOUT,

      CFG_ACTLOWBRAKE,

      CFG_ACTLOWINPOS,

      CFG_ACTLOWLSTOP,

      CFG_ACTLOWLHW,

      CFG_ACTLOWREF,

      CFG_ACTLOWINDEX,

      CFG_ACTLOWUHW,

      CFG_ACTLOWUSTOP,

      CFG_EXECPREINIT,

      CFG_EXECPOSTINIT,

      CFG_EXECPREMOVE,

      CFG_EXECPOSTMOVE,

      END_ENUM_MOTOR // WARNING: This should be always last enumerator.

    };


  public:

    MotorConfig(const std::string filename,

                const std::string name);


    MotorConfig(const std::string name);


    virtual ~MotorConfig() = default;


    virtual void Init();

    virtual void GetConfig(fcf::common::VectorVariant& params) const override;


    virtual void GetConfigList(utils::bat::DbVector& cfg_list,

                               std::string prefix) const override;


    inline int GetNumberOfNamedPositions() const noexcept {return m_num_named_pos;};


    inline int GetNumberOfInitSteps() const noexcept {return m_init_sequence.size();};


    double GetVelocity() const ;


    void SetScaleFactor(double scale);


    double GetScaleFactor() const;


    std::string GetNamedPosition(const int index) const;


    double GetNamedPositionValue(const int index) const;


    bool FindNamedPositionValue(const std::string named_pos,

                                double& position) const;


    bool FindNamedPositionValue(const double position,

                                std::string& name) const;


    double GetNpTolerance() const;


  protected:


    // Map of motor init step to do the conversion between strings to numbers

    std::vector<InitAction> m_init_sequence;


  private:

    // dynamic configuration values

    double m_scale_factor;

    int    m_num_named_pos;

    double m_np_tolerance;


    // Map of motor named positions

    std::unordered_map<std::string, double> m_named_positions;

    log4cplus::Logger  m_logger;


  };


}


#endif //FCF_DEVMGR_DEVICE_MOTOR_CONFIG_H