ln.ethercat/ln.ethercat/controller/drives/CIA402Controller.cs

using System;
using ln.logging;

namespace ln.ethercat.controller.drives
{
    public enum CIA402States {
        UNDEFINED,
        NOT_READY_TO_SWITCH_ON,
        SWITCH_ON_DISABLED,
        READY_TO_SWITCH_ON,
        SWITCHED_ON,
        OPERATION_ENABLED,
        QUICK_STOP_ACTIVE,
        FAULT_REACTION_ACTIVE,
        FAULT
    }

    public enum CIA402ModesOfOperation : byte
    {
        NO_MODE_CHANGE = 0,
        VL_VELOCITY_MODE = 2,
        HOMING_MODE = 6,
        INTERPOLATED_POSITION_MODE = 7,
        CYCLIC_SYNC_POSITION = 8,
        CYCLIC_SYNC_VELOCITY = 9,
        CYCLIC_SYNC_TORQUE = 10
    }

    public class CIA402Controller : DriveController
    {
        SDOValue svControlWord;
        SDOValue svStatusWord;
        SDOValue svErrorCode;
        SDOValue svTargetPosition;
        SDOValue svTargetSpeed;
        SDOValue svTargetTorque;
        SDOValue svActualPosition;
        SDOValue svActualSpeed;
        SDOValue svActualTorque;
        SDOValue svModesOfOperation;
        SDOValue svModesOfOperationDisplay;

        
        public CIA402Controller(ECMaster ecMaster,UInt16 slave)
        :base(ecMaster, slave)
        {
        }

        public override void Initialize()
        {
            if (!(
                ECMaster.GetSDOValue(Slave, 0x603F, 0, out svErrorCode) &&
                ECMaster.GetSDOValue(Slave, 0x6040, 0, out svControlWord) &&
                ECMaster.GetSDOValue(Slave, 0x6041, 0 , out svStatusWord) &&
                ECMaster.GetSDOValue(Slave, 0x607A ,0, out svTargetPosition) &&
                ECMaster.GetSDOValue(Slave, 0x60FF, 0, out svTargetSpeed) &&
                ECMaster.GetSDOValue(Slave, 0x6071, 0, out svTargetTorque) &&
                ECMaster.GetSDOValue(Slave, 0x6064, 0, out svActualPosition) &&
                ECMaster.GetSDOValue(Slave, 0x606C, 0, out svActualSpeed) &&
                ECMaster.GetSDOValue(Slave, 0x6077, 0, out svActualTorque) &&
                ECMaster.GetSDOValue(Slave, 0x6060, 0, out svModesOfOperation) &&
                ECMaster.GetSDOValue(Slave, 0x6061, 0, out svModesOfOperationDisplay)
            ))
                throw new ArgumentOutOfRangeException("CIA402Controller could not retrieve SDOvalues for CiA402 profile");
        }

        public override void UpdateStates() { }
        public override void UpdateDrive() { }

        public override DriveStates DriveState 
        {
            get {
                switch (GetCIA402State())
                {
                    case CIA402States.NOT_READY_TO_SWITCH_ON:
                    case CIA402States.SWITCH_ON_DISABLED:
                    case CIA402States.READY_TO_SWITCH_ON:
                        return DriveStates.INIT;
                    case CIA402States.SWITCHED_ON:
                    case CIA402States.QUICK_STOP_ACTIVE:
                        return DriveStates.POWERED;
                    case CIA402States.OPERATION_ENABLED:
                        return DriveStates.OPERATIONAL;
                    case CIA402States.FAULT_REACTION_ACTIVE:
                    case CIA402States.FAULT:
                        return DriveStates.ERROR;
                    default:
                        return DriveStates.UNDEFINED;
                }
            }
        }

        public override string OEMDriveState {
            get => GetCIA402State().ToString();
        }
        public override string OEMDriveMode => ModeOfOperation.ToString();

        public override DriveMode DriveMode {
            get {
                switch (ModeOfOperation)
                {
                    case CIA402ModesOfOperation.CYCLIC_SYNC_POSITION:
                        return DriveMode.POSITION;
                    case CIA402ModesOfOperation.CYCLIC_SYNC_VELOCITY:
                        return DriveMode.SPEED;
                    case CIA402ModesOfOperation.CYCLIC_SYNC_TORQUE:
                        return DriveMode.TORQUE;
                    default:
                        return DriveMode.UNDEFINED;
                }
            }
            set {
                switch (value)
                {
                    case DriveMode.POSITION:
                        ModeOfOperation = CIA402ModesOfOperation.CYCLIC_SYNC_POSITION;
                        break;
                    case DriveMode.SPEED:
                        ModeOfOperation = CIA402ModesOfOperation.CYCLIC_SYNC_VELOCITY;
                        break;
                    case DriveMode.TORQUE:
                        ModeOfOperation = CIA402ModesOfOperation.CYCLIC_SYNC_TORQUE;
                        break;
                    default:
                        Logging.Log(LogLevel.WARNING, "CIA402Controller: DriveMode {0} not supported", value);
                        break;
                }
            }
        }

