The CiA 402 norm is the “CANopen device profile for drives and motion control”. It aims at standardizing the behavior of motor control devices. CoE based Motor drives often implement this profile, that is why we provide a class to ease implementation of corresponding ethercatcpp drivers.

Overview of the CIA402Device class

The class looks like:

class CIA402Device : public SlaveDevice {
public:
    // cia402 object dictionary default codes
    static constexpr const uint16_t controlword_addr = 0x6040;
    ...

    // flags to check operation state in statusword
    static constexpr const uint16_t not_ready_to_switch_on_code = 0x0000;
    ...

    // values for CIA402 standard operation modes
    static constexpr const int8_t monitor_mode = 0;
    static constexpr const int8_t PP_mode = 1;     //!< Profile position
    static constexpr const int8_t PV_mode = 3;     //!< Profile velocity
    ...
    static constexpr const int8_t CST_mode = 10; //!< Cyclic synchronous torque

    /**
     * @brief Possible state of a CiA402 device state machine
     *
     */
    enum state_t {
        unknown_state,
        not_ready_to_switch_on,
        switch_on_disabled,
        ready_to_switch_on,
        switched_on,
        operation_enabled,
        quick_stop_active,
        fault_reaction_active,
        faulty
    };

    /**
     * @brief Possible control over a CiA402 device state machine
     *
     */
    enum control_t {
        shutdown,                //!< Command to active state transition 2, 6, 8
        switch_on,               //!< Command to active state transition 3
        switch_on_and_enable_op, //!< Command to active state transition 3, 4
        disable_voltage, //!< Command to active state transition 7, 9, 10, 12
        quickstop,       //!< Command to active state transition 11
        disable_op,      //!< Command to active state transition 5
        enable_op,       //!< Command to active state transition 4, 16
        fault_reset_op   //!< Command to active state transition 15
    };
...
};

It define a partially implemented SlaveDevice whose role is primarily to implement the CiA402 standardized state machine. This state machine ensures that device is safely configured and controlled before applying command to motors. The secondary role of this class is to constraint the way control modes of the device are managed: it acts as an interface that must be specialized by concrete devices implementing the CiA402 profile.

These aspects will be explained in next sections.

Controlling the state machine

The class provides to users different methods to control the state machine:

/**
* @brief get current state of the device.
* @return state of the device.
*/
state_t operation_state() const;

/**
* @brief Acknowledge fault and reset faulty status.
* @details this function has an effect only if device is in faulty state
* After callling acknowledge_fault the device is again ready to operate and
* calls to activate() will enable operations.
* @see activate()
*/
void acknowledge_fault();

/**
* @brief Launch the quick stop procedure.
*/
void quick_stop();

/**
* @brief Tell whether the device can be immediately configured
* @return true if configurable, false otherwise
*/
bool is_configurable() const;

/**
* @brief Perform adequate operations depending on state.
* @details if current control mode is monitor then joint stays in
* switched_on state.
* WARNING: This function has to be used in the cyclic loop.
* FSM needs more than 1 cycle to activate/deactivate its power stage
* and release brakes.
* @return true when final state has been reached (depends on
* startup/shutdown, false otherwise)
* @see startup()
* @see shutdown()
*/
bool process_state();

/**
* @brief Make device controlable and control mode cannot be changed
* @details initially the device is started automatically. After this
* call process_state will return true when device is ready to operate
* the control mode.
* @see process_state()
*/
void startup();

/**
* @brief Put the device is a safe, configuration possible, state
* @details to be called before closing the application or any time before
* the control mode has to be switched to another control mode. Then wait
* process to return true to change the control mode and call startup().
* @see process_state()
*/
void finish();
/**
* @brief Tell whether the device is active (is running or will run
* commands) or not (is or will be in a safe state, where control mode can
* be configured)
* @return true if active, false otherwise
*/
bool active() const;

A basic pattern for using these methods is as follow:

ethercatcpp::MyCIA402Device device;
...
master.init();
...
device.startup();//automatic at beginning => call not required, here just for example
while (not stop) {//cyclic loop

        // a long as we are not in operation enabled state activate power stage
        if (device.operation_state() != op_state::operation_enabled) {
            if (device.operation_state() == op_state::switch_on_disabled) {
                (void)device.set_control_mode(control_mode);
            }

        } else { // now actuator is controlled
            // set target depending on mode
            switch (control_mode) {
            case ctrl_mode::cyclic_position:
            case ctrl_mode::profile_position:
                device.set_target_position(target_position);
                //...
                break;
            case ctrl_mode::cyclic_velocity:
            case ctrl_mode::profile_velocity:
                device.set_target_velocity(target_velocity);
                break;
            case ctrl_mode::cyclic_torque:
                device.set_target_motor_torque(target_torque);
                break;
            default: // no _mode !!
                break;
            }
        }

        device.process_state();
        if (master.next_cycle()) {
            // ...
        }
        period.sleep();
    }

    // Ensure the joint is in safe state before closing the connection
    device.finish();
    while (not device.process_state()) {
        (void)master.next_cycle();
        period.sleep();
    }

    // close EtherCAT master and socket.
    master.end();

