326 lines
14 KiB
C#
326 lines
14 KiB
C#
|
|
|
|
using System;
|
|
using System.Diagnostics;
|
|
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;
|
|
SDOValue svActualCurrent;
|
|
SDOValue svVLTargetVelocity;
|
|
SDOValue svVLActualVelocity;
|
|
|
|
SDOValue svMaxSpeed;
|
|
|
|
UInt32 MotorMaxSpeed = 1000;
|
|
|
|
public CIA402Controller(ECMaster ecMaster,UInt16 slave)
|
|
:base(ecMaster, slave)
|
|
{
|
|
PDOMappingRequest[] mappingRequests = new PDOMappingRequest[]{
|
|
new PDOMappingRequest(slave, 0x603F, 0, false),
|
|
new PDOMappingRequest(slave, 0x6040, 0, true),
|
|
new PDOMappingRequest(slave, 0x6041, 0, false),
|
|
new PDOMappingRequest(slave, 0x607A, 0, true),
|
|
new PDOMappingRequest(slave, 0x60FF, 0, true),
|
|
new PDOMappingRequest(slave, 0x6071, 0, true),
|
|
new PDOMappingRequest(slave, 0x6064, 0, false),
|
|
new PDOMappingRequest(slave, 0x606C, 0, false),
|
|
new PDOMappingRequest(slave, 0x6077, 0, false),
|
|
new PDOMappingRequest(slave, 0x6060, 0, true),
|
|
new PDOMappingRequest(slave, 0x6061, 0, false),
|
|
new PDOMappingRequest(slave, 0x6078, 0, false),
|
|
new PDOMappingRequest(slave, 0x6042, 0, true),
|
|
new PDOMappingRequest(slave, 0x6044, 0, false),
|
|
};
|
|
ecMaster.RequestPDOMapping(mappingRequests);
|
|
}
|
|
|
|
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) &&
|
|
ECMaster.GetSDOValue(Slave, 0x6080, 0, out svMaxSpeed) &&
|
|
ECMaster.GetSDOValue(Slave, 0x6044, 0, out svVLActualVelocity) &&
|
|
ECMaster.GetSDOValue(Slave, 0x6042, 0, out svVLTargetVelocity) &&
|
|
ECMaster.GetSDOValue(Slave, 0x6078, 0, out svActualCurrent)
|
|
))
|
|
throw new ArgumentOutOfRangeException("CIA402Controller could not retrieve SDOvalues for CiA402 profile");
|
|
|
|
MotorMaxSpeed = svMaxSpeed.GetValue<UInt32>();
|
|
}
|
|
|
|
public override void UpdateStates() {
|
|
CIA402State = GetCIA402State();
|
|
}
|
|
public override void UpdateDrive()
|
|
{
|
|
if (CIA402TargetState != CIA402States.UNDEFINED)
|
|
{
|
|
if (CIA402State != CIA402TargetState)
|
|
{
|
|
switch (CIA402State)
|
|
{
|
|
case CIA402States.FAULT:
|
|
case CIA402States.FAULT_REACTION_ACTIVE:
|
|
CIA402TargetState = CIA402States.UNDEFINED;
|
|
break;
|
|
case CIA402States.NOT_READY_TO_SWITCH_ON:
|
|
svControlWord.SetValue((ushort)0x0000);
|
|
ModeOfOperation = CIA402ModesOfOperation.CYCLIC_SYNC_TORQUE;
|
|
break;
|
|
case CIA402States.SWITCH_ON_DISABLED:
|
|
svControlWord.SetValue((ushort)0x0006);
|
|
break;
|
|
case CIA402States.READY_TO_SWITCH_ON:
|
|
switch (CIA402TargetState)
|
|
{
|
|
case CIA402States.SWITCH_ON_DISABLED:
|
|
svControlWord.SetValue((ushort)0x0000);
|
|
break;
|
|
case CIA402States.SWITCHED_ON:
|
|
case CIA402States.OPERATION_ENABLED:
|
|
svControlWord.SetValue((ushort)0x0007);
|
|
break;
|
|
}
|
|
break;
|
|
case CIA402States.SWITCHED_ON:
|
|
switch (CIA402TargetState)
|
|
{
|
|
case CIA402States.SWITCH_ON_DISABLED:
|
|
case CIA402States.READY_TO_SWITCH_ON:
|
|
svControlWord.SetValue((ushort)0x0006);
|
|
break;
|
|
case CIA402States.OPERATION_ENABLED:
|
|
svControlWord.SetValue((ushort)0x000F);
|
|
break;
|
|
}
|
|
break;
|
|
case CIA402States.OPERATION_ENABLED:
|
|
switch (CIA402TargetState)
|
|
{
|
|
case CIA402States.SWITCH_ON_DISABLED:
|
|
case CIA402States.READY_TO_SWITCH_ON:
|
|
svControlWord.SetValue((ushort)0x0006);
|
|
break;
|
|
case CIA402States.SWITCHED_ON:
|
|
svControlWord.SetValue((ushort)0x0007);
|
|
break;
|
|
}
|
|
break;
|
|
}
|
|
} else {
|
|
CIA402TargetState = CIA402States.UNDEFINED;
|
|
}
|
|
}
|
|
|
|
if (DriveMode == DriveMode.UNDEFINED)
|
|
DriveMode = DriveMode.SPEED;
|
|
}
|
|
|
|
public override DriveStates DriveState
|
|
{
|
|
get {
|
|
switch (CIA402State)
|
|
{
|
|
case CIA402States.NOT_READY_TO_SWITCH_ON:
|
|
return DriveStates.INIT;
|
|
case CIA402States.SWITCH_ON_DISABLED:
|
|
case CIA402States.READY_TO_SWITCH_ON:
|
|
return DriveStates.READY;
|
|
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 => CIA402State.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:
|
|
case CIA402ModesOfOperation.VL_VELOCITY_MODE:
|
|
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.VL_VELOCITY_MODE;
|
|
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 => (ECMaster.EthercatState==ECSlaveState.OPERATIONAL) ? (CIA402ModesOfOperation)(svModesOfOperationDisplay?.GetValue<sbyte>() ?? 0) : CIA402ModesOfOperation.NO_MODE_CHANGE;
|
|
set {
|
|
svModesOfOperation.SetValue((sbyte)value);
|
|
}
|
|
}
|
|
|
|
public override double ActualPosition => (svActualPosition?.GetValue<int>() ?? 0) ;
|
|
public override double ActualSpeed {
|
|
get {
|
|
if (ModeOfOperation == CIA402ModesOfOperation.CYCLIC_SYNC_VELOCITY)
|
|
return (double)(svActualSpeed?.GetValue<int>() ?? 0) / (1000.0 * MotorMaxSpeed);
|
|
return (double)(svVLActualVelocity?.GetValue<short>() ?? 0) / MotorMaxSpeed;
|
|
}
|
|
}
|
|
public override double ActualTorque => (double)(svActualTorque?.GetValue<short>() ?? 0) / 1000.0;
|
|
public override double ActualLoad => (double)(svActualCurrent?.GetValue<short>() ?? 0) / 1000.0;
|
|
|
|
public override double TargetPosition {
|
|
get => svTargetPosition?.GetValue<int>() ?? 0;
|
|
set => svTargetPosition.SetValue((int)value);
|
|
}
|
|
|
|
public override double TargetSpeed {
|
|
get {
|
|
if (ModeOfOperation == CIA402ModesOfOperation.CYCLIC_SYNC_VELOCITY)
|
|
return (double)(svTargetSpeed?.GetValue<int>() ?? 0) / (1000.0 * MotorMaxSpeed);
|
|
return (double)(svVLTargetVelocity?.GetValue<short>() ?? 0) / MotorMaxSpeed;
|
|
}
|
|
set {
|
|
if (ModeOfOperation == CIA402ModesOfOperation.CYCLIC_SYNC_VELOCITY)
|
|
svTargetSpeed.SetValue((int)(value * (1000.0 * MotorMaxSpeed)));
|
|
svVLTargetVelocity.SetValue((short)(value * MotorMaxSpeed));
|
|
}
|
|
}
|
|
public override double TargetTorque {
|
|
get => (double)(svTargetTorque?.GetValue<short>() ?? 0) / 1000.0;
|
|
set => svTargetTorque.SetValue((short)(1000 * value));
|
|
}
|
|
|
|
public override int ErrorCode => svErrorCode?.GetValue<ushort>() ?? 0;
|
|
|
|
public override string ErrorText => ErrorCode.ToString();
|
|
|
|
public override void Enable(bool enable)
|
|
{
|
|
CIA402TargetState = enable ? CIA402States.OPERATION_ENABLED : CIA402States.READY_TO_SWITCH_ON;
|
|
}
|
|
|
|
public override void Power(bool poweron)
|
|
{
|
|
CIA402TargetState = poweron ? CIA402States.SWITCHED_ON : CIA402States.READY_TO_SWITCH_ON;
|
|
}
|
|
|
|
public override void ClearFault()
|
|
{
|
|
switch (CIA402State)
|
|
{
|
|
case CIA402States.FAULT:
|
|
svControlWord.SetValue((UInt16)0x0086);
|
|
break;
|
|
}
|
|
}
|
|
|
|
public CIA402States CIA402State { get; private set; }
|
|
public CIA402States CIA402TargetState { get; private set; }
|
|
|
|
public CIA402States GetCIA402State()
|
|
{
|
|
UInt16 statusword = svStatusWord?.GetValue<ushort>() ?? 0;
|
|
|
|
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;
|
|
}
|
|
|
|
}
|
|
} |