motorConfig.hpp

Go to the documentation of this file.

#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 <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 {

  // 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           = "action";
  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_NAMES             = "posnames";
  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";

  const std::vector<std::string> cfgMapping = {
    CI_ACTIVE_LOW_STOP
  };

  const std::vector<std::string> statMapping = {
    devmgr::common::CI_STAT_SUBSTATE,
    devmgr::common::CI_STAT_STATE,
    devmgr::common::CI_STAT_LOCAL,
    CI_STAT_TARGET_POS,
    CI_STAT_ACTUAL_POS,
    CI_STAT_ACTUAL_VEL,
    devmgr::common::CI_STAT_ERROR_CODE,
    CI_STAT_SCALE_FACTOR,
    CI_STAT_AXIS_BRAKE,
    CI_STAT_AXIS_ENABLE,
    CI_STAT_AXIS_LOCK,
    //      CI_STAT_POS_ERROR,
    //      CI_STAT_MODE,
    //      CI_STAT_AXIS_READY,
    //      CI_STAT_AXIS_INPOSition,
    //      CI_STAT_SIGNAL_LSTOP,
    //      CI_STAT_SIGNAL_LHW,
    //      CI_STAT_SIGNAL_REF,
    //      CI_STAT_SIGNAL_INDEX,
    //      CI_STAT_SIGNAL_UHW,
    //      CI_STAT_SIGNAL_USTOP,
    //      CI_STAT_AXIS_INFO_DATA_1,
    //      CI_STAT_AXIS_INFO_DATA_2
  };

  const std::vector<std::string> rpcMapping = {
    fcf::devmgr::common::RPC_INIT,
    fcf::devmgr::common::RPC_ENABLE,
    fcf::devmgr::common::RPC_DISABLE,
    fcf::devmgr::common::RPC_RESET,
    fcf::devmgr::common::RPC_STOP,
    RPC_MOVE_ABS,
    RPC_MOVE_REL,
    RPC_MOVE_VEL
  };

  // 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 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);

    virtual ~MotorConfig();

    virtual void ReadConfig();

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


    virtual void GetConfigList(std::vector<std::string>& cfg_list,
                               std::string prefix);



    int GetNumberOfNamedPositions();


    double GetVelocity();

    void SetScaleFactor(double scale);

    double GetScaleFactor() const;

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

    const double GetNamedPositionValue(const std::string named_pos);

    bool FindNamedPositionValue(const std::string named_pos,
                                double& position);

    bool FindNamedPositionValue(const double position,
                                std::string& name);

    double GetNpTolerance() const;


  protected:

    // Map of motor init step to do the conversion between strings to numbers
    std::unordered_map<int, InitAction> m_init_sequence_map;

  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;



  };

}


#endif //FCF_DEVMGR_DEVICE_MOTOR_CONFIG_H