The state machine is controlled using startup() and finish() functions:

  • startup() tries to bring the state machine in operation_enabled state if device has a control mode defined or in switched_on state if no control mode is defined (a.k.a. as monitor mode). The call to startup() is automatic at device creation.
  • finish() brings the state machine back in switch_on_disabled state. In this later state the device control mode can be reconfigured and a call to is_configurable() will answer true.
  • active() function simply tells if the device tries to enter in operation_enabled state (true) which is a consequence of a call to startup() or tries to go back in switch_on_disabled state (false) which is a consequence of a call to finish(). As long as device is active() the driver controls the CiA 402 device such as it goes from state not_ready_to_switch_on up to state switched_on/operation_enabled.

Other functions related to state machine control are:

  • process_state() must be called in the cyclic loop: it is in change of updating current state and controlling state change.
  • operation_state() is called to know the current state of the device.
  • quick_stop() triggers the quick stop procedure of the device (state quick_stop_active), which consists in ensuring the motor goes to 0 velocity and then stay stanstill. This is used to deal with urgency, stopping the motor before eventually engaging the brakes. Once quick stop procedure is finished the device goes into switch_on_disabled state.
  • acknowledge_fault() must be called to allow the device to exit a faulty state and go back to switch_on_disabled state. The faulty state is reached automatically after the device exits the fault_reaction_active. This later is automatically triggerred after a fault has been detected by the device and can do similar job as during quick_stop_active state.

Please note that after acknowledge_fault() or quick_stop() the device will try to reenter in switched_on/operation_enabled if it is still active().

Finally some other functions can be used to evaluate internal properties of the device:

  • fault_alert() and warning_alert() give information about the device. fault_alert() report a fault that makes the device entering the fault_reaction_active state.
  • reset_fault() resets any fault. This allow the user to force an exit of a faulty state at initialization time. Please note that if the internal error is persistent the device will immediately fall back to faulty state.
  • voltage_enabled() tells whether the device is powered.

Controlling the control mode

A CiA 402 device provides a set of control modes to control motors. The basic function to control modes is set_control_mode() but this function does not appear in the interface of the class: it must be declared and implemented in sub classes.

Each sub class interface should following the pattern:

class MyCIA402Device : public coe::cia402::CIA402Device {
    struct OpaqueStateCommands;
    std::unique_ptr<OpaqueStateCommands> sta_cmd_;

    static int8_t control_mode_to_code(control_mode_t);
    static control_mode_t control_code_to_mode(int8_t);

public:
    MyCIA402Device();

    static constexpr int8_t VeryStrangeModeCode = 105;
    enum class control_mode_t {
        monitor,
        profile_position,
        profile_velocity,
        profile_torque,
        cyclic_position,
        cyclic_velocity,
        cyclic_torque,
        very_strange_mode
    };

    /**
     * @brief Select the desired control mode.
     * @param [in] control_mode is the desired control mode
     * @return true if mode has changed, false otherwise
     */
    [[nodiscard]] bool set_control_mode(control_mode_t control_mode);

    /**
     * @brief Get current control mode used inside device
     * @return the current control mode
     */
    control_mode_t control_mode() const;

    using operation_state_t = CIA402Device::state_t;

    // functionalitities that depend on control mode
    void set_target_position(double target_position);
    double position() const;
    ...
};

The device specific driver class must inherit from coe::cia402::CIA402Device. We recommend using an opaque type and unique_pter to implement a pimpl for the internal state of the device, as explained in CoE tutorial.

An enum defines all possible control modes and set_control_mode() allows to change the control mode and control_mode() allows to know what control mode is currently set inside the device. Other functions (e.g. set_target_position and position) are simply used to set commands and monitor device state depending on the control mode currenlty used.

Implementation of the two core functions looks like:

bool MyCIA402Device::set_control_mode(control_mode_t control_mode) {
    if (not can_set_control_mode()) {
        return false;
    }
    sta_cmd_->control_mode_ = control_mode_to_code(control_mode);
    return true;
}

MyCIA402Device::control_mode_t MyCIA402Device::control_mode() const {
    return control_code_to_mode(filter_control_mode());
}

The two functions filter_control_mode() and can_set_control_mode() are provided by CIA402Device class.

  • filter_control_mode() simpy return the current mode of the device as an integer. This data is filtered in the sense that it is not always the direct value of the control mode in device. This function should be used wheever a device want to know its current control mode.
  • can_set_control_mode() is used to check if mode can trully be changed in device, and should be called prior to any action performed in the set_control_mode function.

The two functions control_code_to_mode and control_mode_to_code are internal utilities whose role is to convert the raw integer value representing the mode in CIA402Device and the enum representation in the device specific class. Something like:

