Still early alpha, controller works, stupidremote working
ln.build - build0.waldrennach.l--n.de build job pending
Details
ln.build - build0.waldrennach.l--n.de build job pending
Details
parent
b62b9f6ee9
commit
9c054f3865
|
@ -27,7 +27,7 @@ typedef struct {
|
||||||
} dto_servicedescriptor_t;
|
} dto_servicedescriptor_t;
|
||||||
|
|
||||||
typedef void (*cb_enum_indeces_t)(int slave, int index);
|
typedef void (*cb_enum_indeces_t)(int slave, int index);
|
||||||
typedef void (*cb_enum_sdo_descriptors_t)(int slave, int index, uint16_t dataType, uint16_t objectCode, int sub, char *name);
|
typedef void (*cb_enum_sdo_descriptors_t)(int slave, int index, uint16_t dataType, uint16_t objectCode, int sub, ushort bitlength, char *name);
|
||||||
typedef void (*cb_enum_pdo)(uint16_t slave, uint16_t index, char subindex, int addr_offset, int addr_bit, int bitlength);
|
typedef void (*cb_enum_pdo)(uint16_t slave, uint16_t index, char subindex, int addr_offset, int addr_bit, int bitlength);
|
||||||
|
|
||||||
extern ecd_pdo_entry_t ecd_pdo_map[1024];
|
extern ecd_pdo_entry_t ecd_pdo_map[1024];
|
||||||
|
|
|
@ -181,6 +181,18 @@ int ecmbind_sdo_write(int slave, int index, int subindex, char* buffer, int size
|
||||||
return ec_SDOwrite(slave, index, subindex, FALSE, size, buffer, 250000L);
|
return ec_SDOwrite(slave, index, subindex, FALSE, size, buffer, 250000L);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
int ecmbind_sdo_read_ca(int slave, int index, int subindex, char* buffer, int size)
|
||||||
|
{
|
||||||
|
int wkc = ec_SDOread(slave, index, subindex, TRUE, &size, buffer, 250000L);
|
||||||
|
if (wkc <= 0)
|
||||||
|
return wkc;
|
||||||
|
return size;
|
||||||
|
}
|
||||||
|
int ecmbind_sdo_write_ca(int slave, int index, int subindex, char* buffer, int size)
|
||||||
|
{
|
||||||
|
return ec_SDOwrite(slave, index, subindex, TRUE, size, buffer, 250000L);
|
||||||
|
}
|
||||||
|
|
||||||
/*
|
/*
|
||||||
typedef void (*cb_enum_pdo)(int slave, int index, int subindex, int addr_offset, int addr_bit, int bitlength);
|
typedef void (*cb_enum_pdo)(int slave, int index, int subindex, int addr_offset, int addr_bit, int bitlength);
|
||||||
*/
|
*/
|
||||||
|
|
|
@ -189,7 +189,7 @@ int ecmbind_read_objectdescription(int Slave, int index, cb_enum_sdo_descriptors
|
||||||
strncpy(temp , (char *)&aSDOp->bdata[6], n);
|
strncpy(temp , (char *)&aSDOp->bdata[6], n);
|
||||||
temp[n] = 0x00;
|
temp[n] = 0x00;
|
||||||
|
|
||||||
cb(Slave, index, etohs(aSDOp->wdata[1]), aSDOp->bdata[5], aSDOp->bdata[4], temp);
|
cb(Slave, index, etohs(aSDOp->wdata[1]), aSDOp->bdata[5], aSDOp->bdata[4], 0, temp);
|
||||||
}
|
}
|
||||||
/* got unexpected response from slave */
|
/* got unexpected response from slave */
|
||||||
else
|
else
|
||||||
|
@ -265,7 +265,7 @@ int ecmbind_read_objectdescription_entry(uint16_t slave, uint16_t index, uint16_
|
||||||
strncpy(temp , (char *)&aSDOp->wdata[5], n);
|
strncpy(temp , (char *)&aSDOp->wdata[5], n);
|
||||||
temp[n] = 0x00; /* string terminator */
|
temp[n] = 0x00; /* string terminator */
|
||||||
|
|
||||||
cb(slave, index, (ushort)etohs(aSDOp->wdata[2]), 0, sub, temp);
|
cb(slave, index, (ushort)etohs(aSDOp->wdata[2]), 0, sub, (ushort)etohs(aSDOp->wdata[3]), temp);
|
||||||
|
|
||||||
/*
|
/*
|
||||||
pOElist->ValueInfo[sub] = aSDOp->bdata[3];
|
pOElist->ValueInfo[sub] = aSDOp->bdata[3];
|
||||||
|
|
|
@ -17,8 +17,6 @@ namespace ln.ethercat.service
|
||||||
{
|
{
|
||||||
|
|
||||||
public ECMaster ECMaster { get; private set; }
|
public ECMaster ECMaster { get; private set; }
|
||||||
public Controller Controller { get; private set; }
|
|
||||||
|
|
||||||
|
|
||||||
[StaticArgument(Option = 'i', LongOption = "interface")]
|
[StaticArgument(Option = 'i', LongOption = "interface")]
|
||||||
public string EthercatInterfaceName { get; set; }
|
public string EthercatInterfaceName { get; set; }
|
||||||
|
@ -72,14 +70,9 @@ namespace ln.ethercat.service
|
||||||
httpServer = new HTTPServer(httpLoggingRouter);
|
httpServer = new HTTPServer(httpLoggingRouter);
|
||||||
httpServer.AddEndpoint(new Endpoint(IPv6.ANY, 7676));
|
httpServer.AddEndpoint(new Endpoint(IPv6.ANY, 7676));
|
||||||
|
|
||||||
Controller = new Controller();
|
controllerApiController = new ControllerApiController(ECMaster.Controller);
|
||||||
Controller.Add(new CIA402Controller(ECMaster, 1));
|
|
||||||
Controller.Add(new CIA402Controller(ECMaster, 2));
|
|
||||||
|
|
||||||
controllerApiController = new ControllerApiController(Controller);
|
|
||||||
httpRouter.AddSimpleRoute("/api/v1/*", controllerApiController);
|
httpRouter.AddSimpleRoute("/api/v1/*", controllerApiController);
|
||||||
|
|
||||||
|
|
||||||
timerWebsockets = new System.Timers.Timer(250);
|
timerWebsockets = new System.Timers.Timer(250);
|
||||||
timerWebsockets.Elapsed += WebSocketTimerMethod;
|
timerWebsockets.Elapsed += WebSocketTimerMethod;
|
||||||
}
|
}
|
||||||
|
@ -97,6 +90,7 @@ namespace ln.ethercat.service
|
||||||
{
|
{
|
||||||
try{
|
try{
|
||||||
EthercatWebSocket.SendProcessData(ECMaster);
|
EthercatWebSocket.SendProcessData(ECMaster);
|
||||||
|
ControllerWebSocket.SendUpdates(ECMaster.Controller);
|
||||||
} catch (Exception ex)
|
} catch (Exception ex)
|
||||||
{
|
{
|
||||||
Logging.Log(ex);
|
Logging.Log(ex);
|
||||||
|
@ -108,10 +102,10 @@ namespace ln.ethercat.service
|
||||||
switch (newState)
|
switch (newState)
|
||||||
{
|
{
|
||||||
case ECSlaveState.OPERATIONAL:
|
case ECSlaveState.OPERATIONAL:
|
||||||
Controller.Start();
|
//Controller.Start();
|
||||||
break;
|
break;
|
||||||
default:
|
default:
|
||||||
Controller.Stop();
|
//Controller.Stop();
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
@ -0,0 +1,63 @@
|
||||||
|
|
||||||
|
using System;
|
||||||
|
using System.IO;
|
||||||
|
using ln.ethercat.controller;
|
||||||
|
using ln.json;
|
||||||
|
using ln.json.mapping;
|
||||||
|
|
||||||
|
namespace ln.ethercat.service
|
||||||
|
{
|
||||||
|
|
||||||
|
public class MainAxFeederControllerLogic
|
||||||
|
{
|
||||||
|
public MyParameters Parameters { get; set; }
|
||||||
|
|
||||||
|
ECMaster ECMaster;
|
||||||
|
|
||||||
|
SDOValue svRelais;
|
||||||
|
SDOValue svEnable;
|
||||||
|
|
||||||
|
public MainAxFeederControllerLogic(ECMaster ecMaster)
|
||||||
|
{
|
||||||
|
ECMaster = ecMaster;
|
||||||
|
|
||||||
|
Parameters = new MyParameters();
|
||||||
|
if (File.Exists("mafcl.json"))
|
||||||
|
{
|
||||||
|
JSONValue configValue = JSONParser.ParseFile("mafcl.json");
|
||||||
|
JSONMapper.DefaultMapper.Apply(configValue as JSONObject, Parameters);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
public void Save()
|
||||||
|
{
|
||||||
|
if (JSONMapper.DefaultMapper.Serialize(Parameters, out JSONValue configObject))
|
||||||
|
{
|
||||||
|
using (StreamWriter sw = new StreamWriter("mafcl.json"))
|
||||||
|
{
|
||||||
|
sw.Write(configObject.ToString());
|
||||||
|
sw.Flush();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
public void Initialize()
|
||||||
|
{
|
||||||
|
if (!(
|
||||||
|
ECMaster.GetSDOValue(1, 0x2012, 31, out svRelais) &&
|
||||||
|
ECMaster.GetSDOValue(1, 0x2012, 32, out svEnable)
|
||||||
|
))
|
||||||
|
throw new Exception("could not retrieve needed SDOValues");
|
||||||
|
}
|
||||||
|
|
||||||
|
public void ControllerLogic(Controller contreller)
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
public class MyParameters {
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
}
|
|
@ -1,9 +1,11 @@
|
||||||
using System;
|
using System;
|
||||||
|
using System.Data;
|
||||||
using System.Text;
|
using System.Text;
|
||||||
using System.Threading;
|
using System.Threading;
|
||||||
using ln.application;
|
using ln.application;
|
||||||
using ln.ethercat.controller;
|
using ln.ethercat.controller;
|
||||||
using ln.ethercat.controller.drives;
|
using ln.ethercat.controller.drives;
|
||||||
|
using ln.ethercat.controller.remote;
|
||||||
using ln.logging;
|
using ln.logging;
|
||||||
using ln.type;
|
using ln.type;
|
||||||
|
|
||||||
|
@ -11,6 +13,9 @@ namespace ln.ethercat.service
|
||||||
{
|
{
|
||||||
class Program
|
class Program
|
||||||
{
|
{
|
||||||
|
[StaticArgument(LongOption = "serial-remote")]
|
||||||
|
static string SerialRemotePort { get; set; }
|
||||||
|
|
||||||
static void Main(string[] args)
|
static void Main(string[] args)
|
||||||
{
|
{
|
||||||
Logging.Log(LogLevel.INFO, ".NET EtherCAT service host");
|
Logging.Log(LogLevel.INFO, ".NET EtherCAT service host");
|
||||||
|
@ -23,11 +28,52 @@ namespace ln.ethercat.service
|
||||||
EthercatService ethercatService = new EthercatService(args[0]);
|
EthercatService ethercatService = new EthercatService(args[0]);
|
||||||
|
|
||||||
ArgumentContainer argumentContainer = new ArgumentContainer();
|
ArgumentContainer argumentContainer = new ArgumentContainer();
|
||||||
|
argumentContainer.AddStaticOptions<Program>();
|
||||||
argumentContainer.AddOptions(ethercatService);
|
argumentContainer.AddOptions(ethercatService);
|
||||||
argumentContainer.Parse(ref args);
|
argumentContainer.Parse(ref args);
|
||||||
|
|
||||||
ethercatService.Initialize();
|
ethercatService.Initialize();
|
||||||
|
ethercatService.ECMaster.OnStateChange += (ECMaster ECMaster, ECSlaveState newState) => {
|
||||||
|
if (newState == ECSlaveState.OPERATIONAL)
|
||||||
|
{
|
||||||
|
ECMaster.DriveControllers[1].IgnoredByController = true;
|
||||||
|
|
||||||
|
if (ECMaster.GetSDOValue(1, 0x2012, 32, out SDOValue svEnableDrives))
|
||||||
|
{
|
||||||
|
svEnableDrives.SetValue((byte)0x00);
|
||||||
|
}
|
||||||
|
|
||||||
|
} else if (newState == ECSlaveState.PRE_OP)
|
||||||
|
{
|
||||||
|
ECMaster.RequestPDOMapping(1, 0x2012, 31, true);
|
||||||
|
ECMaster.RequestPDOMapping(1, 0x2012, 32, true);
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
ethercatService.ECMaster.Controller.OnStateChanged += (controller, state) => {
|
||||||
|
Logging.Log(LogLevel.DEBUG, "ControllerState=={0}", state);
|
||||||
|
|
||||||
|
if (state == ControllerStates.NOTREADY)
|
||||||
|
{
|
||||||
|
if (controller.ECMaster.GetSDOValue(1, 0x2012, 32, out SDOValue svEnableDrives))
|
||||||
|
{
|
||||||
|
Logging.Log(LogLevel.DEBUG, "ControllerState=={0} powering up enable signals", state);
|
||||||
|
svEnableDrives.SetValue((byte)0x01);
|
||||||
|
}
|
||||||
|
} else if (state == ControllerStates.READY)
|
||||||
|
{
|
||||||
|
controller.ECMaster.DriveControllers[1].DriveMode = DriveMode.TORQUE;
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
ethercatService.Start();
|
ethercatService.Start();
|
||||||
|
|
||||||
|
|
||||||
|
if (SerialRemotePort != null)
|
||||||
|
{
|
||||||
|
StupidSerialRemote stupidSerialRemote = new StupidSerialRemote(ethercatService.ECMaster.Controller, SerialRemotePort);
|
||||||
|
stupidSerialRemote.Start();
|
||||||
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
@ -1,5 +1,3 @@
|
||||||
|
|
||||||
|
|
||||||
using System;
|
using System;
|
||||||
using System.Globalization;
|
using System.Globalization;
|
||||||
using System.Linq;
|
using System.Linq;
|
||||||
|
@ -30,93 +28,61 @@ namespace ln.ethercat.service.api.v1
|
||||||
|
|
||||||
|
|
||||||
[GET("/sockets/controller")]
|
[GET("/sockets/controller")]
|
||||||
public HttpResponse GetControllerSocket()
|
public HttpResponse GetControllerSocket() => new ControllerWebSocket(Controller);
|
||||||
{
|
|
||||||
Timer timer = new Timer(250);
|
|
||||||
JSONWebSocketResponse websocket = new JSONWebSocketResponse();
|
|
||||||
websocket.OnWebSocketStateChanged += (Socket, newstate) => {
|
|
||||||
if (newstate == WebSocketState.CLOSED)
|
|
||||||
{
|
|
||||||
timer.Stop();
|
|
||||||
timer.Dispose();
|
|
||||||
}
|
|
||||||
};
|
|
||||||
|
|
||||||
timer.Elapsed += (s,e) => {
|
// [GET("/sockets/controller/drives/:drive")]
|
||||||
try{
|
// public HttpResponse GetDriveControllerSocket(int drive)
|
||||||
JSONObject controllerState = new JSONObject()
|
// {
|
||||||
.Add("DriveControllers", new JSONArray().Add(Controller.DriveControllers.Select((dc=> new JSONObject()
|
// DriveController driveController = Controller.DriveControllers[drive];
|
||||||
.Add("slave", dc.Slave)
|
|
||||||
.Add("DriveController", dc.GetType().Name)
|
|
||||||
.Add("DriveState", dc.DriveState.ToString())
|
|
||||||
))))
|
|
||||||
.Add("DrivesState", Controller.DrivesState.ToString())
|
|
||||||
.Add("IsRunning", Controller.IsRunning)
|
|
||||||
;
|
|
||||||
websocket.Send(controllerState);
|
|
||||||
} catch (Exception ex)
|
|
||||||
{
|
|
||||||
Logging.Log(ex);
|
|
||||||
}
|
|
||||||
};
|
|
||||||
|
|
||||||
timer.Start();
|
// Timer timer = new Timer(250);
|
||||||
return websocket;
|
// JSONWebSocketResponse websocket = new JSONWebSocketResponse();
|
||||||
}
|
// websocket.OnWebSocketStateChanged += (Socket, newstate) => {
|
||||||
|
// if (newstate == WebSocketState.CLOSED)
|
||||||
|
// {
|
||||||
|
// timer.Stop();
|
||||||
|
// timer.Dispose();
|
||||||
|
// }
|
||||||
|
// };
|
||||||
|
|
||||||
[GET("/sockets/controller/drives/:drive")]
|
// websocket.OnWebSocketReceivedText += (s,text) => {
|
||||||
public HttpResponse GetDriveControllerSocket(int drive)
|
// JSONObject message = (JSONObject)JSONParser.Parse(text);
|
||||||
{
|
// switch (message["event"].ToNative().ToString())
|
||||||
DriveController driveController = Controller.DriveControllers[drive];
|
// {
|
||||||
|
// case "action":
|
||||||
|
// Logging.Log(LogLevel.DEBUG, "DriveControllerSocket: action: {0}", message["value"].ToNative().ToString());
|
||||||
|
// driveController.GetType().GetMethod(message["value"].ToNative().ToString()).Invoke(driveController, new object[0]{});
|
||||||
|
// break;
|
||||||
|
// case "set":
|
||||||
|
// JSONObject jsonSet = (message["value"] as JSONObject);
|
||||||
|
// foreach (string key in jsonSet.Keys)
|
||||||
|
// {
|
||||||
|
// PropertyInfo propertyInfo = driveController.GetType().GetProperty(key);
|
||||||
|
// propertyInfo.SetValue(driveController, Cast.To(jsonSet[key].ToNative(), propertyInfo.PropertyType));
|
||||||
|
// }
|
||||||
|
// break;
|
||||||
|
// }
|
||||||
|
// };
|
||||||
|
|
||||||
Timer timer = new Timer(250);
|
// timer.Elapsed += (s,e) => {
|
||||||
JSONWebSocketResponse websocket = new JSONWebSocketResponse();
|
// try{
|
||||||
websocket.OnWebSocketStateChanged += (Socket, newstate) => {
|
// JSONObject driveControllerState = new JSONObject()
|
||||||
if (newstate == WebSocketState.CLOSED)
|
// .Add("id", drive)
|
||||||
{
|
// .Add("DriveState", driveController.DriveState.ToString())
|
||||||
timer.Stop();
|
// .Add("OEMDriveState", driveController.OEMDriveState)
|
||||||
timer.Dispose();
|
// .Add("DriveMode", driveController.DriveMode)
|
||||||
}
|
// .Add("OEMDriveMode", driveController.OEMDriveMode)
|
||||||
};
|
// ;
|
||||||
|
// websocket.Send(driveControllerState);
|
||||||
|
// } catch (Exception ex)
|
||||||
|
// {
|
||||||
|
// Logging.Log(ex);
|
||||||
|
// }
|
||||||
|
// };
|
||||||
|
|
||||||
websocket.OnWebSocketReceivedText += (s,text) => {
|
// timer.Start();
|
||||||
JSONObject message = (JSONObject)JSONParser.Parse(text);
|
// return websocket;
|
||||||
switch (message["event"].ToNative().ToString())
|
// }
|
||||||
{
|
|
||||||
case "action":
|
|
||||||
Logging.Log(LogLevel.DEBUG, "DriveControllerSocket: action: {0}", message["value"].ToNative().ToString());
|
|
||||||
driveController.GetType().GetMethod(message["value"].ToNative().ToString()).Invoke(driveController, new object[0]{});
|
|
||||||
break;
|
|
||||||
case "set":
|
|
||||||
JSONObject jsonSet = (message["value"] as JSONObject);
|
|
||||||
foreach (string key in jsonSet.Keys)
|
|
||||||
{
|
|
||||||
PropertyInfo propertyInfo = driveController.GetType().GetProperty(key);
|
|
||||||
propertyInfo.SetValue(driveController, Cast.To(jsonSet[key].ToNative(), propertyInfo.PropertyType));
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
};
|
|
||||||
|
|
||||||
timer.Elapsed += (s,e) => {
|
|
||||||
try{
|
|
||||||
JSONObject driveControllerState = new JSONObject()
|
|
||||||
.Add("id", drive)
|
|
||||||
.Add("DriveState", driveController.DriveState.ToString())
|
|
||||||
.Add("OEMDriveState", driveController.OEMDriveState)
|
|
||||||
.Add("DriveMode", driveController.DriveMode)
|
|
||||||
.Add("OEMDriveMode", driveController.OEMDriveMode)
|
|
||||||
;
|
|
||||||
websocket.Send(driveControllerState);
|
|
||||||
} catch (Exception ex)
|
|
||||||
{
|
|
||||||
Logging.Log(ex);
|
|
||||||
}
|
|
||||||
};
|
|
||||||
|
|
||||||
timer.Start();
|
|
||||||
return websocket;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
|
@ -0,0 +1,115 @@
|
||||||
|
using System;
|
||||||
|
using System.Collections.Concurrent;
|
||||||
|
using System.Collections.Generic;
|
||||||
|
using ln.ethercat.controller;
|
||||||
|
using ln.ethercat.controller.drives;
|
||||||
|
using ln.http.websocket;
|
||||||
|
using ln.json;
|
||||||
|
using ln.logging;
|
||||||
|
|
||||||
|
namespace ln.ethercat.service.api.v1
|
||||||
|
{
|
||||||
|
public class ControllerWebSocket : WebSocketResponse
|
||||||
|
{
|
||||||
|
static List<ControllerWebSocket> webSockets = new List<ControllerWebSocket>();
|
||||||
|
static bool sendingUpdates;
|
||||||
|
public static void SendUpdates(Controller controller)
|
||||||
|
{
|
||||||
|
lock (webSockets)
|
||||||
|
{
|
||||||
|
if (sendingUpdates)
|
||||||
|
return;
|
||||||
|
sendingUpdates = true;
|
||||||
|
}
|
||||||
|
|
||||||
|
try
|
||||||
|
{
|
||||||
|
|
||||||
|
JSONArray jsonDriveControllers = new JSONArray();
|
||||||
|
|
||||||
|
foreach (DriveController driveController in controller.ECMaster.DriveControllers)
|
||||||
|
{
|
||||||
|
JSONObject jsonDriveController =
|
||||||
|
new JSONObject()
|
||||||
|
.Add("DriveState", driveController.DriveState.ToString())
|
||||||
|
.Add("OEMState", driveController.OEMDriveState)
|
||||||
|
.Add("DriveMode", driveController.DriveMode.ToString())
|
||||||
|
.Add("TargetValue", driveController.TargetValue)
|
||||||
|
.Add("ActualSpeed", driveController.ActualSpeed)
|
||||||
|
.Add("ActualTorque", driveController.ActualTorque)
|
||||||
|
.Add("ActualLoad", driveController.ActualLoad)
|
||||||
|
;
|
||||||
|
|
||||||
|
jsonDriveControllers.Add(jsonDriveController);
|
||||||
|
}
|
||||||
|
|
||||||
|
JSONObject jsonController =
|
||||||
|
new JSONObject()
|
||||||
|
.Add("State", controller.ControllerState.ToString())
|
||||||
|
.Add("Drives", jsonDriveControllers)
|
||||||
|
.Add("CycleCounter", controller.CycleCounter)
|
||||||
|
;
|
||||||
|
|
||||||
|
|
||||||
|
JSONObject jsonMessage =
|
||||||
|
new JSONObject()
|
||||||
|
.Add("event", "update")
|
||||||
|
.Add("value", jsonController)
|
||||||
|
;
|
||||||
|
|
||||||
|
string jsonMessageText = jsonMessage.ToString();
|
||||||
|
|
||||||
|
ControllerWebSocket[] currentSockets;
|
||||||
|
lock (webSockets)
|
||||||
|
currentSockets = webSockets.ToArray();
|
||||||
|
|
||||||
|
foreach (ControllerWebSocket webSocket in currentSockets)
|
||||||
|
{
|
||||||
|
if (webSocket.Controller.Equals(controller) && (webSocket.State == WebSocketState.OPEN))
|
||||||
|
webSocket.Send(jsonMessageText);
|
||||||
|
}
|
||||||
|
} catch (Exception e)
|
||||||
|
{
|
||||||
|
Logging.Log(e);
|
||||||
|
} finally
|
||||||
|
{
|
||||||
|
sendingUpdates = false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
Controller Controller;
|
||||||
|
|
||||||
|
public ControllerWebSocket(Controller controller)
|
||||||
|
{
|
||||||
|
Controller = controller;
|
||||||
|
OnWebSocketStateChanged += (WebSocketResponse webSocket, WebSocketState newState) => {
|
||||||
|
switch (newState)
|
||||||
|
{
|
||||||
|
case WebSocketState.OPEN:
|
||||||
|
lock (webSockets)
|
||||||
|
webSockets.Add(this);
|
||||||
|
break;
|
||||||
|
default:
|
||||||
|
lock (webSockets)
|
||||||
|
webSockets.Remove(this);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
};
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
public override void Received(string textMessage)
|
||||||
|
{
|
||||||
|
base.Received(textMessage);
|
||||||
|
|
||||||
|
JSONValue jsonTextValue = JSONParser.Parse(textMessage);
|
||||||
|
if (jsonTextValue is JSONObject jsonMessage)
|
||||||
|
{
|
||||||
|
string eventName = jsonMessage["event"].ToNative().ToString();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
}
|
||||||
|
}
|
|
@ -1,18 +1,11 @@
|
||||||
using System;
|
using System;
|
||||||
using System.Collections.Generic;
|
|
||||||
using System.Linq;
|
|
||||||
using System.Timers;
|
|
||||||
using ln.http;
|
using ln.http;
|
||||||
using ln.http.api;
|
using ln.http.api;
|
||||||
using ln.http.api.attributes;
|
using ln.http.api.attributes;
|
||||||
using ln.http.websocket;
|
|
||||||
using ln.json;
|
using ln.json;
|
||||||
using ln.json.mapping;
|
|
||||||
using ln.logging;
|
|
||||||
|
|
||||||
namespace ln.ethercat.service.api.v1
|
namespace ln.ethercat.service.api.v1
|
||||||
{
|
{
|
||||||
|
|
||||||
public class EthercatApiController : WebApiController
|
public class EthercatApiController : WebApiController
|
||||||
{
|
{
|
||||||
ECMaster ECMaster => EthercatService.ECMaster;
|
ECMaster ECMaster => EthercatService.ECMaster;
|
||||||
|
@ -51,14 +44,6 @@ namespace ln.ethercat.service.api.v1
|
||||||
[GET("/slaves/:slave/state")]
|
[GET("/slaves/:slave/state")]
|
||||||
public HttpResponse GetEthercatState(int slave) => HttpResponse.OK().Content(ECMaster.ReadSlaveState(slave).ToString());
|
public HttpResponse GetEthercatState(int slave) => HttpResponse.OK().Content(ECMaster.ReadSlaveState(slave).ToString());
|
||||||
|
|
||||||
[POST("/slaves/:slave/state")]
|
|
||||||
public HttpResponse RequestEthercatState(int slave, ECSlaveState state)
|
|
||||||
{
|
|
||||||
if (ECMaster.RequestSlaveState(slave, state) > 0)
|
|
||||||
return HttpResponse.NoContent();
|
|
||||||
return HttpResponse.GatewayTimeout();
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
[GET("/master/state")]
|
[GET("/master/state")]
|
||||||
public HttpResponse GetFullState()
|
public HttpResponse GetFullState()
|
||||||
|
|
|
@ -10,6 +10,7 @@ using ln.http.websocket;
|
||||||
using ln.json;
|
using ln.json;
|
||||||
using ln.json.mapping;
|
using ln.json.mapping;
|
||||||
using ln.logging;
|
using ln.logging;
|
||||||
|
using ln.type;
|
||||||
|
|
||||||
namespace ln.ethercat.service.api.v1
|
namespace ln.ethercat.service.api.v1
|
||||||
{
|
{
|
||||||
|
@ -17,9 +18,19 @@ namespace ln.ethercat.service.api.v1
|
||||||
{
|
{
|
||||||
/************** Static ***************************************************************************/
|
/************** Static ***************************************************************************/
|
||||||
static List<EthercatWebSocket> activeWebSockets = new List<EthercatWebSocket>();
|
static List<EthercatWebSocket> activeWebSockets = new List<EthercatWebSocket>();
|
||||||
|
static bool broadcastActive;
|
||||||
|
|
||||||
public static void SendProcessData(ECMaster ecMaster)
|
public static void SendProcessData(ECMaster ecMaster)
|
||||||
{
|
{
|
||||||
|
lock (activeWebSockets)
|
||||||
|
{
|
||||||
|
if (broadcastActive)
|
||||||
|
return;
|
||||||
|
broadcastActive = true;
|
||||||
|
}
|
||||||
|
|
||||||
|
try{
|
||||||
|
|
||||||
JSONArray jsonPDOList = new JSONArray();
|
JSONArray jsonPDOList = new JSONArray();
|
||||||
|
|
||||||
foreach (PDO pdo in ecMaster.GetPDOMap())
|
foreach (PDO pdo in ecMaster.GetPDOMap())
|
||||||
|
@ -49,11 +60,29 @@ namespace ln.ethercat.service.api.v1
|
||||||
);
|
);
|
||||||
string stateMessageText = stateMessage.ToString();
|
string stateMessageText = stateMessage.ToString();
|
||||||
|
|
||||||
foreach (EthercatWebSocket webSocket in activeWebSockets)
|
EthercatWebSocket[] sockets;
|
||||||
|
lock(activeWebSockets)
|
||||||
|
sockets = activeWebSockets.ToArray();
|
||||||
|
|
||||||
|
foreach (EthercatWebSocket webSocket in sockets)
|
||||||
{
|
{
|
||||||
webSocket.Send(messageText);
|
if (webSocket.State != WebSocketState.OPEN)
|
||||||
webSocket.Send(stateMessageText);
|
{
|
||||||
webSocket.SendSubscribedSDOs();
|
lock (activeWebSockets)
|
||||||
|
activeWebSockets.Remove(webSocket);
|
||||||
|
} else
|
||||||
|
{
|
||||||
|
webSocket.Send(messageText);
|
||||||
|
webSocket.Send(stateMessageText);
|
||||||
|
webSocket.SendSubscribedSDOs();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
} catch (Exception e)
|
||||||
|
{
|
||||||
|
Logging.Log(e);
|
||||||
|
} finally
|
||||||
|
{
|
||||||
|
broadcastActive = false;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -93,11 +122,13 @@ namespace ln.ethercat.service.api.v1
|
||||||
{
|
{
|
||||||
case WebSocketState.OPEN:
|
case WebSocketState.OPEN:
|
||||||
Logging.Log(LogLevel.DEBUG, "EthercatWebSocket -> active");
|
Logging.Log(LogLevel.DEBUG, "EthercatWebSocket -> active");
|
||||||
activeWebSockets.Add(this);
|
lock(activeWebSockets)
|
||||||
|
activeWebSockets.Add(this);
|
||||||
break;
|
break;
|
||||||
case WebSocketState.CLOSED:
|
default:
|
||||||
Logging.Log(LogLevel.DEBUG, "EthercatWebSocket -> inactive");
|
Logging.Log(LogLevel.DEBUG, "EthercatWebSocket -> inactive");
|
||||||
activeWebSockets.Remove(this);
|
lock(activeWebSockets)
|
||||||
|
activeWebSockets.Remove(this);
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -127,6 +158,28 @@ namespace ln.ethercat.service.api.v1
|
||||||
break;
|
break;
|
||||||
case "action":
|
case "action":
|
||||||
|
|
||||||
|
break;
|
||||||
|
case "sdowrite":
|
||||||
|
if (jsonMessage["value"] is JSONObject jsonValue)
|
||||||
|
{
|
||||||
|
UInt16 slave_id, index;
|
||||||
|
byte subindex;
|
||||||
|
object value;
|
||||||
|
|
||||||
|
slave_id = Cast.To<UInt16>(jsonValue["Slave"].ToNative());
|
||||||
|
index = Cast.To<UInt16>(jsonValue["Index"].ToNative());
|
||||||
|
subindex = Cast.To<byte>(jsonValue["SubIndex"].ToNative());
|
||||||
|
value = jsonValue["Value"].ToNative();
|
||||||
|
|
||||||
|
if (EthercatService.ECMaster.GetSDOValue(slave_id, index, subindex, out SDOValue sdoValue))
|
||||||
|
{
|
||||||
|
sdoValue.SetValue(value);
|
||||||
|
} else
|
||||||
|
{
|
||||||
|
Logging.Log(LogLevel.DEBUG, "EthercatWebSocket: event sdowrite: could not find SDOValue {0}:{1:X4}.{2}", slave_id, index, subindex);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
@ -50,7 +50,11 @@
|
||||||
<router-view></router-view>
|
<router-view></router-view>
|
||||||
</div>
|
</div>
|
||||||
<aside class="right">
|
<aside class="right">
|
||||||
<pdo-panel id="pdopanel" :processdata="processdata"></pdo-panel>
|
<pdo-panel
|
||||||
|
id="pdopanel"
|
||||||
|
:processdata="processdata"
|
||||||
|
@write="$ECAPP.sendMessage('sdowrite', $event);"
|
||||||
|
></pdo-panel>
|
||||||
</aside>
|
</aside>
|
||||||
</main>
|
</main>
|
||||||
<footer>
|
<footer>
|
||||||
|
|
|
@ -49,13 +49,13 @@ fieldset {
|
||||||
display: flex;
|
display: flex;
|
||||||
}
|
}
|
||||||
|
|
||||||
fieldset > input {
|
|
||||||
flex-grow: 1;
|
|
||||||
}
|
|
||||||
fieldset > label {
|
fieldset > label {
|
||||||
flex-grow: 0;
|
flex-grow: 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
fieldset > input, fieldset > .value {
|
||||||
|
flex-grow: 1;
|
||||||
|
}
|
||||||
|
|
||||||
.panel {
|
.panel {
|
||||||
padding: 6px;
|
padding: 6px;
|
||||||
|
|
|
@ -24,6 +24,10 @@ class EthercatApplication
|
||||||
this.base_url = scheme + "//" + host;
|
this.base_url = scheme + "//" + host;
|
||||||
|
|
||||||
this.connectMainSocket();
|
this.connectMainSocket();
|
||||||
|
|
||||||
|
window.onunload = () => {
|
||||||
|
this.closeWebSockets();
|
||||||
|
};
|
||||||
}
|
}
|
||||||
|
|
||||||
connectMainSocket(){
|
connectMainSocket(){
|
||||||
|
@ -49,6 +53,7 @@ class EthercatApplication
|
||||||
this.socket.onerror = (evt)=>{
|
this.socket.onerror = (evt)=>{
|
||||||
console.log("weboscket error: ", evt);
|
console.log("weboscket error: ", evt);
|
||||||
this.socket.close();
|
this.socket.close();
|
||||||
|
this.socket = null;
|
||||||
};
|
};
|
||||||
this.socket.onclose = (evt)=>{
|
this.socket.onclose = (evt)=>{
|
||||||
console.log("weboscket closed: ", evt);
|
console.log("weboscket closed: ", evt);
|
||||||
|
@ -72,6 +77,7 @@ class EthercatApplication
|
||||||
{
|
{
|
||||||
let ws = this.ac_sockets.pop();
|
let ws = this.ac_sockets.pop();
|
||||||
console.log("disposing:", ws);
|
console.log("disposing:", ws);
|
||||||
|
ws.onclose = null;
|
||||||
ws.close();
|
ws.close();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
@ -12,12 +12,26 @@
|
||||||
data: function(){
|
data: function(){
|
||||||
return {}
|
return {}
|
||||||
},
|
},
|
||||||
|
methods: {
|
||||||
|
promptNewValue: function(slave, index, subindex, datatype, initialValue){
|
||||||
|
console.log(index, subindex, datatype, initialValue);
|
||||||
|
let newValue = window.prompt(`new value for ${slave}:${index.toString(16)}.${subindex}`, initialValue);
|
||||||
|
console.log(newValue);
|
||||||
|
if (newValue)
|
||||||
|
{
|
||||||
|
this.$emit("write", { Slave: slave, Index: index, SubIndex: subindex, Value: newValue});
|
||||||
|
}
|
||||||
|
}
|
||||||
|
},
|
||||||
template: `
|
template: `
|
||||||
<div class="panel">
|
<div class="panel">
|
||||||
<h2>Prozessdaten</h2>
|
<h2>Prozessdaten</h2>
|
||||||
<article v-for="sdo,key in processdata">
|
<article v-for="sdo,key in processdata">
|
||||||
<div>{{sdo.Descriptor.Index.toString(16).toUpperCase()}}.{{sdo.SubIndex}} {{ sdo.Name }}</div>
|
<div>{{sdo.Descriptor.SlaveId}}:{{sdo.Descriptor.Index.toString(16).toUpperCase()}}.{{sdo.SubIndex}} {{ sdo.Name }} ({{sdo.DataType}})</div>
|
||||||
<div class="value">{{ sdo.Value }}</div>
|
<div
|
||||||
|
class="value"
|
||||||
|
@dblclick="promptNewValue(sdo.Descriptor.SlaveId, sdo.Descriptor.Index, sdo.SubIndex, sdo.DataType, sdo.Value);"
|
||||||
|
>{{ sdo.Value }}</div>
|
||||||
</article>
|
</article>
|
||||||
</div>
|
</div>
|
||||||
`,
|
`,
|
||||||
|
|
|
@ -13,18 +13,32 @@
|
||||||
data: function(){
|
data: function(){
|
||||||
return {}
|
return {}
|
||||||
},
|
},
|
||||||
|
methods: {
|
||||||
|
promptNewValue: function(slave, index, subindex, datatype, initialValue){
|
||||||
|
console.log(index, subindex, datatype, initialValue);
|
||||||
|
let newValue = window.prompt(`new value for ${slave}:${index.toString(16)}.${subindex}`, initialValue);
|
||||||
|
console.log(newValue);
|
||||||
|
if (newValue)
|
||||||
|
{
|
||||||
|
this.$emit("write", { Slave: slave, Index: index, SubIndex: subindex, Value: newValue});
|
||||||
|
}
|
||||||
|
}
|
||||||
|
},
|
||||||
template: `
|
template: `
|
||||||
<div class="panel">
|
<div class="panel">
|
||||||
<h2>SDO Panel</h2>
|
<h2>SDO Panel</h2>
|
||||||
<article v-for="sdo,key in subscribed">
|
<article v-for="sdo,key in subscribed">
|
||||||
<h2>{{sdo.Index.toString(16).toUpperCase()}} {{ sdo.Name }} ({{ sdo.DataType}} / M:{{ sdo.MaxSubIndex }})</h2>
|
<h2>{{sdo.Index.toString(16).toUpperCase()}} {{ sdo.Name }} ({{ sdo.DataType}} / M:{{ sdo.MaxSubIndex }})</h2>
|
||||||
<ul>
|
<ul>
|
||||||
<div
|
<fieldset
|
||||||
v-for="sdovalue in sdo.Values"
|
v-for="sdovalue in sdo.Values"
|
||||||
>
|
>
|
||||||
<span>.{{sdovalue.SubIndex}} {{ sdovalue.Name }} ({{ sdo.DataType}})</span>
|
<span>.{{sdovalue.SubIndex}} {{ sdovalue.Name }} ({{ sdovalue.DataType}})</span>
|
||||||
<input :disabled="(sdovalue.DataType == 'NONE')" type="text" :value="sdovalue.Value">
|
<div
|
||||||
</div>
|
class="value"
|
||||||
|
@dblclick="promptNewValue(sdo.SlaveId, sdo.Index, sdovalue.SubIndex, sdovalue.DataType, sdovalue.Value);"
|
||||||
|
>{{ sdovalue.Value }}</div>
|
||||||
|
</fieldset>
|
||||||
</ul>
|
</ul>
|
||||||
</article>
|
</article>
|
||||||
</div>
|
</div>
|
||||||
|
|
|
@ -16,20 +16,35 @@
|
||||||
datatype: null,
|
datatype: null,
|
||||||
},
|
},
|
||||||
selectedIndex: null,
|
selectedIndex: null,
|
||||||
|
manualIndex: "",
|
||||||
};
|
};
|
||||||
},
|
},
|
||||||
methods: {
|
methods: {
|
||||||
getFilteredDescriptors: function(){
|
getFilteredDescriptors: function(){
|
||||||
return this.descriptors.filter((sdo)=>
|
if (this.filters.name)
|
||||||
(!this.filters.name || sdo.name.toLowerCase().includes(this.filters.name.toLowerCase())) ); // &&
|
return this.descriptors.filter((sdo)=>(sdo.name.toLowerCase().includes(this.filters.name.toLowerCase())) );
|
||||||
|
else
|
||||||
|
return this.descriptors;
|
||||||
|
|
||||||
|
// &&
|
||||||
// (!this.filters.index || sdo.index.toString(16).toLowerCase().includes(this.filters.index.toLowerCase())) &&
|
// (!this.filters.index || sdo.index.toString(16).toLowerCase().includes(this.filters.index.toLowerCase())) &&
|
||||||
// (!this.filters.datatype || sdo.datatype.toLowerCase().includes(this.filters.datatype.toLowerCase())) &&
|
// (!this.filters.datatype || sdo.datatype.toLowerCase().includes(this.filters.datatype.toLowerCase())) &&
|
||||||
// (!this.filters.slave || sdo.address.slave.toLowerCase().includes(this.filters.slave.toLowerCase()))
|
// (!this.filters.slave || sdo.address.slave.toLowerCase().includes(this.filters.slave.toLowerCase()))
|
||||||
// );
|
// );
|
||||||
},
|
},
|
||||||
|
addManualIndex: function(){
|
||||||
|
if (this.manualIndex)
|
||||||
|
{
|
||||||
|
this.$emit("selected", parseInt(this.manualIndex, 16));
|
||||||
|
}
|
||||||
|
}
|
||||||
},
|
},
|
||||||
template: `
|
template: `
|
||||||
<sdoselect class="flex" style="flex-direction: column;">
|
<sdoselect class="flex" style="flex-direction: column;">
|
||||||
|
<fieldset>
|
||||||
|
<input type="text" pattern="[a-fA-F0-9]{4}" v-model="manualIndex">
|
||||||
|
<button @click="addManualIndex();">+</button>
|
||||||
|
</fieldset>
|
||||||
<input type="text" v-model="filters.name">
|
<input type="text" v-model="filters.name">
|
||||||
<select size="15" v-model="selectedIndex" @dblclick="if (selectedIndex>=0) $emit('selected', selectedIndex);">
|
<select size="15" v-model="selectedIndex" @dblclick="if (selectedIndex>=0) $emit('selected', selectedIndex);">
|
||||||
<option
|
<option
|
||||||
|
|
|
@ -3,11 +3,6 @@
|
||||||
<p>
|
<p>
|
||||||
{{ controller }}
|
{{ controller }}
|
||||||
</p>
|
</p>
|
||||||
<div>
|
|
||||||
<router-link
|
|
||||||
v-for="dc,idx in controller.DriveControllers"
|
|
||||||
:to="'/controller/drives/' + idx">Drive Controller {{ idx }}</router-link>
|
|
||||||
</div>
|
|
||||||
<div v-if="drive_controller">
|
<div v-if="drive_controller">
|
||||||
<h2>Drive Controller</h2>
|
<h2>Drive Controller</h2>
|
||||||
<button @click="drive_poweroff">Power Off</button>
|
<button @click="drive_poweroff">Power Off</button>
|
||||||
|
@ -27,9 +22,9 @@
|
||||||
<div>
|
<div>
|
||||||
<fieldset>
|
<fieldset>
|
||||||
<label>Target Torque:</label>
|
<label>Target Torque:</label>
|
||||||
<input type="range" min="-1000" max="1000" v-model="targetTorque" id="targetTorque" @input="setProperty('TargetTorque', targetTorque);">
|
<input type="range" min="-1000" max="1000" v-model="targetTorque" id="targetTorque" @input="setProperty('TargetValue', targetTorque);">
|
||||||
<input type="number" v-model="targetTorque" @change="setProperty('TargetTorque', targetTorque);">
|
<input type="number" v-model="targetTorque" @change="setProperty('TargetValue', targetTorque);">
|
||||||
<button @click="targetTorque = 0; setProperty('TargetTorque', targetTorque);">ZERO</button>
|
<button @click="targetTorque = 0; setProperty('TargetValue', targetTorque);">ZERO</button>
|
||||||
</fieldset>
|
</fieldset>
|
||||||
</div>
|
</div>
|
||||||
|
|
||||||
|
|
|
@ -1,4 +1,6 @@
|
||||||
<div>
|
<div>
|
||||||
<h1>Prozessdaten</h1>
|
<h1>Prozessdaten</h1>
|
||||||
<sdo-table :sdolist="this.processdata"></sdo-table>
|
<sdo-table
|
||||||
|
:sdolist="this.processdata"
|
||||||
|
></sdo-table>
|
||||||
</div>
|
</div>
|
|
@ -7,6 +7,9 @@
|
||||||
@selected="$ECAPP.subscribe(slave_id, $event);"
|
@selected="$ECAPP.subscribe(slave_id, $event);"
|
||||||
></sdo-select>
|
></sdo-select>
|
||||||
</div>
|
</div>
|
||||||
<sdo-panel :subscribed="$ECAPP.subscribed.sdo"></sdo-panel>
|
<sdo-panel
|
||||||
|
:subscribed="$ECAPP.subscribed.sdo"
|
||||||
|
@write="$ECAPP.sendMessage('sdowrite', $event);"
|
||||||
|
></sdo-panel>
|
||||||
</div>
|
</div>
|
||||||
</div>
|
</div>
|
|
@ -159,7 +159,7 @@ namespace ln.ethercat
|
||||||
));
|
));
|
||||||
|
|
||||||
AddConverter(ECDataTypes.NONE, new ECValueConverterAdapter(
|
AddConverter(ECDataTypes.NONE, new ECValueConverterAdapter(
|
||||||
typeof(byte[]),
|
typeof(string),
|
||||||
(object value, out byte[] ethercatBytes) => { ethercatBytes = Extensions.BytesFromHexString(value as string); return true; },
|
(object value, out byte[] ethercatBytes) => { ethercatBytes = Extensions.BytesFromHexString(value as string); return true; },
|
||||||
(byte[] ethercatBytes, out object value) => { value = ethercatBytes.ToHexString(); return true; }
|
(byte[] ethercatBytes, out object value) => { value = ethercatBytes.ToHexString(); return true; }
|
||||||
));
|
));
|
||||||
|
|
|
@ -41,7 +41,7 @@ namespace ln.ethercat
|
||||||
}
|
}
|
||||||
|
|
||||||
public delegate void cb_enum_indeces(int slave, int index);
|
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);
|
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")]
|
[DllImport("lib/libecmbind.so")]
|
||||||
public static extern int ecmbind_sdo_write(int slave, int index, int subindex, byte[] buffer, int size);
|
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")]
|
[DllImport("lib/libecmbind.so")]
|
||||||
|
|
|
@ -1,13 +1,13 @@
|
||||||
|
|
||||||
|
|
||||||
using System;
|
using System;
|
||||||
using System.Buffers;
|
|
||||||
using System.Collections.Generic;
|
using System.Collections.Generic;
|
||||||
|
using System.Diagnostics;
|
||||||
using System.IO;
|
using System.IO;
|
||||||
using System.Linq;
|
using System.Linq;
|
||||||
using System.Reflection.Metadata;
|
|
||||||
using System.Security.Cryptography.X509Certificates;
|
|
||||||
using System.Threading;
|
using System.Threading;
|
||||||
|
using ln.ethercat.controller;
|
||||||
|
using ln.ethercat.controller.drives;
|
||||||
using ln.logging;
|
using ln.logging;
|
||||||
using ln.type;
|
using ln.type;
|
||||||
|
|
||||||
|
@ -21,6 +21,23 @@ namespace ln.ethercat
|
||||||
RUNNING,
|
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 delegate void ECStateChange(ECMaster sender,ECSlaveState newState);
|
||||||
|
|
||||||
public class ECMaster
|
public class ECMaster
|
||||||
|
@ -47,8 +64,8 @@ namespace ln.ethercat
|
||||||
|
|
||||||
ECSlaveState ethercatState = ECSlaveState.NONE;
|
ECSlaveState ethercatState = ECSlaveState.NONE;
|
||||||
public ECSlaveState EthercatState {
|
public ECSlaveState EthercatState {
|
||||||
get => UpdateEthercatState();
|
get =>ethercatState;
|
||||||
set {
|
private set {
|
||||||
if (value != ethercatState)
|
if (value != ethercatState)
|
||||||
{
|
{
|
||||||
OnStateChange?.Invoke(this, value);
|
OnStateChange?.Invoke(this, value);
|
||||||
|
@ -73,113 +90,168 @@ namespace ln.ethercat
|
||||||
Logging.Log(LogLevel.INFO, "ecmbind_initialize({0}) = {1}", interfaceName, result);
|
Logging.Log(LogLevel.INFO, "ecmbind_initialize({0}) = {1}", interfaceName, result);
|
||||||
if (result<=0)
|
if (result<=0)
|
||||||
throw new Exception("ecmbind_initialize failed");
|
throw new Exception("ecmbind_initialize failed");
|
||||||
|
|
||||||
|
Controller = new Controller(this);
|
||||||
}
|
}
|
||||||
|
|
||||||
bool stopProcessing;
|
bool stopWatchdog;
|
||||||
public bool StopProccessing {
|
bool stopProcessData;
|
||||||
get => stopProcessing;
|
|
||||||
set => stopProcessing = value;
|
|
||||||
}
|
|
||||||
|
|
||||||
Thread threadProcessData;
|
Thread threadProcessData;
|
||||||
Thread threadWatchdog;
|
Thread threadWatchdog;
|
||||||
|
|
||||||
public bool Start()
|
public Controller Controller { get; }
|
||||||
|
List<DriveController> driveControllers = new List<DriveController>();
|
||||||
|
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)
|
lock (this)
|
||||||
{
|
{
|
||||||
CountSlaves = ECMBind.ecmbind_config_init();
|
if (threadWatchdog?.IsAlive ?? false)
|
||||||
if (CountSlaves <= 0)
|
throw new Exception("already started");
|
||||||
{
|
|
||||||
Logging.Log(LogLevel.DEBUG, "ECMaster: no slaves connected");
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (!CheckState(ECSlaveState.PRE_OP, TIMEOUT_PREOP))
|
ethercatState = ECSlaveState.NONE;
|
||||||
return false;
|
ExpectedWorkCounter = 0;
|
||||||
|
|
||||||
IOMapSize = ECMBind.ecmbind_config_map();
|
stopWatchdog = false;
|
||||||
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;
|
|
||||||
}
|
|
||||||
|
|
||||||
threadWatchdog = new Thread(Watchdog);
|
threadWatchdog = new Thread(Watchdog);
|
||||||
threadWatchdog.Start();
|
threadWatchdog.Start();
|
||||||
|
|
||||||
}
|
}
|
||||||
return true;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
public void Stop()
|
public void Stop()
|
||||||
{
|
{
|
||||||
if (threadProcessData?.IsAlive ?? false)
|
lock (this)
|
||||||
{
|
{
|
||||||
stopProcessing = true;
|
if (threadWatchdog?.IsAlive ?? false)
|
||||||
ECMBind.ecmbind_request_state(0, ECSlaveState.SAFE_OP, TIMEOUT_BACKTO_SAFEOP);
|
|
||||||
|
|
||||||
threadProcessData.Join();
|
|
||||||
threadProcessData = null;
|
|
||||||
|
|
||||||
if (!Thread.CurrentThread.Equals(threadWatchdog))
|
|
||||||
{
|
{
|
||||||
threadWatchdog?.Join();
|
stopWatchdog = true;
|
||||||
threadWatchdog = null;
|
|
||||||
|
lock (this)
|
||||||
|
Monitor.PulseAll(this);
|
||||||
|
|
||||||
|
threadWatchdog.Join();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
object lockIOMap = new object();
|
|
||||||
byte[] iomap = new byte[8192];
|
|
||||||
|
|
||||||
void Watchdog()
|
void Watchdog()
|
||||||
{
|
{
|
||||||
// if (!ReadSDOIndeces())
|
while (!stopWatchdog)
|
||||||
// {
|
{
|
||||||
// Logging.Log(LogLevel.WARNING, "ECMaster: could not read SDO indeces");
|
IOMapSize = 0;
|
||||||
// }
|
if (threadProcessData?.IsAlive ?? false)
|
||||||
|
|
||||||
if (!RequestState(ECSlaveState.OPERATIONAL, out ECSlaveState slaveState, TIMEOUT_SAFEOP))
|
|
||||||
{
|
{
|
||||||
if (slaveState < ECSlaveState.SAFE_OP)
|
stopProcessData = true;
|
||||||
Stop();
|
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();
|
Logging.Log(LogLevel.DEBUG, "ECMaster: slaves did not reach PRE_OP. restart...");
|
||||||
Thread.Sleep(250);
|
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)
|
lock (lockIOMap)
|
||||||
{
|
{
|
||||||
wkc = ECMBind.ecmbind_processdata2(iomap, iomap.Length);
|
wkc = ECMBind.ecmbind_processdata2(iomap, iomap.Length);
|
||||||
|
@ -187,26 +259,160 @@ namespace ln.ethercat
|
||||||
{
|
{
|
||||||
int success = ECMBind.ecmbind_recover();
|
int success = ECMBind.ecmbind_recover();
|
||||||
if (success > 0)
|
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);
|
Thread.Sleep(INTERVALL_PROCESSDATA);
|
||||||
|
}
|
||||||
|
|
||||||
/*
|
Logging.Log(LogLevel.INFO, "ECMaster: ProcessData(): finished");
|
||||||
if (wkc != ExpectedWorkCounter)
|
}
|
||||||
|
|
||||||
|
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();
|
ushort profilecode = BitConverter.ToUInt16(bDeviceType);
|
||||||
if (ExpectedWorkCounter < 0)
|
Logging.Log(LogLevel.INFO, "ECMaster: ScanControllers: Slave {0} has DeviceType {1} [{2}]", slave_id, profilecode, bDeviceType.ToHexString());
|
||||||
break;
|
|
||||||
}
|
|
||||||
*/
|
|
||||||
|
|
||||||
|
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<PDOMappingRequest> mappingRequests = new HashSet<PDOMappingRequest>();
|
||||||
|
|
||||||
|
public void ConfigureProcessDataMappings()
|
||||||
|
{
|
||||||
|
Logging.Log(LogLevel.INFO, "configure PDO mappings");
|
||||||
|
|
||||||
|
for (UInt16 slave = 1; slave <= CountSlaves; slave++)
|
||||||
|
{
|
||||||
|
List<int> rxPDOs = new List<int>();
|
||||||
|
List<int> txPDOs = new List<int>();
|
||||||
|
|
||||||
|
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<PDOMappingRequest> 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)
|
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 );
|
ECMBind.ecmbind_sdo_read(slave, index, subIndex, rawValue, rawValue.Length );
|
||||||
if (dataSize <= 0)
|
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();
|
throw new IOException();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -233,41 +459,20 @@ namespace ln.ethercat
|
||||||
int result;
|
int result;
|
||||||
lock (lockIOMap)
|
lock (lockIOMap)
|
||||||
result = ECMBind.ecmbind_sdo_write(slave, index, subIndex, rawValue, rawValue.Length);
|
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;
|
return result > 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
public ECSlaveState ReadSlaveState(int slave) => ECMBind.ecmbind_get_slave_state(slave);
|
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));
|
||||||
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 bool GetSlave(UInt16 slave_id, out ECSlave slave)
|
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))
|
if ((offset >= 0) && (length > 0) && (offset+length < iomap.Length))
|
||||||
{
|
{
|
||||||
rawData = iomap.Slice(offset, length);
|
rawData = iomap.Slice(offset, length);
|
||||||
|
//Logging.Log(LogLevel.ERROR, "GetIOmapData({0},{1})", offset, length);
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
Logging.Log(LogLevel.ERROR, "GetIOmapData({0},{1}) failed", offset, length);
|
Logging.Log(LogLevel.ERROR, "GetIOmapData({0},{1}) failed", offset, length);
|
||||||
|
@ -308,7 +514,7 @@ namespace ln.ethercat
|
||||||
lock (lockIOMap)
|
lock (lockIOMap)
|
||||||
if ((offset >= 0) && (length > 0) && (offset+length < iomap.Length))
|
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);
|
Buffer.BlockCopy(rawData, 0, iomap, offset, length);
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
@ -334,18 +540,19 @@ namespace ln.ethercat
|
||||||
public void UpdatePDOMap()
|
public void UpdatePDOMap()
|
||||||
{
|
{
|
||||||
List<PDO> pdoList = new List<PDO>();
|
List<PDO> pdoList = new List<PDO>();
|
||||||
ECMBind.ecmbind_pdo_enumerate((UInt16 slave, UInt16 index, byte subindex, int addr_offset, int addr_bit, int bitlength)=>{
|
lock (lockIOMap)
|
||||||
if (addr_bit != 0)
|
ECMBind.ecmbind_pdo_enumerate((UInt16 slave, UInt16 index, byte subindex, int addr_offset, int addr_bit, int bitlength)=>{
|
||||||
Logging.Log(LogLevel.WARNING, "currently only PDO mappings on byte boundaries are supported");
|
if (addr_bit != 0)
|
||||||
else
|
Logging.Log(LogLevel.WARNING, "currently only PDO mappings on byte boundaries are supported");
|
||||||
pdoList.Add(new PDO(this,slave, index, subindex){
|
else
|
||||||
AddressOffset = addr_offset,
|
pdoList.Add(new PDO(this,slave, index, subindex){
|
||||||
AddressBit = addr_bit,
|
AddressOffset = addr_offset,
|
||||||
BitLength = bitlength
|
AddressBit = addr_bit,
|
||||||
});
|
BitLength = bitlength
|
||||||
|
});
|
||||||
});
|
});
|
||||||
|
|
||||||
lock (this)
|
lock (lockIOMap)
|
||||||
{
|
{
|
||||||
IOMapPtr = ECMBind.ecmbind_get_iomap();
|
IOMapPtr = ECMBind.ecmbind_get_iomap();
|
||||||
pdoMap = pdoList;
|
pdoMap = pdoList;
|
||||||
|
|
|
@ -58,7 +58,7 @@ namespace ln.ethercat
|
||||||
|
|
||||||
public void Update()
|
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;
|
Name = name;
|
||||||
DataType = dataType;
|
DataType = dataType;
|
||||||
ObjectCode = objectCode;
|
ObjectCode = objectCode;
|
||||||
|
@ -105,6 +105,7 @@ namespace ln.ethercat
|
||||||
{
|
{
|
||||||
public SDODescriptor Descriptor;
|
public SDODescriptor Descriptor;
|
||||||
public byte SubIndex { get; }
|
public byte SubIndex { get; }
|
||||||
|
public ushort BitLength { get; private set; }
|
||||||
|
|
||||||
public string Name { get; private set; }
|
public string Name { get; private set; }
|
||||||
public ECDataTypes DataType { get; private set; }
|
public ECDataTypes DataType { get; private set; }
|
||||||
|
@ -114,13 +115,14 @@ namespace ln.ethercat
|
||||||
Descriptor = descriptor;
|
Descriptor = descriptor;
|
||||||
SubIndex = subIndex;
|
SubIndex = subIndex;
|
||||||
|
|
||||||
if (SubIndex == 0)
|
if ((Descriptor.MaxSubIndex > 0) && (SubIndex == 0))
|
||||||
{
|
{
|
||||||
DataType = descriptor.DataType;
|
DataType = descriptor.DataType;
|
||||||
Name = descriptor.Name;
|
Name = descriptor.Name;
|
||||||
} else {
|
} else {
|
||||||
if (ECMBind.ecmbind_read_objectdescription_entry(Descriptor.SlaveId, descriptor.Index, SubIndex, (int slave, int index, ECDataTypes dataType, ECObjectCodes objectCode, int si, String 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 = name;
|
Name = (SubIndex == 0) ? descriptor.Name : name;
|
||||||
|
BitLength = bitlength;
|
||||||
DataType = dataType;
|
DataType = dataType;
|
||||||
}) <= 0)
|
}) <= 0)
|
||||||
{
|
{
|
||||||
|
|
|
@ -1,4 +1,5 @@
|
||||||
|
|
||||||
|
using System;
|
||||||
using System.Collections.Generic;
|
using System.Collections.Generic;
|
||||||
using System.Threading;
|
using System.Threading;
|
||||||
using ln.ethercat.controller.drives;
|
using ln.ethercat.controller.drives;
|
||||||
|
@ -8,98 +9,225 @@ namespace ln.ethercat.controller
|
||||||
{
|
{
|
||||||
|
|
||||||
public delegate void ControllerLogicDelegate(Controller 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 class Controller
|
||||||
{
|
{
|
||||||
public event ControllerLogicDelegate ControllerLogic;
|
public event ControllerLogicDelegate ControllerLogic;
|
||||||
|
public event ControllerStateChangeDelegate OnStateChanging;
|
||||||
|
public event ControllerStateChangeDelegate OnStateChanged;
|
||||||
|
|
||||||
|
public ECMaster ECMaster { get; }
|
||||||
|
|
||||||
List<DriveController> driveControllers = new List<DriveController>();
|
public bool IgnoreRemoteInterface { get; set; }
|
||||||
public DriveController[] DriveControllers => driveControllers.ToArray();
|
|
||||||
|
|
||||||
List<ControlLoop> controlLoops = new List<ControlLoop>();
|
List<ControlLoop> controlLoops = new List<ControlLoop>();
|
||||||
public ControlLoop[] ControlLoops => controlLoops.ToArray();
|
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 ControllerLoopInterval { get; set; } = 0.1;
|
||||||
public double ControllerLoopFrequency {
|
public double ControllerLoopFrequency {
|
||||||
get => 1.0 / ControllerLoopInterval;
|
get => 1.0 / ControllerLoopInterval;
|
||||||
set => ControllerLoopInterval = 1.0 / value;
|
set => ControllerLoopInterval = 1.0 / value;
|
||||||
}
|
}
|
||||||
|
public long CycleCounter { get; private set; }
|
||||||
|
|
||||||
bool stopRequested;
|
Thread threadWatchdog;
|
||||||
Thread threadController;
|
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 Add(ControlLoop controlLoop) => controlLoops.Add(controlLoop);
|
||||||
public void Remove(ControlLoop controlLoop) => controlLoops.Remove(controlLoop);
|
public void Remove(ControlLoop controlLoop) => controlLoops.Remove(controlLoop);
|
||||||
|
|
||||||
public DriveStates DrivesState {
|
public void Initialize()
|
||||||
get {
|
|
||||||
DriveStates lowestState = DriveStates.OPERATIONAL;
|
|
||||||
foreach (DriveController driveController in driveControllers)
|
|
||||||
{
|
|
||||||
DriveStates driveState = driveController.DriveState;
|
|
||||||
if (driveState < lowestState)
|
|
||||||
lowestState = driveState;
|
|
||||||
}
|
|
||||||
return lowestState;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void ControllerThread()
|
|
||||||
{
|
{
|
||||||
while (!stopRequested)
|
CycleCounter = 0;
|
||||||
|
|
||||||
|
if (!(threadWatchdog?.IsAlive ?? false))
|
||||||
{
|
{
|
||||||
foreach (DriveController driveController in driveControllers)
|
threadWatchdog = new Thread(Watchdog);
|
||||||
driveController.UpdateStates();
|
threadWatchdog.Start();
|
||||||
|
}
|
||||||
|
|
||||||
foreach (ControlLoop controlLoop in controlLoops)
|
foreach (DriveController driveController in ECMaster.DriveControllers)
|
||||||
controlLoop.Loop();
|
driveController.Initialize();
|
||||||
|
|
||||||
ControllerLogic?.Invoke(this);
|
}
|
||||||
|
|
||||||
foreach (DriveController driveController in driveControllers)
|
void UpdateControllerState()
|
||||||
driveController.UpdateDrive();
|
{
|
||||||
|
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);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
}
|
}
|
||||||
}
|
}
|
|
@ -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();
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
}
|
|
@ -41,11 +41,30 @@ namespace ln.ethercat.controller.drives
|
||||||
SDOValue svActualTorque;
|
SDOValue svActualTorque;
|
||||||
SDOValue svModesOfOperation;
|
SDOValue svModesOfOperation;
|
||||||
SDOValue svModesOfOperationDisplay;
|
SDOValue svModesOfOperationDisplay;
|
||||||
|
SDOValue svActualCurrent;
|
||||||
|
|
||||||
|
SDOValue svMaxSpeed;
|
||||||
|
|
||||||
|
UInt32 MotorMaxSpeed = 1000;
|
||||||
|
|
||||||
public CIA402Controller(ECMaster ecMaster,UInt16 slave)
|
public CIA402Controller(ECMaster ecMaster,UInt16 slave)
|
||||||
:base(ecMaster, 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()
|
public override void Initialize()
|
||||||
|
@ -61,23 +80,82 @@ namespace ln.ethercat.controller.drives
|
||||||
ECMaster.GetSDOValue(Slave, 0x606C, 0, out svActualSpeed) &&
|
ECMaster.GetSDOValue(Slave, 0x606C, 0, out svActualSpeed) &&
|
||||||
ECMaster.GetSDOValue(Slave, 0x6077, 0, out svActualTorque) &&
|
ECMaster.GetSDOValue(Slave, 0x6077, 0, out svActualTorque) &&
|
||||||
ECMaster.GetSDOValue(Slave, 0x6060, 0, out svModesOfOperation) &&
|
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");
|
throw new ArgumentOutOfRangeException("CIA402Controller could not retrieve SDOvalues for CiA402 profile");
|
||||||
}
|
}
|
||||||
|
|
||||||
public override void UpdateStates() { }
|
public override void UpdateStates() {
|
||||||
public override void UpdateDrive() { }
|
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
|
public override DriveStates DriveState
|
||||||
{
|
{
|
||||||
get {
|
get {
|
||||||
switch (GetCIA402State())
|
switch (CIA402State)
|
||||||
{
|
{
|
||||||
case CIA402States.NOT_READY_TO_SWITCH_ON:
|
case CIA402States.NOT_READY_TO_SWITCH_ON:
|
||||||
|
return DriveStates.INIT;
|
||||||
case CIA402States.SWITCH_ON_DISABLED:
|
case CIA402States.SWITCH_ON_DISABLED:
|
||||||
case CIA402States.READY_TO_SWITCH_ON:
|
case CIA402States.READY_TO_SWITCH_ON:
|
||||||
return DriveStates.INIT;
|
return DriveStates.READY;
|
||||||
case CIA402States.SWITCHED_ON:
|
case CIA402States.SWITCHED_ON:
|
||||||
case CIA402States.QUICK_STOP_ACTIVE:
|
case CIA402States.QUICK_STOP_ACTIVE:
|
||||||
return DriveStates.POWERED;
|
return DriveStates.POWERED;
|
||||||
|
@ -93,7 +171,7 @@ namespace ln.ethercat.controller.drives
|
||||||
}
|
}
|
||||||
|
|
||||||
public override string OEMDriveState {
|
public override string OEMDriveState {
|
||||||
get => GetCIA402State().ToString();
|
get => CIA402State.ToString();
|
||||||
}
|
}
|
||||||
public override string OEMDriveMode => ModeOfOperation.ToString();
|
public override string OEMDriveMode => ModeOfOperation.ToString();
|
||||||
|
|
||||||
|
@ -135,26 +213,27 @@ namespace ln.ethercat.controller.drives
|
||||||
public void SetDriveModeTorque() => DriveMode = DriveMode.TORQUE;
|
public void SetDriveModeTorque() => DriveMode = DriveMode.TORQUE;
|
||||||
|
|
||||||
public CIA402ModesOfOperation ModeOfOperation {
|
public CIA402ModesOfOperation ModeOfOperation {
|
||||||
get => (CIA402ModesOfOperation)(svModesOfOperationDisplay.GetValue<sbyte>());
|
get => (ECMaster.EthercatState==ECSlaveState.OPERATIONAL) ? (CIA402ModesOfOperation)(svModesOfOperationDisplay?.GetValue<sbyte>() ?? 0) : CIA402ModesOfOperation.NO_MODE_CHANGE;
|
||||||
set => svModesOfOperation.SetValue((sbyte)value);
|
set => svModesOfOperation.SetValue((sbyte)value);
|
||||||
}
|
}
|
||||||
|
|
||||||
public override decimal ActualPosition => svActualPosition.GetValue<int>();
|
public override double ActualPosition => (svActualPosition?.GetValue<int>() ?? 0) ;
|
||||||
public override decimal ActualSpeed => svActualSpeed.GetValue<int>();
|
public override double ActualSpeed => (double)(svActualSpeed?.GetValue<int>() ?? 0) / (1000.0 * MotorMaxSpeed);
|
||||||
public override decimal ActualTorque => svActualTorque.GetValue<short>();
|
public override double ActualTorque => (double)(svActualTorque?.GetValue<short>() ?? 0) / 1000.0;
|
||||||
|
public override double ActualLoad => (double)(svActualCurrent?.GetValue<short>() ?? 0) / 1000.0;
|
||||||
|
|
||||||
public override decimal TargetPosition {
|
public override double TargetPosition {
|
||||||
get => svTargetPosition.GetValue<int>();
|
get => svTargetPosition.GetValue<int>();
|
||||||
set => svTargetPosition.SetValue((int)value);
|
set => svTargetPosition.SetValue((int)value);
|
||||||
}
|
}
|
||||||
|
|
||||||
public override decimal TargetSpeed {
|
public override double TargetSpeed {
|
||||||
get => svTargetSpeed.GetValue<int>();
|
get => (double)svTargetSpeed.GetValue<int>() / (1000.0 * MotorMaxSpeed);
|
||||||
set => svTargetSpeed.SetValue((int)value);
|
set => svTargetSpeed.SetValue((int)(value * (1000.0 * MotorMaxSpeed)));
|
||||||
}
|
}
|
||||||
public override decimal TargetTorque {
|
public override double TargetTorque {
|
||||||
get => svTargetTorque.GetValue<short>();
|
get => (double)svTargetTorque.GetValue<short>() / 1000.0;
|
||||||
set => svTargetTorque.SetValue((short)value);
|
set => svTargetTorque.SetValue((short)(1000 * value));
|
||||||
}
|
}
|
||||||
|
|
||||||
public override int ErrorCode => svErrorCode.GetValue<int>();
|
public override int ErrorCode => svErrorCode.GetValue<int>();
|
||||||
|
@ -163,51 +242,12 @@ namespace ln.ethercat.controller.drives
|
||||||
|
|
||||||
public override void Enable(bool enable)
|
public override void Enable(bool enable)
|
||||||
{
|
{
|
||||||
if (enable)
|
CIA402TargetState = enable ? CIA402States.OPERATION_ENABLED : CIA402States.READY_TO_SWITCH_ON;
|
||||||
{
|
|
||||||
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)
|
public override void Power(bool poweron)
|
||||||
{
|
{
|
||||||
if (poweron)
|
CIA402TargetState = poweron ? CIA402States.SWITCHED_ON : CIA402States.READY_TO_SWITCH_ON;
|
||||||
{
|
|
||||||
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()
|
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()
|
public CIA402States GetCIA402State()
|
||||||
{
|
{
|
||||||
UInt16 statusword = svStatusWord.GetValue<ushort>();
|
UInt16 statusword = svStatusWord?.GetValue<ushort>() ?? 0;
|
||||||
|
|
||||||
if ((statusword & 0x004F)==0)
|
if ((statusword & 0x004F)==0)
|
||||||
return CIA402States.NOT_READY_TO_SWITCH_ON;
|
return CIA402States.NOT_READY_TO_SWITCH_ON;
|
||||||
|
|
|
@ -1,6 +1,7 @@
|
||||||
|
|
||||||
|
|
||||||
using System;
|
using System;
|
||||||
|
using System.Net.Http.Headers;
|
||||||
using System.Runtime;
|
using System.Runtime;
|
||||||
|
|
||||||
namespace ln.ethercat.controller.drives
|
namespace ln.ethercat.controller.drives
|
||||||
|
@ -9,6 +10,7 @@ namespace ln.ethercat.controller.drives
|
||||||
UNDEFINED,
|
UNDEFINED,
|
||||||
BOOT,
|
BOOT,
|
||||||
INIT,
|
INIT,
|
||||||
|
READY,
|
||||||
ERROR,
|
ERROR,
|
||||||
POWERED,
|
POWERED,
|
||||||
OPERATIONAL,
|
OPERATIONAL,
|
||||||
|
@ -26,6 +28,9 @@ namespace ln.ethercat.controller.drives
|
||||||
protected ECMaster ECMaster { get; }
|
protected ECMaster ECMaster { get; }
|
||||||
public UInt16 Slave { get; }
|
public UInt16 Slave { get; }
|
||||||
|
|
||||||
|
public bool IgnoredByController { get; set; }
|
||||||
|
|
||||||
|
|
||||||
public DriveController(ECMaster ecMaster, UInt16 slave)
|
public DriveController(ECMaster ecMaster, UInt16 slave)
|
||||||
{
|
{
|
||||||
ECMaster = ecMaster;
|
ECMaster = ecMaster;
|
||||||
|
@ -57,13 +62,46 @@ namespace ln.ethercat.controller.drives
|
||||||
public void DisableDrive() => Enable(false);
|
public void DisableDrive() => Enable(false);
|
||||||
public abstract void Enable(bool enabled);
|
public abstract void Enable(bool enabled);
|
||||||
|
|
||||||
public abstract decimal ActualPosition { get; }
|
public abstract double ActualPosition { get; }
|
||||||
public abstract decimal ActualSpeed { get; }
|
public abstract double ActualSpeed { get; }
|
||||||
public abstract decimal ActualTorque { get; }
|
public abstract double ActualTorque { get; }
|
||||||
public abstract decimal TargetPosition { get; set; }
|
public abstract double TargetPosition { get; set; }
|
||||||
public abstract decimal TargetSpeed { get; set; }
|
public abstract double TargetSpeed { get; set; }
|
||||||
public abstract decimal TargetTorque { 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;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -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();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
Binary file not shown.
|
@ -16,6 +16,7 @@
|
||||||
</ItemGroup>
|
</ItemGroup>
|
||||||
|
|
||||||
<ItemGroup>
|
<ItemGroup>
|
||||||
|
<PackageReference Include="System.IO.Ports" Version="5.0.0" />
|
||||||
<PackageReference Include="ln.type" Version="0.1.7-ci" />
|
<PackageReference Include="ln.type" Version="0.1.7-ci" />
|
||||||
<PackageReference Include="ln.logging" Version="1.0.2" />
|
<PackageReference Include="ln.logging" Version="1.0.2" />
|
||||||
<PackageReference Include="ln.json" Version="1.0.6" />
|
<PackageReference Include="ln.json" Version="1.0.6" />
|
||||||
|
|
Loading…
Reference in New Issue