        public void SetDriveModePosition() => DriveMode = DriveMode.POSITION;
        public void SetDriveModeSpeed() => DriveMode = DriveMode.SPEED;
        public void SetDriveModeTorque() => DriveMode = DriveMode.TORQUE;

        public CIA402ModesOfOperation ModeOfOperation {
            get => (CIA402ModesOfOperation)(svModesOfOperationDisplay.GetValue<sbyte>());
            set => svModesOfOperation.SetValue((sbyte)value);
        }

        public override decimal ActualPosition => svActualPosition.GetValue<int>();
        public override decimal ActualSpeed => svActualSpeed.GetValue<int>();
        public override decimal ActualTorque => svActualTorque.GetValue<short>();

        public override decimal TargetPosition {
            get => svTargetPosition.GetValue<int>();
            set => svTargetPosition.SetValue((int)value);
        }

        public override decimal TargetSpeed {
            get => svTargetSpeed.GetValue<int>();
            set => svTargetSpeed.SetValue((int)value);
        }
        public override decimal TargetTorque {
            get => svTargetTorque.GetValue<short>();
            set => svTargetTorque.SetValue((short)value);
        }

        public override int ErrorCode => svErrorCode.GetValue<int>();

        public override string ErrorText => ErrorCode.ToString();

        public override void Enable(bool enable)
        {
            if (enable)
            {
                switch (CIA402State)
                {
                    case CIA402States.SWITCHED_ON:
                        svControlWord.SetValue((UInt16)0x000F);
                        break;
                    default:
                        Logging.Log(LogLevel.WARNING, "CIA402Controller: EnableDrive(): current state is {0}, can't enable drive", CIA402State.ToString());
                        break;
                }
            } else {
                switch (CIA402State)
                {
                    case CIA402States.OPERATION_ENABLED:
                        svControlWord.SetValue((UInt16)0x0007);
                        break;
                    default:
                        Logging.Log(LogLevel.WARNING, "CIA402Controller: EnableDrive(): current state is {0}, can't disable drive", CIA402State.ToString());
                        break;
                }
            }
        }

        public override void Power(bool poweron)
        {
            if (poweron)
            {
                switch (CIA402State)
                {
                    case CIA402States.FAULT:
                    case CIA402States.FAULT_REACTION_ACTIVE:
                        Logging.Log(LogLevel.WARNING, "CIA402Controller: Power(): Drive in fault state, not ready to switch power on");
                        break;
                    case CIA402States.NOT_READY_TO_SWITCH_ON:
                        Logging.Log(LogLevel.WARNING, "CIA402Controller: Power(): Drive not ready to switch power on");
                        break;
                    case CIA402States.SWITCH_ON_DISABLED:    
                    case CIA402States.READY_TO_SWITCH_ON:
                        svControlWord.SetValue((UInt16)0x0007); // Switch ON
                        break;
                }
            } else {
                svControlWord.SetValue((UInt16)0x0006);
            }
        }

        public override void ClearFault()
        {
            switch (CIA402State)
            {
                case CIA402States.FAULT:
                    svControlWord.SetValue((UInt16)0x0086);
                    break;
            }
        }

        CIA402States CIA402State => GetCIA402State();
        public CIA402States GetCIA402State()
        {
            UInt16 statusword = svStatusWord.GetValue<ushort>();

            if ((statusword & 0x004F)==0)
                return CIA402States.NOT_READY_TO_SWITCH_ON;
            if ((statusword & 0x004F)==0x0040)
                return CIA402States.SWITCH_ON_DISABLED;
            if ((statusword & 0x006F)==0x0021)
                return CIA402States.READY_TO_SWITCH_ON;
            if ((statusword & 0x006F)==0x0023)
                return CIA402States.SWITCHED_ON;
            if ((statusword & 0x006F)==0x0027)
                return CIA402States.OPERATION_ENABLED;
            if ((statusword & 0x006F)==0x0007)
                return CIA402States.QUICK_STOP_ACTIVE;
            if ((statusword & 0x004F)==0x000F)
                return CIA402States.FAULT_REACTION_ACTIVE;
            if ((statusword & 0x004F)==0x0008)
                return CIA402States.FAULT;
                
            return CIA402States.UNDEFINED;
        }

    }
}