MyCIA402Device::control_mode_t MyCIA402Device::control_code_to_mode(int8_t code) {
    switch (code) {
    case CIA402Device::PP_mode:
        return control_mode_t::profile_position;
    case CIA402Device::PV_mode:
        return control_mode_t::profile_velocity;
    case CIA402Device::PT_mode:
        return control_mode_t::profile_torque;
    case CIA402Device::CSP_mode:
        return control_mode_t::cyclic_position;
    case CIA402Device::CSV_mode:
        return control_mode_t::cyclic_velocity;
    case CIA402Device::CST_mode:
        return control_mode_t::cyclic_torque;
    case MyCIA402Device::VeryStrangeModeCode:
        return control_mode_t::very_strange_mode;
    default:
        return control_mode_t::monitor;
    }
}
//same logic but reverse in control_mode_to_code()

Why such a pattern ? The answer is quite simple: the CiA402 stanard does not force a device to implement all standard modes (even if their code is reserved) and they can also provide completely new modes (e.g. the very_strange_mode in our example). This pattern allows great flexibility while preserving a common way to deal with modes. Using an enum to define device specific control modes also ensures that the user will not give values that are not consistent with the device.

modes specific functions

The CIA402Device class also provide a set of utility functions that are usefull depending on the mode used:

  • target_reached() tells whether the target has been reached in profile modes.
  • manage_new_setpoint() allows to force the motor driver to take into account a new target position in profile position mode.
  • new_setpoint_following() tells whether a new set point is tracked in profile position mode.
  • motion_halted() tells whether motion generated by a profile mode is halted.
  • halt_motion() allows to pause or restart the motion generated by a profile mode.
  • internal_limit() tells whether an internal limit has been reached (max velocity) in profile mode.

CoE specific configuration

As a CoE device a CiA 402 device needs to be correctly configured at construction time :

configure_at_init([this, options]() {
    // pdo maps configuration/assignment
    // Alway reset fault before start the init step
    reset_fault();
    // other configruation actions
});

It is recommended to always use reset_fault() during the initial configuration to avoid a device in a faulty state to be blocked in this state.

set_quick_stop_reaction([this] {
    // situation where the quick stop is launched => automatic procedure
    // do specific additional action here when needed
});
set_leaving_enable_reaction([this] {
    if (std::abs(velocity()) < options_.velocity_threshold) { // standstill
        set_operation_state(control_t::disable_voltage);
    } else { // launch a quick stop to avoid activate brakes at too high
                // velocity
        quick_stop();
    }
});

User can customize some very specific semi-automatic behavior of te device using set_quick_stop_reaction and set_leaving_enable_reaction. The first one is used to perform specific actions during a quick_stop_active state. The second allows the user to performs some actions before leaving the operation_enabled state. In the example we use it to automatically launch a quick_stop reaction if the motor is not standstill.

Finally, we have to provide ethercat variables used to control state machine and mode during init phase:

struct OpaqueStateCommands{
    uint16_t control_word_;
    int8_t control_mode_;
    uint16_t status_word_;
    int8_t operation_mode_;
    //other device specific variables
};

MyCIA402Device::MyCIA402Device()
    : coe::cia402::CIA402Device(),
      sta_cmd_{std::make_unique<OpaqueStateCommands>()} {
    //configruation
}

add_init_step(
    [this]() {
        //do whatever needed, e.g. binding CoE buffers
        initialize_state_variables(
            reinterpret_cast<uint8_t*>(&sta_cmd_->control_word_),
            reinterpret_cast<uint8_t*>(&sta_cmd_->status_word_),
            reinterpret_cast<uint8_t*>(&sta_cmd_->control_mode_),
            reinterpret_cast<uint8_t*>(&sta_cmd_->operation_mode_));
        put_device_in_safe_state_at_init();
    },
    [this]() {
      // do whatever needed
    });

 add_end_step(
        [this]() {
            put_device_in_safe_state_at_end();
            //....
        },
        [this]() { //... 
        });

The call to initialize_state_variables is mandatory to make the CIA402Device working. The base class needs to access to data fields that contains CiA 402 standard data : control and status word to control the state machine and control mode reading/writing to set the control mode. initialize_state_variables simply pass the variables defined in the child class to the base class that trully use them. The reason why those variables are not put into the CIA402Device class is to let the user having a maximum flexibility on the way those variables are defined : they can be for instance defined as references to buffer, as explained in CoE utilities tutorial.

Finally put_device_in_safe_state_at_init() and put_device_in_safe_state_at_end() are used to starup/shutdown the device in safe state.

Conclusion

The benefit of using the CIA402Device class as a base pattern for drivers of CiA 402 device is:

  • to share a common way of controllling the state machine, with same API.
  • to avoid bad implementations of the CiA 402 state machine.
  • to automate all aspects of CiA 402 device as far as possible, so reducing code required to be written for each specific device.