motorConfig.hpp

Go to the documentation of this file.

#ifndef DEVMGR_DEVICE_MOTOR_CONFIG_HPP
#define 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 <devmgr/common/deviceConfig.hpp>
#include <devmgr/common/configSet.hpp>
#include <devmgr/common/dataContext.hpp>
#include <devmgr/devices/motorRpcErrors.hpp>
#include <devmgr/devices/motorHwErrors.hpp>


namespace devmgr {
  namespace motor {

    // Constants for internal mapping
    const std::string CI_BRAKE                 = "brake";
    const std::string CI_BACKLASH              = "backlash";
    const std::string CI_AXIS_TYPE             = "axis_type";
    const std::string CI_DEF_VEL               = "velocity";
    const std::string CI_MAX_POS               = "max_pos";
    const std::string CI_MIN_POS               = "min_pos";

    const std::string CI_CHECK_INPOS           = "check_inpos";
    const std::string CI_LOCK                  = "lock";
    const std::string CI_LOCK_POS              = "lock_pos";
    const std::string CI_LOCK_TOLERANCE        = "lock_tolerance";
    const std::string CI_DISABLE               = "disable";

    const std::string CI_TIMEOUT_INIT          = "tout_init";
    const std::string CI_TIMEOUT_MOVE          = "tout_move";
    const std::string CI_TIMEOUT_SWITCH        = "tout_switch";

    const std::string CI_ACTIVE_LOW_BRAKE      = "low_brake";
    const std::string CI_ACTIVE_LOW_INPOS      = "low_inpos";
    const std::string CI_ACTIVE_LOW_STOP       = "active_low_lstop";
    const std::string CI_ACTIVE_LOW_LHW        = "active_low_lhw";
    const std::string CI_ACTIVE_LOW_REF        = "active_low_ref";
    const std::string CI_ACTIVE_LOW_INDEX      = "active_low_index";
    const std::string CI_ACTIVE_LOW_UHW        = "active_low_uhw";
    const std::string CI_ACTIVE_LOW_USTOP      = "active_low_ustop";

    const std::string CI_EXEC_PRE_INIT         = "exec_pre_init";
    const std::string CI_EXEC_POST_INIT        = "exec_post_init";
    const std::string CI_EXEC_PRE_MOVE         = "exec_pre_move";
    const std::string CI_EXEC_POST_MOVE        = "exec_post_move";

    const std::string CI_INITIALISATION        = "initialisation";
    const std::string CI_INIT_SEQUENCE         = "sequence";
    const std::string CI_INIT_ACTION           = "action";
    const std::string CI_INIT_VALUE_1          = "value1";
    const std::string CI_INIT_VALUE_2          = "value2";


    // WS configuration keywords
    // These configuration only matters for WS level
    const std::string CI_POSITIONS             = "positions";
    const std::string CI_POS_NAMES             = "posnames";
    const std::string CI_NAME_POS_TOLERANCE    = "tolerance";
    const std::string CI_SCALE_FACTOR          = "scale_factor";

    const int CI_AXIS_TYPE_LINEAR              = 1;
    const int CI_AXIS_TYPE_CIRCULAR            = 2;
    const int CI_AXIS_TYPE_CIRCULAR_OPT        = 3;

    const std::string CI_AXIS_TYPE_LINEAR_STR         = "LINEAR";
    const std::string CI_AXIS_TYPE_CIRCULAR_STR       = "CIRCULAR";
    const std::string CI_AXIS_TYPE_CIRCULAR_OPT_STR   = "CIRCULAR_OPT";

    const int CI_AXIS_MODE_INPOS               = 0;
    const int CI_AXIS_MODE_INVEL               = 1;

