242 lines
8.9 KiB
C#
242 lines
8.9 KiB
C#
|
|
|
|
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
|
|
{
|
|
public readonly SDOAddr saControlWord;
|
|
public readonly SDOAddr saStatusWord;
|
|
public readonly SDOAddr saErrorCode;
|
|
public readonly SDOAddr saTargetPosition;
|
|
public readonly SDOAddr saTargetSpeed;
|
|
public readonly SDOAddr saTargetTorque;
|
|
public readonly SDOAddr saActualPosition;
|
|
public readonly SDOAddr saActualSpeed;
|
|
public readonly SDOAddr saActualTorque;
|
|
public readonly SDOAddr saModesOfOperation;
|
|
public readonly SDOAddr saModesOfOperationDisplay;
|
|
|
|
public CIA402Controller(ECMaster ecMaster,int slave)
|
|
:base(ecMaster, slave)
|
|
{
|
|
saErrorCode = new SDOAddr(slave, 0x603f);
|
|
saControlWord = new SDOAddr(slave, 0x6040);
|
|
saStatusWord = new SDOAddr(slave, 0x6041);
|
|
|
|
saTargetPosition = new SDOAddr(slave, 0x607A);
|
|
saTargetSpeed = new SDOAddr(slave, 0x60FF);
|
|
saTargetTorque = new SDOAddr(slave, 0x6071);
|
|
|
|
saActualPosition = new SDOAddr(slave, 0x6064);
|
|
saActualSpeed = new SDOAddr(slave, 0x606C);
|
|
saActualTorque = new SDOAddr(slave, 0x6077);
|
|
|
|
saModesOfOperation = new SDOAddr(slave, 0x6060);
|
|
saModesOfOperationDisplay = new SDOAddr(slave, 0x6061);
|
|
}
|
|
|
|
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 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_POSITION;
|
|
break;
|
|
case DriveMode.TORQUE:
|
|
ModeOfOperation = CIA402ModesOfOperation.CYCLIC_SYNC_POSITION;
|
|
break;
|
|
default:
|
|
Logging.Log(LogLevel.WARNING, "CIA402Controller: DriveMode {0} not supported", value);
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
public CIA402ModesOfOperation ModeOfOperation {
|
|
get => (CIA402ModesOfOperation)GetSDOValue<byte>(saModesOfOperationDisplay);
|
|
set => SetSDOValue<byte>(saModesOfOperation, (byte)value);
|
|
}
|
|
|
|
|
|
public override decimal ActualPosition => GetSDOValue<int>(saActualPosition);
|
|
public override decimal ActualSpeed => GetSDOValue<int>(saActualSpeed);
|
|
public override decimal ActualTorque => GetSDOValue<int>(saActualTorque);
|
|
|
|
public override decimal TargetPosition {
|
|
get => GetSDOValue<int>(saTargetPosition);
|
|
set => SetSDOValue(saTargetPosition, value);
|
|
}
|
|
|
|
public override decimal TargetSpeed {
|
|
get => GetSDOValue<int>(saTargetSpeed);
|
|
set => SetSDOValue(saTargetSpeed, value);
|
|
}
|
|
public override decimal TargetTorque {
|
|
get => GetSDOValue<int>(saTargetTorque);
|
|
set => SetSDOValue(saTargetTorque, value);
|
|
}
|
|
|
|
public override int ErrorCode => GetSDOValue<int>(saErrorCode);
|
|
|
|
public override string ErrorText => ErrorCode.ToString();
|
|
|
|
public override void EnableDrive(bool enable)
|
|
{
|
|
if (enable)
|
|
{
|
|
switch (CIA402State)
|
|
{
|
|
case CIA402States.SWITCHED_ON:
|
|
SetSDOValue(saControlWord, (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:
|
|
SetSDOValue(saControlWord, (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:
|
|
SetSDOValue(saControlWord, (UInt16)0x0007); // Switch ON
|
|
break;
|
|
}
|
|
} else {
|
|
SetSDOValue(saControlWord, (UInt16)0x0006);
|
|
}
|
|
}
|
|
|
|
public override void ClearFault()
|
|
{
|
|
switch (CIA402State)
|
|
{
|
|
case CIA402States.FAULT:
|
|
SetSDOValue(saControlWord, (UInt16)0x0086);
|
|
break;
|
|
}
|
|
}
|
|
|
|
CIA402States CIA402State => GetCIA402State();
|
|
public CIA402States GetCIA402State()
|
|
{
|
|
if (ECMaster.GetSDO(saStatusWord, out SDO sdoStatusWord))
|
|
{
|
|
UInt16 statusword = sdoStatusWord.GetValue<UInt16>();
|
|
|
|
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;
|
|
}
|
|
|
|
}
|
|
} |