\ No newline at end of file
diff --git a/ln.ethercat.service/www/static/pages/slave.html b/ln.ethercat.service/www/static/pages/slave.html
index 3c06145..e1174de 100644
--- a/ln.ethercat.service/www/static/pages/slave.html
+++ b/ln.ethercat.service/www/static/pages/slave.html
@@ -7,6 +7,9 @@
@selected="$ECAPP.subscribe(slave_id, $event);"
>
-
+
\ No newline at end of file
diff --git a/ln.ethercat/ECDataTypeConverter.cs b/ln.ethercat/ECDataTypeConverter.cs
index 15de7d3..2665381 100644
--- a/ln.ethercat/ECDataTypeConverter.cs
+++ b/ln.ethercat/ECDataTypeConverter.cs
@@ -159,7 +159,7 @@ namespace ln.ethercat
));
AddConverter(ECDataTypes.NONE, new ECValueConverterAdapter(
- typeof(byte[]),
+ typeof(string),
(object value, out byte[] ethercatBytes) => { ethercatBytes = Extensions.BytesFromHexString(value as string); return true; },
(byte[] ethercatBytes, out object value) => { value = ethercatBytes.ToHexString(); return true; }
));
diff --git a/ln.ethercat/ECMBind.cs b/ln.ethercat/ECMBind.cs
index e0850a3..6f9f513 100644
--- a/ln.ethercat/ECMBind.cs
+++ b/ln.ethercat/ECMBind.cs
@@ -41,7 +41,7 @@ namespace ln.ethercat
}
public delegate void cb_enum_indeces(int slave, int index);
- public delegate void cb_enum_sdo_descriptors(int slave, int index, ECDataTypes dataType, ECObjectCodes objectCode, int maxsub, String name);
+ public delegate void cb_enum_sdo_descriptors(int slave, int index, ECDataTypes dataType, ECObjectCodes objectCode, int maxsub, ushort bitlength, String name);
public delegate void cb_enum_pdo(UInt16 slave, UInt16 index, byte subindex, int addr_offset, int addr_bit, int bitlength);
@@ -106,6 +106,12 @@ namespace ln.ethercat
[DllImport("lib/libecmbind.so")]
public static extern int ecmbind_sdo_write(int slave, int index, int subindex, byte[] buffer, int size);
+ [DllImport("lib/libecmbind.so")]
+ public static extern int ecmbind_sdo_read_ca(int slave, int index, int subindex, byte[] buffer, int size);
+
+ [DllImport("lib/libecmbind.so")]
+ public static extern int ecmbind_sdo_write_ca(int slave, int index, int subindex, byte[] buffer, int size);
+
[DllImport("lib/libecmbind.so")]
diff --git a/ln.ethercat/ECMaster.cs b/ln.ethercat/ECMaster.cs
index ce352a1..f0711bf 100644
--- a/ln.ethercat/ECMaster.cs
+++ b/ln.ethercat/ECMaster.cs
@@ -1,13 +1,13 @@
using System;
-using System.Buffers;
using System.Collections.Generic;
+using System.Diagnostics;
using System.IO;
using System.Linq;
-using System.Reflection.Metadata;
-using System.Security.Cryptography.X509Certificates;
using System.Threading;
+using ln.ethercat.controller;
+using ln.ethercat.controller.drives;
using ln.logging;
using ln.type;
@@ -21,6 +21,23 @@ namespace ln.ethercat
RUNNING,
}
+ public struct PDOMappingRequest
+ {
+ public ushort Slave;
+ public ushort Index;
+ public byte SubIndex;
+ public bool RxPDO;
+
+ public PDOMappingRequest(ushort slave,ushort index,byte subIndex,bool rxpdo)
+ {
+ Slave = slave;
+ Index = index;
+ SubIndex = subIndex;
+ RxPDO = rxpdo;
+ }
+
+ }
+
public delegate void ECStateChange(ECMaster sender,ECSlaveState newState);
public class ECMaster
@@ -47,8 +64,8 @@ namespace ln.ethercat
ECSlaveState ethercatState = ECSlaveState.NONE;
public ECSlaveState EthercatState {
- get => UpdateEthercatState();
- set {
+ get =>ethercatState;
+ private set {
if (value != ethercatState)
{
OnStateChange?.Invoke(this, value);
@@ -73,113 +90,168 @@ namespace ln.ethercat
Logging.Log(LogLevel.INFO, "ecmbind_initialize({0}) = {1}", interfaceName, result);
if (result<=0)
throw new Exception("ecmbind_initialize failed");
+
+ Controller = new Controller(this);
}
- bool stopProcessing;
- public bool StopProccessing {
- get => stopProcessing;
- set => stopProcessing = value;
- }
+ bool stopWatchdog;
+ bool stopProcessData;
Thread threadProcessData;
Thread threadWatchdog;
- public bool Start()
+ public Controller Controller { get; }
+ List driveControllers = new List();
+ public DriveController[] DriveControllers => driveControllers.ToArray();
+
+
+ public void Start()
{
- if ((threadProcessData?.IsAlive ?? false) || (threadWatchdog?.IsAlive ?? false))
- throw new Exception("already started");
-
- stopProcessing = false;
-
- EthercatState = ECSlaveState.BOOT;
- ExpectedWorkCounter = 0;
-
- EthercatState = ECSlaveState.INIT;
lock (this)
{
- CountSlaves = ECMBind.ecmbind_config_init();
- if (CountSlaves <= 0)
- {
- Logging.Log(LogLevel.DEBUG, "ECMaster: no slaves connected");
- return false;
- }
+ if (threadWatchdog?.IsAlive ?? false)
+ throw new Exception("already started");
- if (!CheckState(ECSlaveState.PRE_OP, TIMEOUT_PREOP))
- return false;
+ ethercatState = ECSlaveState.NONE;
+ ExpectedWorkCounter = 0;
- IOMapSize = ECMBind.ecmbind_config_map();
- Logging.Log(LogLevel.DEBUG, "ECMaster: IOMapSize={0}", IOMapSize);
-
- UpdatePDOMap();
-
- threadProcessData = new Thread(MasterThread);
- threadProcessData.Start();
-
- Thread.Sleep(20);
-
- if (!RequestState(ECSlaveState.SAFE_OP, out ECSlaveState slaveState, TIMEOUT_SAFEOP))
- {
- Stop();
- return false;
- }
+ stopWatchdog = false;
threadWatchdog = new Thread(Watchdog);
threadWatchdog.Start();
-
}
- return true;
}
public void Stop()
{
- if (threadProcessData?.IsAlive ?? false)
+ lock (this)
{
- stopProcessing = true;
- ECMBind.ecmbind_request_state(0, ECSlaveState.SAFE_OP, TIMEOUT_BACKTO_SAFEOP);
-
- threadProcessData.Join();
- threadProcessData = null;
-
- if (!Thread.CurrentThread.Equals(threadWatchdog))
+ if (threadWatchdog?.IsAlive ?? false)
{
- threadWatchdog?.Join();
- threadWatchdog = null;
+ stopWatchdog = true;
+
+ lock (this)
+ Monitor.PulseAll(this);
+
+ threadWatchdog.Join();
}
}
}
-
-
- object lockIOMap = new object();
- byte[] iomap = new byte[8192];
void Watchdog()
{
- // if (!ReadSDOIndeces())
- // {
- // Logging.Log(LogLevel.WARNING, "ECMaster: could not read SDO indeces");
- // }
-
- if (!RequestState(ECSlaveState.OPERATIONAL, out ECSlaveState slaveState, TIMEOUT_SAFEOP))
+ while (!stopWatchdog)
+ {
+ IOMapSize = 0;
+ if (threadProcessData?.IsAlive ?? false)
{
- if (slaveState < ECSlaveState.SAFE_OP)
- Stop();
+ stopProcessData = true;
+ while (threadProcessData.IsAlive)
+ threadProcessData.Join(250);
+ }
+ stopProcessData = false;
+
+ EthercatState = ECSlaveState.INIT;
+
+ CountSlaves = ECMBind.ecmbind_config_init();
+ if (CountSlaves <= 0)
+ {
+ Logging.Log(LogLevel.DEBUG, "ECMaster: no slaves connected, scheduling restart...");
+ Thread.Sleep(2500);
+ continue;
}
- while (!stopProcessing)
+
+ if (!WaitForState(ECSlaveState.PRE_OP, TIMEOUT_PREOP))
{
- UpdateEthercatState();
- Thread.Sleep(250);
+ Logging.Log(LogLevel.DEBUG, "ECMaster: slaves did not reach PRE_OP. restart...");
+ Thread.Sleep(2500);
+ continue;
}
+
+
+ /* Bus State PRE_OP */
+ ScanControllers();
+
+ EthercatState = ECSlaveState.PRE_OP;
+
+ ConfigureProcessDataMappings();
+ SetupPDOMapping();
+ UpdatePDOMap();
+
+ threadProcessData = new Thread(ProcessData);
+ threadProcessData.Start();
+
+ Thread.Sleep(20);
+ RequestSlaveState(0, ECSlaveState.SAFE_OP);
+
+ if (!WaitForState(ECSlaveState.SAFE_OP, TIMEOUT_SAFEOP))
+ {
+ Logging.Log(LogLevel.DEBUG, "ECMaster: slaves did not reach SAFE_OP. restart...");
+ Thread.Sleep(2500);
+ continue;
+ }
+
+ EthercatState = ECSlaveState.SAFE_OP;
+
+ Thread.Sleep(20);
+ RequestSlaveState(0, ECSlaveState.OPERATIONAL);
+
+ Thread.Sleep(250);
+ if (!WaitForState(ECSlaveState.OPERATIONAL, TIMEOUT_SAFEOP))
+ {
+ Logging.Log(LogLevel.DEBUG, "ECMaster: slaves did not reach OPERATIONAL. restart...");
+ Thread.Sleep(2500);
+ continue;
+ }
+
+ EthercatState = ECSlaveState.OPERATIONAL;
+
+ /***** we handle the controller cycle from here on *****/
+
+ Controller.Initialize();
+
+ DateTime nextControllerRun = DateTime.Now;
+ while (!stopWatchdog)
+ {
+ EthercatState = IOLocked(()=>ECMBind.ecmbind_read_state());
+ if (EthercatState != ECSlaveState.OPERATIONAL)
+ break;
+
+ DateTime currentTime = DateTime.Now;
+ nextControllerRun += TimeSpan.FromSeconds(Controller.ControllerLoopInterval);
+ if (currentTime > nextControllerRun)
+ {
+ Logging.Log(LogLevel.WARNING, "missed controller cycle");
+ while (currentTime > nextControllerRun)
+ nextControllerRun += TimeSpan.FromSeconds(Controller.ControllerLoopInterval);
+ }
+ TimeSpan timeToNextCycle = nextControllerRun - currentTime;
+ try
+ {
+ Thread.Sleep(timeToNextCycle);
+ Controller.Cycle();
+ } catch (Exception e)
+ {
+ Logging.Log(e);
+ }
+ }
+
+ }
+
}
- void MasterThread()
+ void ProcessData()
{
- ExpectedWorkCounter = ECMBind.ecmbind_get_expected_wkc_size();
+ int wkc;
- while (!stopProcessing)
+ lock (lockIOMap)
+ ExpectedWorkCounter = ECMBind.ecmbind_get_expected_wkc_size();
+
+ Logging.Log(LogLevel.INFO, "ECMaster: ProcessData(): start");
+
+ while (!stopProcessData)
{
- int wkc;
-
lock (lockIOMap)
{
wkc = ECMBind.ecmbind_processdata2(iomap, iomap.Length);
@@ -187,26 +259,160 @@ namespace ln.ethercat
{
int success = ECMBind.ecmbind_recover();
if (success > 0)
- ExpectedWorkCounter = success;
- else
{
- RequestSlaveState(0, ECSlaveState.SAFE_OP);
+ Logging.Log(LogLevel.ERROR, "ECMaster: bus recovery successfull...");
+ ExpectedWorkCounter = success;
+ //SetupPDOMapping();
+ } else
+ {
+ RequestSlaveState(0, ECSlaveState.INIT);
+ Logging.Log(LogLevel.ERROR, "ECMaster: bus recovery failed, scheduling restart...");
+ ScheduleRestart();
+ stopProcessData = true;
}
}
}
Thread.Sleep(INTERVALL_PROCESSDATA);
+ }
-/*
- if (wkc != ExpectedWorkCounter)
+ Logging.Log(LogLevel.INFO, "ECMaster: ProcessData(): finished");
+ }
+
+ public void ScanControllers()
+ {
+ driveControllers.Clear();
+
+ for (UInt16 slave_id = 1; slave_id <= CountSlaves; slave_id++)
+ {
+ if (ReadSDO(slave_id, 0x1000, 0, out byte[] bDeviceType))
{
- ExpectedWorkCounter = ECMBind.ecmbind_recover();
- if (ExpectedWorkCounter < 0)
- break;
- }
-*/
+ ushort profilecode = BitConverter.ToUInt16(bDeviceType);
+ Logging.Log(LogLevel.INFO, "ECMaster: ScanControllers: Slave {0} has DeviceType {1} [{2}]", slave_id, profilecode, bDeviceType.ToHexString());
+ switch (profilecode)
+ {
+ case 402:
+ driveControllers.Add(new CIA402Controller(this, slave_id));
+ break;
+ }
+ }
}
}
+
+
+ bool WaitForState(ECSlaveState expectedState, int timeout)
+ {
+ int waitIncrement = 100;
+
+ ECSlaveState currentState = IOLocked(()=>ECMBind.ecmbind_read_state());
+ while ((timeout > 0) && (currentState != expectedState))
+ {
+ Thread.Sleep(waitIncrement);
+ timeout -= waitIncrement;
+ currentState = IOLocked(()=>ECMBind.ecmbind_read_state());
+ }
+ return currentState == expectedState;
+ }
+
+
+
+ object lockIOMap = new object();
+ byte[] iomap = new byte[8192];
+
+ void SetupPDOMapping()
+ {
+ IOMapSize = ECMBind.ecmbind_config_map();
+ Logging.Log(LogLevel.DEBUG, "ECMaster: IOMapSize={0}", IOMapSize);
+ //UpdatePDOMap();
+ }
+
+ HashSet mappingRequests = new HashSet();
+
+ public void ConfigureProcessDataMappings()
+ {
+ Logging.Log(LogLevel.INFO, "configure PDO mappings");
+
+ for (UInt16 slave = 1; slave <= CountSlaves; slave++)
+ {
+ List rxPDOs = new List();
+ List txPDOs = new List();
+
+ foreach (PDOMappingRequest mappingRequest in mappingRequests)
+ {
+ if (mappingRequest.Slave == slave)
+ {
+ if (GetSDOValue(mappingRequest.Slave, mappingRequest.Index, mappingRequest.SubIndex, out SDOValue sdoValue))
+ {
+ int mappingEntry = (mappingRequest.Index << 16) | (mappingRequest.SubIndex << 8) | sdoValue.BitLength;
+ Logging.Log(LogLevel.DEBUG, "PDO mapping: {0:X8}", mappingEntry);
+
+ if (mappingRequest.RxPDO)
+ rxPDOs.Add(mappingEntry);
+ else
+ txPDOs.Add(mappingEntry);
+
+ } else {
+ Logging.Log(LogLevel.WARNING, "ECMaster: SetupProcessDataMappings(): could not retrieve SDOValue {0}:{1:X4}.{2}", mappingRequest.Slave, mappingRequest.Index, mappingRequest.SubIndex);
+ }
+ }
+ }
+
+ MemoryStream rxStream = new MemoryStream();
+ rxStream.WriteByte((byte)rxPDOs.Count);
+ rxStream.WriteByte(0);
+ foreach (int pdoMapping in rxPDOs)
+ rxStream.WriteBytes(pdoMapping.GetBytes());
+
+ Logging.Log(LogLevel.DEBUG, "RxPDO mappings for slave {0} => {1}", slave, rxPDOs.Count);
+
+ if (!WriteSDOCA(slave, 0x1600, 0, rxStream.ToArray()))
+ {
+ Logging.Log(LogLevel.WARNING, "ECMaster: SetupProcessDataMappings(): could not write RxPDO mappings for slave {0}", slave);
+ }
+
+ MemoryStream txStream = new MemoryStream();
+ txStream.WriteByte((byte)txPDOs.Count);
+ txStream.WriteByte(0);
+ foreach (int pdoMapping in txPDOs)
+ txStream.WriteBytes(pdoMapping.GetBytes());
+
+ Logging.Log(LogLevel.DEBUG, "TxPDO mappings for slave {0} => {1}", slave, txPDOs.Count);
+
+ if (!WriteSDOCA(slave, 0x1A00, 0, txStream.ToArray()))
+ {
+ Logging.Log(LogLevel.WARNING, "ECMaster: SetupProcessDataMappings(): could not write TxPDO mappings for slave {0}", slave);
+ }
+
+ if (!WriteSDOCA(slave, 0x1c12, 0, new byte[]{ 0x01, 0x00, 0x00, 0x16 }))
+ {
+ Logging.Log(LogLevel.WARNING, "ECMaster: SetupProcessDataMappings(): could not configure syncmanager 2 PDO assignment for slave {0}", slave);
+ }
+ if (!WriteSDOCA(slave, 0x1c13, 0, new byte[]{ 0x01, 0x00, 0x00, 0x1A }))
+ {
+ Logging.Log(LogLevel.WARNING, "ECMaster: SetupProcessDataMappings(): could not configure syncmanager 3 PDO assignment for slave {0}", slave);
+ }
+ }
+ }
+
+ public void RequestPDOMapping(UInt16 slave,UInt16 index,byte subIndex, bool RxPDO) => RequestPDOMapping(new PDOMappingRequest(slave,index,subIndex,RxPDO));
+ public void RequestPDOMapping(IEnumerable mappingRequests) => RequestPDOMapping(mappingRequests.ToArray());
+ public void RequestPDOMapping(params PDOMappingRequest[] mappingRequests)
+ {
+ foreach (PDOMappingRequest mappingRequest in mappingRequests)
+ this.mappingRequests.Add(mappingRequest);
+ }
+
+
+ void ScheduleRestart()
+ {
+ ThreadPool.QueueUserWorkItem((o)=>{
+ Stop();
+ Thread.Sleep(500);
+ Start();
+ });
+ }
+
+
public bool ReadSDO(UInt16 slave,UInt16 index, byte subIndex, out byte[] rawValue)
{
@@ -220,7 +426,27 @@ namespace ln.ethercat
ECMBind.ecmbind_sdo_read(slave, index, subIndex, rawValue, rawValue.Length );
if (dataSize <= 0)
{
- Logging.Log(LogLevel.ERROR, "ECMBind.ecmbind_sdo_read({0},{1},{2},..,{3}) == {4}", slave, index, subIndex, rawValue.Length, rawValue.ToHexString());
+ Logging.Log(LogLevel.ERROR, "ECMBind.ecmbind_sdo_read({0},{1},{2},..,{3}) [{5}] == {4}", slave, index, subIndex, rawValue.Length, rawValue.ToHexString(), dataSize);
+ throw new IOException();
+ }
+ }
+ rawValue = rawValue.Slice(0, dataSize);
+ return true;
+ }
+
+ public bool ReadSDOCA(UInt16 slave,UInt16 index, byte subIndex, out byte[] rawValue)
+ {
+ rawValue =
+ new byte[128];
+ int dataSize;
+
+ lock (lockIOMap)
+ {
+ dataSize =
+ ECMBind.ecmbind_sdo_read_ca(slave, index, subIndex, rawValue, rawValue.Length );
+ if (dataSize <= 0)
+ {
+ Logging.Log(LogLevel.ERROR, "ECMBind.ecmbind_sdo_read_ca({0},{1},{2},..,{3}) == {4}", slave, index, subIndex, rawValue.Length, rawValue.ToHexString());
throw new IOException();
}
}
@@ -233,41 +459,20 @@ namespace ln.ethercat
int result;
lock (lockIOMap)
result = ECMBind.ecmbind_sdo_write(slave, index, subIndex, rawValue, rawValue.Length);
- Logging.Log(LogLevel.DEBUG, "ECMaster: WriteSDO({0},{1},{2},{3}) => {4}", slave, index, subIndex, rawValue.ToHexString(), result);
+ //Logging.Log(LogLevel.DEBUG, "ECMaster: WriteSDO({0},{1},{2},{3}) => {4}", slave, index, subIndex, rawValue.ToHexString(), result);
+ return result > 0;
+ }
+ public bool WriteSDOCA(UInt16 slave,UInt16 index, byte subIndex, byte[] rawValue)
+ {
+ int result;
+ lock (lockIOMap)
+ result = ECMBind.ecmbind_sdo_write_ca(slave, index, subIndex, rawValue, rawValue.Length);
+ //Logging.Log(LogLevel.DEBUG, "ECMaster: WriteSDOCA({0},{1},{2},{3}) => {4}", slave, index, subIndex, rawValue.ToHexString(), result);
return result > 0;
}
- public ECSlaveState ReadSlaveState(int slave) => ECMBind.ecmbind_get_slave_state(slave);
-
- private ECSlaveState UpdateEthercatState()
- {
- EthercatState = ECMBind.ecmbind_read_state();
- return ethercatState;
- }
-
- public bool CheckState(ECSlaveState minimumState, int timeout)
- {
- for (; timeout > 0; timeout -= 10)
- {
- ECSlaveState state = ECMBind.ecmbind_read_state();
- if (state >= minimumState)
- {
- EthercatState = state;
- return true;
- }
- Thread.Sleep(10);
- }
- return false;
- }
- public bool RequestState(ECSlaveState requestedState, out ECSlaveState reachedState, int timeout)
- {
- reachedState = ECMBind.ecmbind_request_state(0, requestedState, timeout);
- Logging.Log(LogLevel.DEBUG, "ECMaster.RequestState({1}): lowest slave state: {0}", reachedState, requestedState);
- EthercatState = reachedState;
- return (reachedState >= requestedState);
- }
-
- public int RequestSlaveState(int slave, ECSlaveState slaveState) => ECMBind.ecmbind_write_slave_state(slave, slaveState);
+ public ECSlaveState ReadSlaveState(int slave) => IOLocked(()=>ECMBind.ecmbind_get_slave_state(slave));
+ int RequestSlaveState(int slave, ECSlaveState slaveState) => IOLocked(()=>ECMBind.ecmbind_write_slave_state(slave, slaveState));
public bool GetSlave(UInt16 slave_id, out ECSlave slave)
@@ -297,6 +502,7 @@ namespace ln.ethercat
if ((offset >= 0) && (length > 0) && (offset+length < iomap.Length))
{
rawData = iomap.Slice(offset, length);
+ //Logging.Log(LogLevel.ERROR, "GetIOmapData({0},{1})", offset, length);
return true;
}
Logging.Log(LogLevel.ERROR, "GetIOmapData({0},{1}) failed", offset, length);
@@ -308,7 +514,7 @@ namespace ln.ethercat
lock (lockIOMap)
if ((offset >= 0) && (length > 0) && (offset+length < iomap.Length))
{
- Logging.Log(LogLevel.DEBUG, "ECMaster: SetIOmapData({0},{1},{2})", offset, length, rawData.ToHexString());
+ //Logging.Log(LogLevel.DEBUG, "ECMaster: SetIOmapData({0},{1},{2})", offset, length, rawData.ToHexString());
Buffer.BlockCopy(rawData, 0, iomap, offset, length);
return true;
}
@@ -334,18 +540,19 @@ namespace ln.ethercat
public void UpdatePDOMap()
{
List pdoList = new List();
- ECMBind.ecmbind_pdo_enumerate((UInt16 slave, UInt16 index, byte subindex, int addr_offset, int addr_bit, int bitlength)=>{
- if (addr_bit != 0)
- Logging.Log(LogLevel.WARNING, "currently only PDO mappings on byte boundaries are supported");
- else
- pdoList.Add(new PDO(this,slave, index, subindex){
- AddressOffset = addr_offset,
- AddressBit = addr_bit,
- BitLength = bitlength
- });
+ lock (lockIOMap)
+ ECMBind.ecmbind_pdo_enumerate((UInt16 slave, UInt16 index, byte subindex, int addr_offset, int addr_bit, int bitlength)=>{
+ if (addr_bit != 0)
+ Logging.Log(LogLevel.WARNING, "currently only PDO mappings on byte boundaries are supported");
+ else
+ pdoList.Add(new PDO(this,slave, index, subindex){
+ AddressOffset = addr_offset,
+ AddressBit = addr_bit,
+ BitLength = bitlength
+ });
});
- lock (this)
+ lock (lockIOMap)
{
IOMapPtr = ECMBind.ecmbind_get_iomap();
pdoMap = pdoList;
diff --git a/ln.ethercat/ECSlave.cs b/ln.ethercat/ECSlave.cs
index 9341952..f0a0b05 100644
--- a/ln.ethercat/ECSlave.cs
+++ b/ln.ethercat/ECSlave.cs
@@ -58,7 +58,7 @@ namespace ln.ethercat
public void Update()
{
- if (ECMBind.ecmbind_read_objectdescription(Slave.Id, Index, (int slave, int index, ECDataTypes dataType, ECObjectCodes objectCode, int maxsub, String name)=>{
+ if (ECMBind.ecmbind_read_objectdescription(Slave.Id, Index, (int slave, int index, ECDataTypes dataType, ECObjectCodes objectCode, int maxsub, ushort bitlength, String name)=>{
Name = name;
DataType = dataType;
ObjectCode = objectCode;
@@ -105,6 +105,7 @@ namespace ln.ethercat
{
public SDODescriptor Descriptor;
public byte SubIndex { get; }
+ public ushort BitLength { get; private set; }
public string Name { get; private set; }
public ECDataTypes DataType { get; private set; }
@@ -114,13 +115,14 @@ namespace ln.ethercat
Descriptor = descriptor;
SubIndex = subIndex;
- if (SubIndex == 0)
+ if ((Descriptor.MaxSubIndex > 0) && (SubIndex == 0))
{
DataType = descriptor.DataType;
Name = descriptor.Name;
} else {
- if (ECMBind.ecmbind_read_objectdescription_entry(Descriptor.SlaveId, descriptor.Index, SubIndex, (int slave, int index, ECDataTypes dataType, ECObjectCodes objectCode, int si, String name)=>{
- Name = name;
+ if (ECMBind.ecmbind_read_objectdescription_entry(Descriptor.SlaveId, descriptor.Index, SubIndex, (int slave, int index, ECDataTypes dataType, ECObjectCodes objectCode, int si, ushort bitlength, String name)=>{
+ Name = (SubIndex == 0) ? descriptor.Name : name;
+ BitLength = bitlength;
DataType = dataType;
}) <= 0)
{
diff --git a/ln.ethercat/controller/Controller.cs b/ln.ethercat/controller/Controller.cs
index e044875..b9e82cc 100644
--- a/ln.ethercat/controller/Controller.cs
+++ b/ln.ethercat/controller/Controller.cs
@@ -1,4 +1,5 @@
+using System;
using System.Collections.Generic;
using System.Threading;
using ln.ethercat.controller.drives;
@@ -8,98 +9,225 @@ namespace ln.ethercat.controller
{
public delegate void ControllerLogicDelegate(Controller controller);
+ public delegate void ControllerStateChangeDelegate(Controller controller, ControllerStates newState);
+
+ public enum ControllerStates {
+ NONE,
+ NOTREADY,
+ FAULT,
+ READY,
+ DISABLING,
+ ENABLING,
+ OPERATIONAL
+ }
public class Controller
{
public event ControllerLogicDelegate ControllerLogic;
+ public event ControllerStateChangeDelegate OnStateChanging;
+ public event ControllerStateChangeDelegate OnStateChanged;
+ public ECMaster ECMaster { get; }
- List driveControllers = new List();
- public DriveController[] DriveControllers => driveControllers.ToArray();
+ public bool IgnoreRemoteInterface { get; set; }
List controlLoops = new List();
public ControlLoop[] ControlLoops => controlLoops.ToArray();
- public bool IsRunning => threadController?.IsAlive ?? false;
+ public ControllerStates ControllerState { get; private set; }
public double ControllerLoopInterval { get; set; } = 0.1;
public double ControllerLoopFrequency {
get => 1.0 / ControllerLoopInterval;
set => ControllerLoopInterval = 1.0 / value;
}
+ public long CycleCounter { get; private set; }
- bool stopRequested;
- Thread threadController;
+ Thread threadWatchdog;
+ int wdogCounter;
+ public int WatchdogReset { get; set; } = 5;
- public Controller()
+ public Controller(ECMaster ecMaster)
{
+ ECMaster = ecMaster;
}
- public void Start()
- {
- if (threadController?.IsAlive ?? false)
- {
- Logging.Log(LogLevel.WARNING, "Controller; Start(): already started");
- } else {
- stopRequested = false;
-
- foreach (DriveController driveController in driveControllers)
- driveController.Initialize();
-
- threadController = new Thread(ControllerThread);
- threadController.Start();
- }
- }
-
- public void Stop()
- {
- if (threadController?.IsAlive ?? false)
- {
- stopRequested = true;
- threadController.Join();
- threadController = null;
- }
- }
-
- public void Add(DriveController driveController) => driveControllers.Add(driveController);
- public void Remove(DriveController driveController) => driveControllers.Remove(driveController);
-
public void Add(ControlLoop controlLoop) => controlLoops.Add(controlLoop);
public void Remove(ControlLoop controlLoop) => controlLoops.Remove(controlLoop);
- public DriveStates DrivesState {
- get {
- DriveStates lowestState = DriveStates.OPERATIONAL;
- foreach (DriveController driveController in driveControllers)
- {
- DriveStates driveState = driveController.DriveState;
- if (driveState < lowestState)
- lowestState = driveState;
- }
- return lowestState;
- }
- }
-
- void ControllerThread()
+ public void Initialize()
{
- while (!stopRequested)
+ CycleCounter = 0;
+
+ if (!(threadWatchdog?.IsAlive ?? false))
{
- foreach (DriveController driveController in driveControllers)
- driveController.UpdateStates();
+ threadWatchdog = new Thread(Watchdog);
+ threadWatchdog.Start();
+ }
- foreach (ControlLoop controlLoop in controlLoops)
- controlLoop.Loop();
+ foreach (DriveController driveController in ECMaster.DriveControllers)
+ driveController.Initialize();
- ControllerLogic?.Invoke(this);
+ }
- foreach (DriveController driveController in driveControllers)
- driveController.UpdateDrive();
+ void UpdateControllerState()
+ {
+ ControllerStates nextState = ControllerStates.OPERATIONAL;
- Thread.Sleep((int)(1000.0 * ControllerLoopInterval));
+ foreach (DriveController driveController in ECMaster.DriveControllers)
+ {
+ if (!driveController.IgnoredByController)
+ {
+ ControllerStates driveState;
+ switch (driveController.DriveState)
+ {
+ case DriveStates.OPERATIONAL:
+ driveState = ControllerStates.OPERATIONAL;
+ break;
+ case DriveStates.ERROR:
+ driveState = ControllerStates.FAULT;
+ break;
+ case DriveStates.INIT:
+ case DriveStates.BOOT:
+ driveState = ControllerStates.NOTREADY;
+ break;
+ default:
+ driveState = ControllerStates.READY;
+ break;
+ }
+ if (driveState < nextState)
+ nextState = driveState;
+ }
+ }
+ ChangeState(nextState);
+ }
+
+ void ChangeState(ControllerStates newState)
+ {
+ if (newState == ControllerState)
+ return;
+
+ OnStateChanging?.Invoke(this, newState);
+
+ switch (newState)
+ {
+ case ControllerStates.FAULT:
+ DisableDrives();
+ break;
+ case ControllerStates.DISABLING:
+ foreach (DriveController driveController in ECMaster.DriveControllers)
+ driveController.DisableDrive();
+ break;
+ case ControllerStates.ENABLING:
+ foreach (DriveController driveController in ECMaster.DriveControllers)
+ if (!driveController.IgnoredByController)
+ driveController.EnableDrive();
+ break;
+ }
+
+ ControllerState = newState;
+ OnStateChanged?.Invoke(this, newState);
+ }
+
+ public void DisableDrives()
+ {
+ ChangeState(ControllerStates.DISABLING);
+ }
+
+ public void EnableDrives()
+ {
+ if (ControllerState == ControllerStates.READY)
+ {
+ ChangeState(ControllerStates.ENABLING);
+ } else {
+ Logging.Log(LogLevel.INFO, "Controller: EnableDrives(): Current ControllerState={0}. Refusing to enable drives", ControllerState.ToString());
+ }
+ }
+
+ public void ClearFaults()
+ {
+ foreach (DriveController driveController in ECMaster.DriveControllers)
+ {
+ if (driveController.DriveState == DriveStates.ERROR)
+ driveController.ClearFault();
+ }
+ }
+
+ public void RemoteAction(CRActions action)
+ {
+ if (!IgnoreRemoteInterface)
+ {
+ switch (action)
+ {
+ case CRActions.CLEARFAULT:
+ ClearFaults();
+ break;
+ case CRActions.DISABLE:
+ DisableDrives();
+ break;
+ case CRActions.ENABLE:
+ EnableDrives();
+ break;
+ }
+ }
+ }
+
+ public void RemoteUpdateTarget(int drive, double targetValue)
+ {
+ if (!IgnoreRemoteInterface)
+ {
+ targetValue = Math.Clamp(targetValue, 0, 1);
+
+ if (drive < 0)
+ throw new ArgumentOutOfRangeException(nameof(targetValue));
+ if (drive > ECMaster.DriveControllers.Length)
+ {
+ Logging.Log(LogLevel.WARNING, "Controller: RemoteUpdateTarget(): trying to update non-existent drive {0}", drive);
+ return;
+ }
+
+ ECMaster.DriveControllers[drive].TargetValue = targetValue;
}
}
+ public void Cycle()
+ {
+ wdogCounter = WatchdogReset;
+ CycleCounter++;
+
+ foreach (DriveController driveController in ECMaster.DriveControllers)
+ driveController.UpdateStates();
+
+ UpdateControllerState();
+
+ foreach (ControlLoop controlLoop in controlLoops)
+ controlLoop.Loop();
+
+ ControllerLogic?.Invoke(this);
+
+ foreach (DriveController driveController in ECMaster.DriveControllers)
+ driveController.UpdateDrive();
+ }
+
+ void Watchdog()
+ {
+ while (true)
+ {
+ try
+ {
+ Thread.Sleep(TimeSpan.FromSeconds(ControllerLoopInterval));
+ if ((wdogCounter--) < 0)
+ {
+ ChangeState(ControllerStates.FAULT);
+ }
+ } catch (Exception e)
+ {
+ Logging.Log(e);
+ }
+ }
+
+ }
}
}
\ No newline at end of file
diff --git a/ln.ethercat/controller/ControllerRemote.cs b/ln.ethercat/controller/ControllerRemote.cs
new file mode 100644
index 0000000..7236748
--- /dev/null
+++ b/ln.ethercat/controller/ControllerRemote.cs
@@ -0,0 +1,105 @@
+
+
+using System;
+using System.Threading;
+using ln.ethercat.controller.drives;
+using ln.logging;
+
+namespace ln.ethercat.controller
+{
+ public enum CRActions {
+ NONE, // No Action
+ ENABLE, // Enable system (turn on)
+ DISABLE, // Disable system (turn off)
+ CLEARFAULT, // Clear fault state
+ }
+
+ public abstract class ControllerRemote
+ {
+ protected Controller Controller;
+
+ public double CycleIntervall { get; set; }
+ public double CycleFrequency {
+ get => 1.0 / CycleIntervall;
+ set => CycleIntervall = 1.0 / value;
+ }
+
+ public long CycleCounter { get; private set; }
+
+ public bool IsRunning => threadCycle?.IsAlive ?? false;
+
+ bool stopCycleThread;
+ Thread threadCycle;
+
+ public ControllerRemote(Controller controller)
+ {
+ Controller = controller;
+ }
+
+ public void Start()
+ {
+ lock (this)
+ {
+ if (threadCycle?.IsAlive ?? false)
+ Stop();
+
+ threadCycle = new Thread(CycleThread);
+ threadCycle.Start();
+ }
+ }
+
+ public void Stop()
+ {
+ lock (this)
+ {
+ if (threadCycle?.IsAlive ?? false)
+ {
+ stopCycleThread = true;
+ threadCycle.Join(250);
+ if (threadCycle.IsAlive)
+ threadCycle.Abort();
+ threadCycle = null;
+ }
+ }
+ }
+
+ public void SetTargetValue(int drive, double targetValue) => Controller.RemoteUpdateTarget( drive, targetValue );
+ public void Action(CRActions action) => Controller.RemoteAction(action);
+
+ protected abstract void Initialize();
+ protected abstract void Cycle();
+ protected abstract void Shutdown();
+
+ private void CycleThread()
+ {
+ CycleCounter = 0;
+ Initialize();
+
+ DateTime nextCycleRun = DateTime.Now;
+ while (!stopCycleThread)
+ {
+ DateTime currentTime = DateTime.Now;
+ nextCycleRun += TimeSpan.FromSeconds(CycleIntervall);
+ if (currentTime > nextCycleRun)
+ {
+ Logging.Log(LogLevel.WARNING, "ControllerRemote: missed controller cycle");
+ while (currentTime > nextCycleRun)
+ nextCycleRun += TimeSpan.FromSeconds(CycleIntervall);
+ }
+ TimeSpan timeToNextCycle = nextCycleRun - currentTime;
+ try
+ {
+ Thread.Sleep(timeToNextCycle);
+ CycleCounter++;
+ Cycle();
+ } catch (Exception e)
+ {
+ Logging.Log(e);
+ }
+ }
+
+ Shutdown();
+ }
+
+ }
+}
\ No newline at end of file
diff --git a/ln.ethercat/controller/drives/CIA402Controller.cs b/ln.ethercat/controller/drives/CIA402Controller.cs
index 43ed13e..cd9f04e 100644
--- a/ln.ethercat/controller/drives/CIA402Controller.cs
+++ b/ln.ethercat/controller/drives/CIA402Controller.cs
@@ -41,11 +41,30 @@ namespace ln.ethercat.controller.drives
SDOValue svActualTorque;
SDOValue svModesOfOperation;
SDOValue svModesOfOperationDisplay;
+ SDOValue svActualCurrent;
+ 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)
+ };
+ ecMaster.RequestPDOMapping(mappingRequests);
}
public override void Initialize()
@@ -61,23 +80,82 @@ namespace ln.ethercat.controller.drives
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, 0x6061, 0, out svModesOfOperationDisplay) &&
+ ECMaster.GetSDOValue(Slave, 0x6080, 0, out svMaxSpeed) &&
+ ECMaster.GetSDOValue(Slave, 0x6078, 0, out svActualCurrent)
))
throw new ArgumentOutOfRangeException("CIA402Controller could not retrieve SDOvalues for CiA402 profile");
}
- public override void UpdateStates() { }
- public override void UpdateDrive() { }
+ public override void UpdateStates() {
+ CIA402State = GetCIA402State();
+ }
+ public override void UpdateDrive() {
+
+ if (CIA402State != CIA402TargetState)
+ {
+ switch (CIA402State)
+ {
+ 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;
+ }
+ }
+
+ if (DriveMode == DriveMode.UNDEFINED)
+ DriveMode = DriveMode.TORQUE;
+ }
public override DriveStates DriveState
{
get {
- switch (GetCIA402State())
+ 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.INIT;
+ return DriveStates.READY;
case CIA402States.SWITCHED_ON:
case CIA402States.QUICK_STOP_ACTIVE:
return DriveStates.POWERED;
@@ -93,7 +171,7 @@ namespace ln.ethercat.controller.drives
}
public override string OEMDriveState {
- get => GetCIA402State().ToString();
+ get => CIA402State.ToString();
}
public override string OEMDriveMode => ModeOfOperation.ToString();
@@ -135,26 +213,27 @@ namespace ln.ethercat.controller.drives
public void SetDriveModeTorque() => DriveMode = DriveMode.TORQUE;
public CIA402ModesOfOperation ModeOfOperation {
- get => (CIA402ModesOfOperation)(svModesOfOperationDisplay.GetValue());
+ get => (ECMaster.EthercatState==ECSlaveState.OPERATIONAL) ? (CIA402ModesOfOperation)(svModesOfOperationDisplay?.GetValue() ?? 0) : CIA402ModesOfOperation.NO_MODE_CHANGE;
set => svModesOfOperation.SetValue((sbyte)value);
}
- public override decimal ActualPosition => svActualPosition.GetValue();
- public override decimal ActualSpeed => svActualSpeed.GetValue();
- public override decimal ActualTorque => svActualTorque.GetValue();
+ public override double ActualPosition => (svActualPosition?.GetValue() ?? 0) ;
+ public override double ActualSpeed => (double)(svActualSpeed?.GetValue() ?? 0) / (1000.0 * MotorMaxSpeed);
+ public override double ActualTorque => (double)(svActualTorque?.GetValue() ?? 0) / 1000.0;
+ public override double ActualLoad => (double)(svActualCurrent?.GetValue() ?? 0) / 1000.0;
- public override decimal TargetPosition {
+ public override double TargetPosition {
get => svTargetPosition.GetValue();
set => svTargetPosition.SetValue((int)value);
}
- public override decimal TargetSpeed {
- get => svTargetSpeed.GetValue();
- set => svTargetSpeed.SetValue((int)value);
+ public override double TargetSpeed {
+ get => (double)svTargetSpeed.GetValue() / (1000.0 * MotorMaxSpeed);
+ set => svTargetSpeed.SetValue((int)(value * (1000.0 * MotorMaxSpeed)));
}
- public override decimal TargetTorque {
- get => svTargetTorque.GetValue();
- set => svTargetTorque.SetValue((short)value);
+ public override double TargetTorque {
+ get => (double)svTargetTorque.GetValue() / 1000.0;
+ set => svTargetTorque.SetValue((short)(1000 * value));
}
public override int ErrorCode => svErrorCode.GetValue();
@@ -163,51 +242,12 @@ namespace ln.ethercat.controller.drives
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;
- }
- }
+ CIA402TargetState = enable ? CIA402States.OPERATION_ENABLED : CIA402States.READY_TO_SWITCH_ON;
}
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);
- }
+ CIA402TargetState = poweron ? CIA402States.SWITCHED_ON : CIA402States.READY_TO_SWITCH_ON;
}
public override void ClearFault()
@@ -220,10 +260,12 @@ namespace ln.ethercat.controller.drives
}
}
- CIA402States CIA402State => GetCIA402State();
+ public CIA402States CIA402State { get; private set; }
+ public CIA402States CIA402TargetState { get; private set; }
+
public CIA402States GetCIA402State()
{
- UInt16 statusword = svStatusWord.GetValue();
+ UInt16 statusword = svStatusWord?.GetValue() ?? 0;
if ((statusword & 0x004F)==0)
return CIA402States.NOT_READY_TO_SWITCH_ON;
diff --git a/ln.ethercat/controller/drives/DriveController.cs b/ln.ethercat/controller/drives/DriveController.cs
index 97120a8..427e8b8 100644
--- a/ln.ethercat/controller/drives/DriveController.cs
+++ b/ln.ethercat/controller/drives/DriveController.cs
@@ -1,6 +1,7 @@
using System;
+using System.Net.Http.Headers;
using System.Runtime;
namespace ln.ethercat.controller.drives
@@ -9,6 +10,7 @@ namespace ln.ethercat.controller.drives
UNDEFINED,
BOOT,
INIT,
+ READY,
ERROR,
POWERED,
OPERATIONAL,
@@ -26,6 +28,9 @@ namespace ln.ethercat.controller.drives
protected ECMaster ECMaster { get; }
public UInt16 Slave { get; }
+ public bool IgnoredByController { get; set; }
+
+
public DriveController(ECMaster ecMaster, UInt16 slave)
{
ECMaster = ecMaster;
@@ -57,13 +62,46 @@ namespace ln.ethercat.controller.drives
public void DisableDrive() => Enable(false);
public abstract void Enable(bool enabled);
- public abstract decimal ActualPosition { get; }
- public abstract decimal ActualSpeed { get; }
- public abstract decimal ActualTorque { get; }
- public abstract decimal TargetPosition { get; set; }
- public abstract decimal TargetSpeed { get; set; }
- public abstract decimal TargetTorque { get; set; }
+ public abstract double ActualPosition { get; }
+ public abstract double ActualSpeed { get; }
+ public abstract double ActualTorque { get; }
+ public abstract double TargetPosition { get; set; }
+ public abstract double TargetSpeed { get; set; }
+ public abstract double TargetTorque { get; set; }
+
+ public abstract double ActualLoad { get; }
+ public virtual double TargetValue {
+ get {
+ switch (DriveMode)
+ {
+ case DriveMode.POSITION:
+ return TargetPosition;
+ case DriveMode.SPEED:
+ return TargetSpeed;
+ case DriveMode.TORQUE:
+ return TargetTorque;
+ default:
+ return 0;
+ }
+ }
+ set {
+ switch (DriveMode)
+ {
+ case DriveMode.POSITION:
+ TargetPosition = value;
+ break;
+ case DriveMode.SPEED:
+ TargetSpeed = value;
+ break;
+ case DriveMode.TORQUE:
+ TargetTorque = value;
+ break;
+ default:
+ break;
+ }
+ }
+ }
}
diff --git a/ln.ethercat/controller/remote/StupidSerialRemote.cs b/ln.ethercat/controller/remote/StupidSerialRemote.cs
new file mode 100644
index 0000000..977f23a
--- /dev/null
+++ b/ln.ethercat/controller/remote/StupidSerialRemote.cs
@@ -0,0 +1,171 @@
+
+
+using System;
+using System.IO.Ports;
+using System.Net.Http.Headers;
+using System.Text;
+using System.Threading;
+using ln.logging;
+using ln.type;
+
+namespace ln.ethercat.controller.remote
+{
+ /*
+#define LED_ERROR 0x00
+#define LED_RUN 0x01
+#define LED_LOAD25 0x02
+#define LED_LOAD50 0x03
+#define LED_LOAD75 0x04
+#define LED_LOAD100 0x05
+#define LED_SERVICE 0x06
+#define LED_AUX 0x07
+*/
+
+ [Flags]
+ public enum StupidLEDs : int
+ {
+ NONE = 0,
+ ERROR = (1<<15),
+ RUN = (1<<1),
+ LOAD25 = (1<<2),
+ LOAD50 = (1<<3),
+ LOAD75 = (1<<4),
+ LOAD100 = (1<<15),
+ ALL = -1
+ }
+ public class StupidSerialRemote : ControllerRemote
+ {
+ public string SerialPortName { get; }
+
+ SerialPort serialPort;
+
+ bool stopReceiverThread;
+ Thread threadReceiver;
+
+ public StupidSerialRemote(Controller controller)
+ :this(controller, SerialPort.GetPortNames()[0]){}
+ public StupidSerialRemote(Controller controller, string serialDevice)
+ :base(controller)
+ {
+ SerialPortName = serialDevice;
+ CycleFrequency = 10.0;
+ }
+
+ protected override void Cycle()
+ {
+ byte cycleDisplayStep = (byte)(CycleCounter & 0x0F);
+
+ switch (Controller.ControllerState)
+ {
+ case ControllerStates.NOTREADY:
+ SetLEDs(((cycleDisplayStep & 0x07) < 0x04) ? StupidLEDs.ALL : StupidLEDs.NONE);
+ break;
+ case ControllerStates.FAULT:
+ SetLEDs(((cycleDisplayStep & 0x07) < 0x04) ? StupidLEDs.ERROR : StupidLEDs.NONE);
+ break;
+ case ControllerStates.READY:
+ SetLEDs((cycleDisplayStep < 0x08) ? StupidLEDs.RUN : StupidLEDs.NONE);
+ break;
+ case ControllerStates.OPERATIONAL:
+ StupidLEDs leds = StupidLEDs.RUN;
+ if (Controller.ECMaster.DriveControllers[0].ActualLoad >= 0.25)
+ leds |= StupidLEDs.LOAD25;
+ if (Controller.ECMaster.DriveControllers[0].ActualLoad >= 0.5)
+ leds |= StupidLEDs.LOAD50;
+ if (Controller.ECMaster.DriveControllers[0].ActualLoad >= 0.75)
+ leds |= StupidLEDs.LOAD75;
+ if (Controller.ECMaster.DriveControllers[0].ActualLoad >= 1)
+ leds |= StupidLEDs.LOAD100;
+ SetLEDs(leds);
+ break;
+ case ControllerStates.ENABLING:
+ break;
+ case ControllerStates.DISABLING:
+ break;
+ }
+ }
+
+ void Receiver()
+ {
+ while (!stopReceiverThread)
+ {
+ string rxLine = serialPort.ReadLine();
+ //Logging.Log(LogLevel.DEBUGDETAIL, rxLine);
+ if (rxLine.Length >= 6)
+ {
+ ushort av = ushort.Parse(rxLine.Substring(2,4), System.Globalization.NumberStyles.HexNumber);
+ switch (rxLine[0])
+ {
+ case 'A':
+ int drive = rxLine[1] - '0';
+ if (drive == 0) drive = 1;
+ else if (drive == 1) drive = 2;
+ else if (drive == 2) drive = 0;
+ double rel = (double)av / 65535;
+ if ((drive >= 0) && (drive < Controller.ECMaster.DriveControllers.Length))
+ Controller.RemoteUpdateTarget(drive, rel);
+ break;
+ case 'B':
+ switch (rxLine[1])
+ {
+ case 'P':
+ switch (av)
+ {
+ case 0:
+ if (Controller.ControllerState == ControllerStates.FAULT)
+ Controller.RemoteAction(CRActions.CLEARFAULT);
+ else if (Controller.ControllerState == ControllerStates.READY)
+ Controller.RemoteAction(CRActions.ENABLE);
+ break;
+ case 1:
+ Controller.RemoteAction(CRActions.DISABLE);
+ break;
+ case 3:
+ // ToDo: Feeder left
+ break;
+ case 4:
+ // ToDo: Feeder right
+ break;
+ case 14:
+ // ToDo: cycle drilling
+ break;
+ }
+ break;
+ }
+ break;
+ }
+ }
+ }
+ }
+
+ void SetLEDs(StupidLEDs leds)
+ {
+ serialPort.Write(string.Format("LS{0:X4}\r\n", (ushort)leds));
+ }
+
+
+ protected override void Initialize()
+ {
+ stopReceiverThread = false;
+
+ serialPort = new SerialPort(SerialPortName);
+ serialPort.BaudRate = 57600;
+ serialPort.Parity = Parity.None;
+ serialPort.DataBits = 8;
+ serialPort.StopBits = StopBits.One;
+ serialPort.Open();
+
+ if (!(threadReceiver?.IsAlive ?? false))
+ {
+ threadReceiver = new Thread(Receiver);
+ threadReceiver.Start();
+ }
+ }
+
+ protected override void Shutdown()
+ {
+ stopReceiverThread = true;
+ serialPort.Close();
+ }
+ }
+}
\ No newline at end of file
diff --git a/ln.ethercat/lib/libecmbind.so b/ln.ethercat/lib/libecmbind.so
index fcb6537..512b119 100755
Binary files a/ln.ethercat/lib/libecmbind.so and b/ln.ethercat/lib/libecmbind.so differ
diff --git a/ln.ethercat/ln.ethercat.csproj b/ln.ethercat/ln.ethercat.csproj
index 4a5a2af..8fe3848 100644
--- a/ln.ethercat/ln.ethercat.csproj
+++ b/ln.ethercat/ln.ethercat.csproj
@@ -16,6 +16,7 @@
+