    const std::string CI_AXIS_MODE_INPOS_STR   = "InPosition";
    const std::string 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}
    };

    const int CI_MAX_INIT_STEPS                     = 10;

    const std::string CI_INIT_ACTION_END            = "END";
    const std::string CI_INIT_ACTION_FIND_INDEX     = "FIND_INDEX";
    const std::string CI_INIT_ACTION_FIND_REF_LE    = "FIND_REF_LE";
    const std::string CI_INIT_ACTION_FIND_REF_UE    = "FIND_REF_UE";
    const std::string CI_INIT_ACTION_FIND_LHW       = "FIND_LHW";
    const std::string CI_INIT_ACTION_FIND_UHW       = "FIND_UHW";
    const std::string CI_INIT_ACTION_DELAY          = "DELAY";
    const std::string CI_INIT_ACTION_MOVE_ABS       = "MOVE_ABS";
    const std::string CI_INIT_ACTION_MOVE_REL       = "MOVE_REL";
    const std::string CI_INIT_ACTION_CALIB_ABS      = "CALIB_ABS";
    const std::string CI_INIT_ACTION_CALIB_REL      = "CALIB_REL";
    const std::string CI_INIT_ACTION_CALIB_SWITCH   = "CALIB_SWITCH";


    const std::string RPC_MOVE_ABS               = "rpcMoveAbs";
    const std::string RPC_MOVE_REL               = "rpcMoveRel";
    const std::string RPC_MOVE_VEL               = "rpcMoveVel";


    const short SUBSTATE_NOTREADY             =  100;
    const short SUBSTATE_INITIALISING         =  101;
    const short SUBSTATE_READY                =  102;
    const short SUBSTATE_FAILURE              =  103;
    const short SUBSTATE_ABORTING             =  104;

    const short SUBSTATE_OP_STANDSTILL        =  200;
    const short SUBSTATE_OP_MOVING            =  201;
    const short SUBSTATE_OP_SETTING_POS       =  202;
    const short SUBSTATE_OP_STOPPING          =  203;
    const short SUBSTATE_OP_FAILURE           =  204;

    const std::string SUBSTATE_NOTREADY_STR       =  "NotReady";
    const std::string SUBSTATE_INITIALISING_STR   =  "Initialising";
    const std::string SUBSTATE_READY_STR          =  "Ready";
    const std::string SUBSTATE_FAILURE_STR        =  "Failure";
    const std::string SUBSTATE_ABORTING_STR       =  "Aborting";

    const std::string SUBSTATE_OP_STANDSTILL_STR  =  "Standstill";
    const std::string SUBSTATE_OP_MOVING_STR      =  "Moving";
    const std::string SUBSTATE_OP_SETTING_POS_STR =  "SettingPos";
    const std::string SUBSTATE_OP_STOPPING_STR    =  "Stopping";
    const std::string SUBSTATE_OP_FAILURE_STR     =  "Failure";

    const std::unordered_map<short, std::string>  SubstateMap = {
      {SUBSTATE_NOTREADY,       SUBSTATE_NOTREADY_STR},
      {SUBSTATE_INITIALISING,   SUBSTATE_INITIALISING_STR},
      {SUBSTATE_READY,          SUBSTATE_READY_STR},
      {SUBSTATE_FAILURE,        SUBSTATE_FAILURE_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},
      {SUBSTATE_OP_FAILURE,     SUBSTATE_OP_FAILURE_STR},
    };

    // encoders are used only at the WS level
    const std::string CI_STAT_ENC_POS              = "pos_enc";
    const std::string CI_STAT_TARGET_ENC           = "target_enc";
    const std::string CI_STAT_TARGET_POSNAME       = "pos_target_name";
    const std::string CI_STAT_ACTUAL_POSNAME       = "pos_actual_name";
    const std::string CI_STAT_TARGET_POS           = "pos_target";
    const std::string CI_STAT_ACTUAL_POS           = "pos_actual";
    const std::string CI_STAT_POS_ERROR            = "pos_error";
    const std::string CI_STAT_ACTUAL_VEL           = "vel_actual";
    const std::string CI_STAT_TARGET_VEL           = "vel_target";
    const std::string CI_STAT_SCALE_FACTOR         = "scale_factor";
    const std::string CI_STAT_BACKLASH_STEP        = "backlash_step";
    const std::string CI_STAT_MODE                 = "mode";
    const std::string CI_STAT_INITIALISED          = "initialised";
    const std::string CI_STAT_INIT_STEP            = "init_step";
    const std::string CI_STAT_INIT_ACTION          = "init_action";
    const std::string CI_STAT_AXIS_READY           = "axis_ready";
    const std::string CI_STAT_AXIS_ENABLE          = "axis_enable";
    const std::string CI_STAT_AXIS_INPOS           = "axis_inposition";
    const std::string CI_STAT_AXIS_LOCK            = "axis_lock";
    const std::string CI_STAT_AXIS_BRAKE           = "axis_brake";
    const std::string CI_STAT_AXIS_STOP_NEG        = "axis_stop_pos";
    const std::string CI_STAT_AXIS_STOP_POS        = "axis_stop_neg";
    const std::string CI_STAT_AXIS_INFO_DATA_1     = "axis_info_data1";
    const std::string CI_STAT_AXIS_INFO_DATA_2     = "axis_info_data2";

    const std::string CI_STAT_SIGNAL_LSTOP         = "signal_lstop";
    const std::string CI_STAT_SIGNAL_LHW           = "signal_lhw";
    const std::string CI_STAT_SIGNAL_REF           = "signal_ref";
    const std::string CI_STAT_SIGNAL_INDEX         = "signal_index";
    const std::string CI_STAT_SIGNAL_UHW           = "signal_uhw";
    const std::string CI_STAT_SIGNAL_USTOP         = "signal_ustop";

    enum class Signal : short {LSTOP = 0,
                               LHW   = 1,
                               REF   = 2,
                               INDEX = 3,
                               UHW   = 4,
                               USTOP = 5};


    const std::string FITS_PARAM_POSNAME      = "POSNAME";
    const std::string FITS_PARAM_POS          = "POS";
    const std::string FITS_PARAM_BRAKE        = "BRAKE";
    const std::string FITS_PARAM_LOCK         = "LOCK";


    const std::string 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 = {
      devmgr::common::RPC_INIT,
      devmgr::common::RPC_ENABLE,
      devmgr::common::RPC_DISABLE,
      devmgr::common::RPC_RESET,
      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;

      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 std::string  GetMapFile();

      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;


    private:
      // All configuration parameters related to the LCS are stored in
      // dedicated STL maps to make them easilly accessible when downloading
      // the configuration to the LCS.
      devmgr::common::MapCfgBool   m_map_cfg_bool;
      devmgr::common::MapCfgInt    m_map_cfg_int;
      devmgr::common::MapCfgUInt   m_map_cfg_uint;
      devmgr::common::MapCfgDouble m_map_cfg_double;


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

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

    };
  }
}


#endif //DEVMGR_DEVICE_MOTOR_CONFIG_H