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;
|
||||
|
||||
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);
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
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);
|
||||
*/
|
||||
|
|
|
@ -189,7 +189,7 @@ int ecmbind_read_objectdescription(int Slave, int index, cb_enum_sdo_descriptors
|
|||
strncpy(temp , (char *)&aSDOp->bdata[6], n);
|
||||
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 */
|
||||
else
|
||||
|
@ -265,7 +265,7 @@ int ecmbind_read_objectdescription_entry(uint16_t slave, uint16_t index, uint16_
|
|||
strncpy(temp , (char *)&aSDOp->wdata[5], n);
|
||||
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];
|
||||
|
|
|
@ -17,8 +17,6 @@ namespace ln.ethercat.service
|
|||
{
|
||||
|
||||
public ECMaster ECMaster { get; private set; }
|
||||
public Controller Controller { get; private set; }
|
||||
|
||||
|
||||
[StaticArgument(Option = 'i', LongOption = "interface")]
|
||||
public string EthercatInterfaceName { get; set; }
|
||||
|
@ -72,14 +70,9 @@ namespace ln.ethercat.service
|
|||
httpServer = new HTTPServer(httpLoggingRouter);
|
||||
httpServer.AddEndpoint(new Endpoint(IPv6.ANY, 7676));
|
||||
|
||||
Controller = new Controller();
|
||||
Controller.Add(new CIA402Controller(ECMaster, 1));
|
||||
Controller.Add(new CIA402Controller(ECMaster, 2));
|
||||
|
||||
controllerApiController = new ControllerApiController(Controller);
|
||||
controllerApiController = new ControllerApiController(ECMaster.Controller);
|
||||
httpRouter.AddSimpleRoute("/api/v1/*", controllerApiController);
|
||||
|
||||
|
||||
timerWebsockets = new System.Timers.Timer(250);
|
||||
timerWebsockets.Elapsed += WebSocketTimerMethod;
|
||||
}
|
||||
|
@ -97,6 +90,7 @@ namespace ln.ethercat.service
|
|||
{
|
||||
try{
|
||||
EthercatWebSocket.SendProcessData(ECMaster);
|
||||
ControllerWebSocket.SendUpdates(ECMaster.Controller);
|
||||
} catch (Exception ex)
|
||||
{
|
||||
Logging.Log(ex);
|
||||
|
@ -108,10 +102,10 @@ namespace ln.ethercat.service
|
|||
switch (newState)
|
||||
{
|
||||
case ECSlaveState.OPERATIONAL:
|
||||
Controller.Start();
|
||||
//Controller.Start();
|
||||
break;
|
||||
default:
|
||||
Controller.Stop();
|
||||
//Controller.Stop();
|
||||
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.Data;
|
||||
using System.Text;
|
||||
using System.Threading;
|
||||
using ln.application;
|
||||
using ln.ethercat.controller;
|
||||
using ln.ethercat.controller.drives;
|
||||
using ln.ethercat.controller.remote;
|
||||
using ln.logging;
|
||||
using ln.type;
|
||||
|
||||
|
@ -11,6 +13,9 @@ namespace ln.ethercat.service
|
|||
{
|
||||
class Program
|
||||
{
|
||||
[StaticArgument(LongOption = "serial-remote")]
|
||||
static string SerialRemotePort { get; set; }
|
||||
|
||||
static void Main(string[] args)
|
||||
{
|
||||
Logging.Log(LogLevel.INFO, ".NET EtherCAT service host");
|
||||
|
@ -23,11 +28,52 @@ namespace ln.ethercat.service
|
|||
EthercatService ethercatService = new EthercatService(args[0]);
|
||||
|
||||
ArgumentContainer argumentContainer = new ArgumentContainer();
|
||||
argumentContainer.AddStaticOptions<Program>();
|
||||
argumentContainer.AddOptions(ethercatService);
|
||||
argumentContainer.Parse(ref args);
|
||||
|
||||
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();
|
||||
|
||||
|
||||
if (SerialRemotePort != null)
|
||||
{
|
||||
StupidSerialRemote stupidSerialRemote = new StupidSerialRemote(ethercatService.ECMaster.Controller, SerialRemotePort);
|
||||
stupidSerialRemote.Start();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
@ -1,5 +1,3 @@
|
|||
|
||||
|
||||
using System;
|
||||
using System.Globalization;
|
||||
using System.Linq;
|
||||
|
@ -30,93 +28,61 @@ namespace ln.ethercat.service.api.v1
|
|||
|
||||
|
||||
[GET("/sockets/controller")]
|
||||
public HttpResponse GetControllerSocket()
|
||||
{
|
||||
Timer timer = new Timer(250);
|
||||
JSONWebSocketResponse websocket = new JSONWebSocketResponse();
|
||||
websocket.OnWebSocketStateChanged += (Socket, newstate) => {
|
||||
if (newstate == WebSocketState.CLOSED)
|
||||
{
|
||||
timer.Stop();
|
||||
timer.Dispose();
|
||||
}
|
||||
};
|
||||
public HttpResponse GetControllerSocket() => new ControllerWebSocket(Controller);
|
||||
|
||||
timer.Elapsed += (s,e) => {
|
||||
try{
|
||||
JSONObject controllerState = new JSONObject()
|
||||
.Add("DriveControllers", new JSONArray().Add(Controller.DriveControllers.Select((dc=> new JSONObject()
|
||||
.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);
|
||||
}
|
||||
};
|
||||
// [GET("/sockets/controller/drives/:drive")]
|
||||
// public HttpResponse GetDriveControllerSocket(int drive)
|
||||
// {
|
||||
// DriveController driveController = Controller.DriveControllers[drive];
|
||||
|
||||
timer.Start();
|
||||
return websocket;
|
||||
}
|
||||
// Timer timer = new Timer(250);
|
||||
// JSONWebSocketResponse websocket = new JSONWebSocketResponse();
|
||||
// websocket.OnWebSocketStateChanged += (Socket, newstate) => {
|
||||
// if (newstate == WebSocketState.CLOSED)
|
||||
// {
|
||||
// timer.Stop();
|
||||
// timer.Dispose();
|
||||
// }
|
||||
// };
|
||||
|
||||
[GET("/sockets/controller/drives/:drive")]
|
||||
public HttpResponse GetDriveControllerSocket(int drive)
|
||||
{
|
||||
DriveController driveController = Controller.DriveControllers[drive];
|
||||
// websocket.OnWebSocketReceivedText += (s,text) => {
|
||||
// JSONObject message = (JSONObject)JSONParser.Parse(text);
|
||||
// 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 timer = new Timer(250);
|
||||
JSONWebSocketResponse websocket = new JSONWebSocketResponse();
|
||||
websocket.OnWebSocketStateChanged += (Socket, newstate) => {
|
||||
if (newstate == WebSocketState.CLOSED)
|
||||
{
|
||||
timer.Stop();
|
||||
timer.Dispose();
|
||||
}
|
||||
};
|
||||
// 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);
|
||||
// }
|
||||
// };
|
||||
|
||||
websocket.OnWebSocketReceivedText += (s,text) => {
|
||||
JSONObject message = (JSONObject)JSONParser.Parse(text);
|
||||
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;
|
||||
}
|
||||
// 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.Collections.Generic;
|
||||
using System.Linq;
|
||||
using System.Timers;
|
||||
using ln.http;
|
||||
using ln.http.api;
|
||||
using ln.http.api.attributes;
|
||||
using ln.http.websocket;
|
||||
using ln.json;
|
||||
using ln.json.mapping;
|
||||
using ln.logging;
|
||||
|
||||
namespace ln.ethercat.service.api.v1
|
||||
{
|
||||
|
||||
public class EthercatApiController : WebApiController
|
||||
{
|
||||
ECMaster ECMaster => EthercatService.ECMaster;
|
||||
|
@ -51,14 +44,6 @@ namespace ln.ethercat.service.api.v1
|
|||
[GET("/slaves/:slave/state")]
|
||||
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")]
|
||||
public HttpResponse GetFullState()
|
||||
|
|
|
@ -10,6 +10,7 @@ using ln.http.websocket;
|
|||
using ln.json;
|
||||
using ln.json.mapping;
|
||||
using ln.logging;
|
||||
using ln.type;
|
||||
|
||||
namespace ln.ethercat.service.api.v1
|
||||
{
|
||||
|
@ -17,9 +18,19 @@ namespace ln.ethercat.service.api.v1
|
|||
{
|
||||
/************** Static ***************************************************************************/
|
||||
static List<EthercatWebSocket> activeWebSockets = new List<EthercatWebSocket>();
|
||||
static bool broadcastActive;
|
||||
|
||||
public static void SendProcessData(ECMaster ecMaster)
|
||||
{
|
||||
lock (activeWebSockets)
|
||||
{
|
||||
if (broadcastActive)
|
||||
return;
|
||||
broadcastActive = true;
|
||||
}
|
||||
|
||||
try{
|
||||
|
||||
JSONArray jsonPDOList = new JSONArray();
|
||||
|
||||
foreach (PDO pdo in ecMaster.GetPDOMap())
|
||||
|
@ -49,13 +60,31 @@ namespace ln.ethercat.service.api.v1
|
|||
);
|
||||
string stateMessageText = stateMessage.ToString();
|
||||
|
||||
foreach (EthercatWebSocket webSocket in activeWebSockets)
|
||||
EthercatWebSocket[] sockets;
|
||||
lock(activeWebSockets)
|
||||
sockets = activeWebSockets.ToArray();
|
||||
|
||||
foreach (EthercatWebSocket webSocket in sockets)
|
||||
{
|
||||
if (webSocket.State != WebSocketState.OPEN)
|
||||
{
|
||||
lock (activeWebSockets)
|
||||
activeWebSockets.Remove(webSocket);
|
||||
} else
|
||||
{
|
||||
webSocket.Send(messageText);
|
||||
webSocket.Send(stateMessageText);
|
||||
webSocket.SendSubscribedSDOs();
|
||||
}
|
||||
}
|
||||
} catch (Exception e)
|
||||
{
|
||||
Logging.Log(e);
|
||||
} finally
|
||||
{
|
||||
broadcastActive = false;
|
||||
}
|
||||
}
|
||||
|
||||
public static JSONObject Message(string eventName, JSONValue value) =>
|
||||
new JSONObject()
|
||||
|
@ -93,10 +122,12 @@ namespace ln.ethercat.service.api.v1
|
|||
{
|
||||
case WebSocketState.OPEN:
|
||||
Logging.Log(LogLevel.DEBUG, "EthercatWebSocket -> active");
|
||||
lock(activeWebSockets)
|
||||
activeWebSockets.Add(this);
|
||||
break;
|
||||
case WebSocketState.CLOSED:
|
||||
default:
|
||||
Logging.Log(LogLevel.DEBUG, "EthercatWebSocket -> inactive");
|
||||
lock(activeWebSockets)
|
||||
activeWebSockets.Remove(this);
|
||||
break;
|
||||
}
|
||||
|
@ -127,6 +158,28 @@ namespace ln.ethercat.service.api.v1
|
|||
break;
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
|
|
@ -50,7 +50,11 @@
|
|||
<router-view></router-view>
|
||||
</div>
|
||||
<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>
|
||||
</main>
|
||||
<footer>
|
||||
|
|
|
@ -49,13 +49,13 @@ fieldset {
|
|||
display: flex;
|
||||
}
|
||||
|
||||
fieldset > input {
|
||||
flex-grow: 1;
|
||||
}
|
||||
fieldset > label {
|
||||
flex-grow: 0;
|
||||
}
|
||||
|
||||
fieldset > input, fieldset > .value {
|
||||
flex-grow: 1;
|
||||
}
|
||||
|
||||
.panel {
|
||||
padding: 6px;
|
||||
|
|
|
@ -24,6 +24,10 @@ class EthercatApplication
|
|||
this.base_url = scheme + "//" + host;
|
||||
|
||||
this.connectMainSocket();
|
||||
|
||||
window.onunload = () => {
|
||||
this.closeWebSockets();
|
||||
};
|
||||
}
|
||||
|
||||
connectMainSocket(){
|
||||
|
@ -49,6 +53,7 @@ class EthercatApplication
|
|||
this.socket.onerror = (evt)=>{
|
||||
console.log("weboscket error: ", evt);
|
||||
this.socket.close();
|
||||
this.socket = null;
|
||||
};
|
||||
this.socket.onclose = (evt)=>{
|
||||
console.log("weboscket closed: ", evt);
|
||||
|
@ -72,6 +77,7 @@ class EthercatApplication
|
|||
{
|
||||
let ws = this.ac_sockets.pop();
|
||||
console.log("disposing:", ws);
|
||||
ws.onclose = null;
|
||||
ws.close();
|
||||
}
|
||||
}
|
||||
|
|
|
@ -12,12 +12,26 @@
|
|||
data: function(){
|
||||
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: `
|
||||
<div class="panel">
|
||||
<h2>Prozessdaten</h2>
|
||||
<article v-for="sdo,key in processdata">
|
||||
<div>{{sdo.Descriptor.Index.toString(16).toUpperCase()}}.{{sdo.SubIndex}} {{ sdo.Name }}</div>
|
||||
<div class="value">{{ sdo.Value }}</div>
|
||||
<div>{{sdo.Descriptor.SlaveId}}:{{sdo.Descriptor.Index.toString(16).toUpperCase()}}.{{sdo.SubIndex}} {{ sdo.Name }} ({{sdo.DataType}})</div>
|
||||
<div
|
||||
class="value"
|
||||
@dblclick="promptNewValue(sdo.Descriptor.SlaveId, sdo.Descriptor.Index, sdo.SubIndex, sdo.DataType, sdo.Value);"
|
||||
>{{ sdo.Value }}</div>
|
||||
</article>
|
||||
</div>
|
||||
`,
|
||||
|
|
|
@ -13,18 +13,32 @@
|
|||
data: function(){
|
||||
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: `
|
||||
<div class="panel">
|
||||
<h2>SDO Panel</h2>
|
||||
<article v-for="sdo,key in subscribed">
|
||||
<h2>{{sdo.Index.toString(16).toUpperCase()}} {{ sdo.Name }} ({{ sdo.DataType}} / M:{{ sdo.MaxSubIndex }})</h2>
|
||||
<ul>
|
||||
<div
|
||||
<fieldset
|
||||
v-for="sdovalue in sdo.Values"
|
||||
>
|
||||
<span>.{{sdovalue.SubIndex}} {{ sdovalue.Name }} ({{ sdo.DataType}})</span>
|
||||
<input :disabled="(sdovalue.DataType == 'NONE')" type="text" :value="sdovalue.Value">
|
||||
</div>
|
||||
<span>.{{sdovalue.SubIndex}} {{ sdovalue.Name }} ({{ sdovalue.DataType}})</span>
|
||||
<div
|
||||
class="value"
|
||||
@dblclick="promptNewValue(sdo.SlaveId, sdo.Index, sdovalue.SubIndex, sdovalue.DataType, sdovalue.Value);"
|
||||
>{{ sdovalue.Value }}</div>
|
||||
</fieldset>
|
||||
</ul>
|
||||
</article>
|
||||
</div>
|
||||
|
|
|
@ -16,20 +16,35 @@
|
|||
datatype: null,
|
||||
},
|
||||
selectedIndex: null,
|
||||
manualIndex: "",
|
||||
};
|
||||
},
|
||||
methods: {
|
||||
getFilteredDescriptors: function(){
|
||||
return this.descriptors.filter((sdo)=>
|
||||
(!this.filters.name || sdo.name.toLowerCase().includes(this.filters.name.toLowerCase())) ); // &&
|
||||
if (this.filters.name)
|
||||
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.datatype || sdo.datatype.toLowerCase().includes(this.filters.datatype.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: `
|
||||
<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">
|
||||
<select size="15" v-model="selectedIndex" @dblclick="if (selectedIndex>=0) $emit('selected', selectedIndex);">
|
||||
<option
|
||||
|
|
|
@ -3,11 +3,6 @@
|
|||
<p>
|
||||
{{ controller }}
|
||||
</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">
|
||||
<h2>Drive Controller</h2>
|
||||
<button @click="drive_poweroff">Power Off</button>
|
||||
|
@ -27,9 +22,9 @@
|
|||
<div>
|
||||
<fieldset>
|
||||
<label>Target Torque:</label>
|
||||
<input type="range" min="-1000" max="1000" v-model="targetTorque" id="targetTorque" @input="setProperty('TargetTorque', targetTorque);">
|
||||
<input type="number" v-model="targetTorque" @change="setProperty('TargetTorque', targetTorque);">
|
||||
<button @click="targetTorque = 0; setProperty('TargetTorque', targetTorque);">ZERO</button>
|
||||
<input type="range" min="-1000" max="1000" v-model="targetTorque" id="targetTorque" @input="setProperty('TargetValue', targetTorque);">
|
||||
<input type="number" v-model="targetTorque" @change="setProperty('TargetValue', targetTorque);">
|
||||
<button @click="targetTorque = 0; setProperty('TargetValue', targetTorque);">ZERO</button>
|
||||
</fieldset>
|
||||
</div>
|
||||
|
||||
|
|
|
@ -1,4 +1,6 @@
|
|||
<div>
|
||||
<h1>Prozessdaten</h1>
|
||||
<sdo-table :sdolist="this.processdata"></sdo-table>
|
||||
<sdo-table
|
||||
:sdolist="this.processdata"
|
||||
></sdo-table>
|
||||
</div>
|
|
@ -7,6 +7,9 @@
|
|||
@selected="$ECAPP.subscribe(slave_id, $event);"
|
||||
></sdo-select>
|
||||
</div>
|
||||
<sdo-panel :subscribed="$ECAPP.subscribed.sdo"></sdo-panel>
|
||||
<sdo-panel
|
||||
:subscribed="$ECAPP.subscribed.sdo"
|
||||
@write="$ECAPP.sendMessage('sdowrite', $event);"
|
||||
></sdo-panel>
|
||||
</div>
|
||||
</div>
|
|
@ -159,7 +159,7 @@ namespace ln.ethercat
|
|||
));
|
||||
|
||||
AddConverter(ECDataTypes.NONE, new ECValueConverterAdapter(
|
||||
typeof(byte[]),
|
||||
typeof(string),
|
||||
(object value, out byte[] ethercatBytes) => { ethercatBytes = Extensions.BytesFromHexString(value as string); return true; },
|
||||
(byte[] ethercatBytes, out object value) => { value = ethercatBytes.ToHexString(); return true; }
|
||||
));
|
||||
|
|
|
@ -41,7 +41,7 @@ namespace ln.ethercat
|
|||
}
|
||||
|
||||
public delegate void cb_enum_indeces(int slave, int index);
|
||||
public delegate void cb_enum_sdo_descriptors(int slave, int index, ECDataTypes dataType, ECObjectCodes objectCode, int maxsub, String name);
|
||||
public delegate void cb_enum_sdo_descriptors(int slave, int index, ECDataTypes dataType, ECObjectCodes objectCode, int maxsub, ushort bitlength, String name);
|
||||
public delegate void cb_enum_pdo(UInt16 slave, UInt16 index, byte subindex, int addr_offset, int addr_bit, int bitlength);
|
||||
|
||||
|
||||
|
@ -106,6 +106,12 @@ namespace ln.ethercat
|
|||
[DllImport("lib/libecmbind.so")]
|
||||
public static extern int ecmbind_sdo_write(int slave, int index, int subindex, byte[] buffer, int size);
|
||||
|
||||
[DllImport("lib/libecmbind.so")]
|
||||
public static extern int ecmbind_sdo_read_ca(int slave, int index, int subindex, byte[] buffer, int size);
|
||||
|
||||
[DllImport("lib/libecmbind.so")]
|
||||
public static extern int ecmbind_sdo_write_ca(int slave, int index, int subindex, byte[] buffer, int size);
|
||||
|
||||
|
||||
|
||||
[DllImport("lib/libecmbind.so")]
|
||||
|
|
|
@ -1,13 +1,13 @@
|
|||
|
||||
|
||||
using System;
|
||||
using System.Buffers;
|
||||
using System.Collections.Generic;
|
||||
using System.Diagnostics;
|
||||
using System.IO;
|
||||
using System.Linq;
|
||||
using System.Reflection.Metadata;
|
||||
using System.Security.Cryptography.X509Certificates;
|
||||
using System.Threading;
|
||||
using ln.ethercat.controller;
|
||||
using ln.ethercat.controller.drives;
|
||||
using ln.logging;
|
||||
using ln.type;
|
||||
|
||||
|
@ -21,6 +21,23 @@ namespace ln.ethercat
|
|||
RUNNING,
|
||||
}
|
||||
|
||||
public struct PDOMappingRequest
|
||||
{
|
||||
public ushort Slave;
|
||||
public ushort Index;
|
||||
public byte SubIndex;
|
||||
public bool RxPDO;
|
||||
|
||||
public PDOMappingRequest(ushort slave,ushort index,byte subIndex,bool rxpdo)
|
||||
{
|
||||
Slave = slave;
|
||||
Index = index;
|
||||
SubIndex = subIndex;
|
||||
RxPDO = rxpdo;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
public delegate void ECStateChange(ECMaster sender,ECSlaveState newState);
|
||||
|
||||
public class ECMaster
|
||||
|
@ -47,8 +64,8 @@ namespace ln.ethercat
|
|||
|
||||
ECSlaveState ethercatState = ECSlaveState.NONE;
|
||||
public ECSlaveState EthercatState {
|
||||
get => UpdateEthercatState();
|
||||
set {
|
||||
get =>ethercatState;
|
||||
private set {
|
||||
if (value != ethercatState)
|
||||
{
|
||||
OnStateChange?.Invoke(this, value);
|
||||
|
@ -73,113 +90,168 @@ namespace ln.ethercat
|
|||
Logging.Log(LogLevel.INFO, "ecmbind_initialize({0}) = {1}", interfaceName, result);
|
||||
if (result<=0)
|
||||
throw new Exception("ecmbind_initialize failed");
|
||||
|
||||
Controller = new Controller(this);
|
||||
}
|
||||
|
||||
bool stopProcessing;
|
||||
public bool StopProccessing {
|
||||
get => stopProcessing;
|
||||
set => stopProcessing = value;
|
||||
}
|
||||
bool stopWatchdog;
|
||||
bool stopProcessData;
|
||||
|
||||
Thread threadProcessData;
|
||||
Thread threadWatchdog;
|
||||
|
||||
public bool Start()
|
||||
public Controller Controller { get; }
|
||||
List<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)
|
||||
{
|
||||
CountSlaves = ECMBind.ecmbind_config_init();
|
||||
if (CountSlaves <= 0)
|
||||
{
|
||||
Logging.Log(LogLevel.DEBUG, "ECMaster: no slaves connected");
|
||||
return false;
|
||||
}
|
||||
if (threadWatchdog?.IsAlive ?? false)
|
||||
throw new Exception("already started");
|
||||
|
||||
if (!CheckState(ECSlaveState.PRE_OP, TIMEOUT_PREOP))
|
||||
return false;
|
||||
ethercatState = ECSlaveState.NONE;
|
||||
ExpectedWorkCounter = 0;
|
||||
|
||||
IOMapSize = ECMBind.ecmbind_config_map();
|
||||
Logging.Log(LogLevel.DEBUG, "ECMaster: IOMapSize={0}", IOMapSize);
|
||||
|
||||
UpdatePDOMap();
|
||||
|
||||
threadProcessData = new Thread(MasterThread);
|
||||
threadProcessData.Start();
|
||||
|
||||
Thread.Sleep(20);
|
||||
|
||||
if (!RequestState(ECSlaveState.SAFE_OP, out ECSlaveState slaveState, TIMEOUT_SAFEOP))
|
||||
{
|
||||
Stop();
|
||||
return false;
|
||||
}
|
||||
stopWatchdog = false;
|
||||
|
||||
threadWatchdog = new Thread(Watchdog);
|
||||
threadWatchdog.Start();
|
||||
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
public void Stop()
|
||||
{
|
||||
if (threadProcessData?.IsAlive ?? false)
|
||||
lock (this)
|
||||
{
|
||||
stopProcessing = true;
|
||||
ECMBind.ecmbind_request_state(0, ECSlaveState.SAFE_OP, TIMEOUT_BACKTO_SAFEOP);
|
||||
|
||||
threadProcessData.Join();
|
||||
threadProcessData = null;
|
||||
|
||||
if (!Thread.CurrentThread.Equals(threadWatchdog))
|
||||
if (threadWatchdog?.IsAlive ?? false)
|
||||
{
|
||||
threadWatchdog?.Join();
|
||||
threadWatchdog = null;
|
||||
}
|
||||
}
|
||||
}
|
||||
stopWatchdog = true;
|
||||
|
||||
lock (this)
|
||||
Monitor.PulseAll(this);
|
||||
|
||||
object lockIOMap = new object();
|
||||
byte[] iomap = new byte[8192];
|
||||
threadWatchdog.Join();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Watchdog()
|
||||
{
|
||||
// if (!ReadSDOIndeces())
|
||||
// {
|
||||
// Logging.Log(LogLevel.WARNING, "ECMaster: could not read SDO indeces");
|
||||
// }
|
||||
|
||||
if (!RequestState(ECSlaveState.OPERATIONAL, out ECSlaveState slaveState, TIMEOUT_SAFEOP))
|
||||
while (!stopWatchdog)
|
||||
{
|
||||
if (slaveState < ECSlaveState.SAFE_OP)
|
||||
Stop();
|
||||
IOMapSize = 0;
|
||||
if (threadProcessData?.IsAlive ?? false)
|
||||
{
|
||||
stopProcessData = true;
|
||||
while (threadProcessData.IsAlive)
|
||||
threadProcessData.Join(250);
|
||||
}
|
||||
stopProcessData = false;
|
||||
|
||||
EthercatState = ECSlaveState.INIT;
|
||||
|
||||
CountSlaves = ECMBind.ecmbind_config_init();
|
||||
if (CountSlaves <= 0)
|
||||
{
|
||||
Logging.Log(LogLevel.DEBUG, "ECMaster: no slaves connected, scheduling restart...");
|
||||
Thread.Sleep(2500);
|
||||
continue;
|
||||
}
|
||||
|
||||
while (!stopProcessing)
|
||||
|
||||
if (!WaitForState(ECSlaveState.PRE_OP, TIMEOUT_PREOP))
|
||||
{
|
||||
UpdateEthercatState();
|
||||
Logging.Log(LogLevel.DEBUG, "ECMaster: slaves did not reach PRE_OP. restart...");
|
||||
Thread.Sleep(2500);
|
||||
continue;
|
||||
}
|
||||
|
||||
|
||||
/* Bus State PRE_OP */
|
||||
ScanControllers();
|
||||
|
||||
EthercatState = ECSlaveState.PRE_OP;
|
||||
|
||||
ConfigureProcessDataMappings();
|
||||
SetupPDOMapping();
|
||||
UpdatePDOMap();
|
||||
|
||||
threadProcessData = new Thread(ProcessData);
|
||||
threadProcessData.Start();
|
||||
|
||||
Thread.Sleep(20);
|
||||
RequestSlaveState(0, ECSlaveState.SAFE_OP);
|
||||
|
||||
if (!WaitForState(ECSlaveState.SAFE_OP, TIMEOUT_SAFEOP))
|
||||
{
|
||||
Logging.Log(LogLevel.DEBUG, "ECMaster: slaves did not reach SAFE_OP. restart...");
|
||||
Thread.Sleep(2500);
|
||||
continue;
|
||||
}
|
||||
|
||||
EthercatState = ECSlaveState.SAFE_OP;
|
||||
|
||||
Thread.Sleep(20);
|
||||
RequestSlaveState(0, ECSlaveState.OPERATIONAL);
|
||||
|
||||
Thread.Sleep(250);
|
||||
}
|
||||
}
|
||||
|
||||
void MasterThread()
|
||||
if (!WaitForState(ECSlaveState.OPERATIONAL, TIMEOUT_SAFEOP))
|
||||
{
|
||||
ExpectedWorkCounter = ECMBind.ecmbind_get_expected_wkc_size();
|
||||
Logging.Log(LogLevel.DEBUG, "ECMaster: slaves did not reach OPERATIONAL. restart...");
|
||||
Thread.Sleep(2500);
|
||||
continue;
|
||||
}
|
||||
|
||||
while (!stopProcessing)
|
||||
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 ProcessData()
|
||||
{
|
||||
int wkc;
|
||||
|
||||
lock (lockIOMap)
|
||||
ExpectedWorkCounter = ECMBind.ecmbind_get_expected_wkc_size();
|
||||
|
||||
Logging.Log(LogLevel.INFO, "ECMaster: ProcessData(): start");
|
||||
|
||||
while (!stopProcessData)
|
||||
{
|
||||
lock (lockIOMap)
|
||||
{
|
||||
wkc = ECMBind.ecmbind_processdata2(iomap, iomap.Length);
|
||||
|
@ -187,26 +259,160 @@ namespace ln.ethercat
|
|||
{
|
||||
int success = ECMBind.ecmbind_recover();
|
||||
if (success > 0)
|
||||
ExpectedWorkCounter = success;
|
||||
else
|
||||
{
|
||||
RequestSlaveState(0, ECSlaveState.SAFE_OP);
|
||||
Logging.Log(LogLevel.ERROR, "ECMaster: bus recovery successfull...");
|
||||
ExpectedWorkCounter = success;
|
||||
//SetupPDOMapping();
|
||||
} else
|
||||
{
|
||||
RequestSlaveState(0, ECSlaveState.INIT);
|
||||
Logging.Log(LogLevel.ERROR, "ECMaster: bus recovery failed, scheduling restart...");
|
||||
ScheduleRestart();
|
||||
stopProcessData = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
Thread.Sleep(INTERVALL_PROCESSDATA);
|
||||
}
|
||||
|
||||
/*
|
||||
if (wkc != ExpectedWorkCounter)
|
||||
Logging.Log(LogLevel.INFO, "ECMaster: ProcessData(): finished");
|
||||
}
|
||||
|
||||
public void ScanControllers()
|
||||
{
|
||||
ExpectedWorkCounter = ECMBind.ecmbind_recover();
|
||||
if (ExpectedWorkCounter < 0)
|
||||
driveControllers.Clear();
|
||||
|
||||
for (UInt16 slave_id = 1; slave_id <= CountSlaves; slave_id++)
|
||||
{
|
||||
if (ReadSDO(slave_id, 0x1000, 0, out byte[] bDeviceType))
|
||||
{
|
||||
ushort profilecode = BitConverter.ToUInt16(bDeviceType);
|
||||
Logging.Log(LogLevel.INFO, "ECMaster: ScanControllers: Slave {0} has DeviceType {1} [{2}]", slave_id, profilecode, bDeviceType.ToHexString());
|
||||
|
||||
switch (profilecode)
|
||||
{
|
||||
case 402:
|
||||
driveControllers.Add(new CIA402Controller(this, slave_id));
|
||||
break;
|
||||
}
|
||||
*/
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
bool WaitForState(ECSlaveState expectedState, int timeout)
|
||||
{
|
||||
int waitIncrement = 100;
|
||||
|
||||
ECSlaveState currentState = IOLocked(()=>ECMBind.ecmbind_read_state());
|
||||
while ((timeout > 0) && (currentState != expectedState))
|
||||
{
|
||||
Thread.Sleep(waitIncrement);
|
||||
timeout -= waitIncrement;
|
||||
currentState = IOLocked(()=>ECMBind.ecmbind_read_state());
|
||||
}
|
||||
return currentState == expectedState;
|
||||
}
|
||||
|
||||
|
||||
|
||||
object lockIOMap = new object();
|
||||
byte[] iomap = new byte[8192];
|
||||
|
||||
void SetupPDOMapping()
|
||||
{
|
||||
IOMapSize = ECMBind.ecmbind_config_map();
|
||||
Logging.Log(LogLevel.DEBUG, "ECMaster: IOMapSize={0}", IOMapSize);
|
||||
//UpdatePDOMap();
|
||||
}
|
||||
|
||||
HashSet<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)
|
||||
{
|
||||
|
@ -220,7 +426,27 @@ namespace ln.ethercat
|
|||
ECMBind.ecmbind_sdo_read(slave, index, subIndex, rawValue, rawValue.Length );
|
||||
if (dataSize <= 0)
|
||||
{
|
||||
Logging.Log(LogLevel.ERROR, "ECMBind.ecmbind_sdo_read({0},{1},{2},..,{3}) == {4}", slave, index, subIndex, rawValue.Length, rawValue.ToHexString());
|
||||
Logging.Log(LogLevel.ERROR, "ECMBind.ecmbind_sdo_read({0},{1},{2},..,{3}) [{5}] == {4}", slave, index, subIndex, rawValue.Length, rawValue.ToHexString(), dataSize);
|
||||
throw new IOException();
|
||||
}
|
||||
}
|
||||
rawValue = rawValue.Slice(0, dataSize);
|
||||
return true;
|
||||
}
|
||||
|
||||
public bool ReadSDOCA(UInt16 slave,UInt16 index, byte subIndex, out byte[] rawValue)
|
||||
{
|
||||
rawValue =
|
||||
new byte[128];
|
||||
int dataSize;
|
||||
|
||||
lock (lockIOMap)
|
||||
{
|
||||
dataSize =
|
||||
ECMBind.ecmbind_sdo_read_ca(slave, index, subIndex, rawValue, rawValue.Length );
|
||||
if (dataSize <= 0)
|
||||
{
|
||||
Logging.Log(LogLevel.ERROR, "ECMBind.ecmbind_sdo_read_ca({0},{1},{2},..,{3}) == {4}", slave, index, subIndex, rawValue.Length, rawValue.ToHexString());
|
||||
throw new IOException();
|
||||
}
|
||||
}
|
||||
|
@ -233,41 +459,20 @@ namespace ln.ethercat
|
|||
int result;
|
||||
lock (lockIOMap)
|
||||
result = ECMBind.ecmbind_sdo_write(slave, index, subIndex, rawValue, rawValue.Length);
|
||||
Logging.Log(LogLevel.DEBUG, "ECMaster: WriteSDO({0},{1},{2},{3}) => {4}", slave, index, subIndex, rawValue.ToHexString(), result);
|
||||
//Logging.Log(LogLevel.DEBUG, "ECMaster: WriteSDO({0},{1},{2},{3}) => {4}", slave, index, subIndex, rawValue.ToHexString(), result);
|
||||
return result > 0;
|
||||
}
|
||||
public bool WriteSDOCA(UInt16 slave,UInt16 index, byte subIndex, byte[] rawValue)
|
||||
{
|
||||
int result;
|
||||
lock (lockIOMap)
|
||||
result = ECMBind.ecmbind_sdo_write_ca(slave, index, subIndex, rawValue, rawValue.Length);
|
||||
//Logging.Log(LogLevel.DEBUG, "ECMaster: WriteSDOCA({0},{1},{2},{3}) => {4}", slave, index, subIndex, rawValue.ToHexString(), result);
|
||||
return result > 0;
|
||||
}
|
||||
|
||||
public ECSlaveState ReadSlaveState(int slave) => ECMBind.ecmbind_get_slave_state(slave);
|
||||
|
||||
private ECSlaveState UpdateEthercatState()
|
||||
{
|
||||
EthercatState = ECMBind.ecmbind_read_state();
|
||||
return ethercatState;
|
||||
}
|
||||
|
||||
public bool CheckState(ECSlaveState minimumState, int timeout)
|
||||
{
|
||||
for (; timeout > 0; timeout -= 10)
|
||||
{
|
||||
ECSlaveState state = ECMBind.ecmbind_read_state();
|
||||
if (state >= minimumState)
|
||||
{
|
||||
EthercatState = state;
|
||||
return true;
|
||||
}
|
||||
Thread.Sleep(10);
|
||||
}
|
||||
return false;
|
||||
}
|
||||
public bool RequestState(ECSlaveState requestedState, out ECSlaveState reachedState, int timeout)
|
||||
{
|
||||
reachedState = ECMBind.ecmbind_request_state(0, requestedState, timeout);
|
||||
Logging.Log(LogLevel.DEBUG, "ECMaster.RequestState({1}): lowest slave state: {0}", reachedState, requestedState);
|
||||
EthercatState = reachedState;
|
||||
return (reachedState >= requestedState);
|
||||
}
|
||||
|
||||
public int RequestSlaveState(int slave, ECSlaveState slaveState) => ECMBind.ecmbind_write_slave_state(slave, slaveState);
|
||||
public ECSlaveState ReadSlaveState(int slave) => IOLocked(()=>ECMBind.ecmbind_get_slave_state(slave));
|
||||
int RequestSlaveState(int slave, ECSlaveState slaveState) => IOLocked(()=>ECMBind.ecmbind_write_slave_state(slave, slaveState));
|
||||
|
||||
|
||||
public bool GetSlave(UInt16 slave_id, out ECSlave slave)
|
||||
|
@ -297,6 +502,7 @@ namespace ln.ethercat
|
|||
if ((offset >= 0) && (length > 0) && (offset+length < iomap.Length))
|
||||
{
|
||||
rawData = iomap.Slice(offset, length);
|
||||
//Logging.Log(LogLevel.ERROR, "GetIOmapData({0},{1})", offset, length);
|
||||
return true;
|
||||
}
|
||||
Logging.Log(LogLevel.ERROR, "GetIOmapData({0},{1}) failed", offset, length);
|
||||
|
@ -308,7 +514,7 @@ namespace ln.ethercat
|
|||
lock (lockIOMap)
|
||||
if ((offset >= 0) && (length > 0) && (offset+length < iomap.Length))
|
||||
{
|
||||
Logging.Log(LogLevel.DEBUG, "ECMaster: SetIOmapData({0},{1},{2})", offset, length, rawData.ToHexString());
|
||||
//Logging.Log(LogLevel.DEBUG, "ECMaster: SetIOmapData({0},{1},{2})", offset, length, rawData.ToHexString());
|
||||
Buffer.BlockCopy(rawData, 0, iomap, offset, length);
|
||||
return true;
|
||||
}
|
||||
|
@ -334,6 +540,7 @@ namespace ln.ethercat
|
|||
public void UpdatePDOMap()
|
||||
{
|
||||
List<PDO> pdoList = new List<PDO>();
|
||||
lock (lockIOMap)
|
||||
ECMBind.ecmbind_pdo_enumerate((UInt16 slave, UInt16 index, byte subindex, int addr_offset, int addr_bit, int bitlength)=>{
|
||||
if (addr_bit != 0)
|
||||
Logging.Log(LogLevel.WARNING, "currently only PDO mappings on byte boundaries are supported");
|
||||
|
@ -345,7 +552,7 @@ namespace ln.ethercat
|
|||
});
|
||||
});
|
||||
|
||||
lock (this)
|
||||
lock (lockIOMap)
|
||||
{
|
||||
IOMapPtr = ECMBind.ecmbind_get_iomap();
|
||||
pdoMap = pdoList;
|
||||
|
|
|
@ -58,7 +58,7 @@ namespace ln.ethercat
|
|||
|
||||
public void Update()
|
||||
{
|
||||
if (ECMBind.ecmbind_read_objectdescription(Slave.Id, Index, (int slave, int index, ECDataTypes dataType, ECObjectCodes objectCode, int maxsub, String name)=>{
|
||||
if (ECMBind.ecmbind_read_objectdescription(Slave.Id, Index, (int slave, int index, ECDataTypes dataType, ECObjectCodes objectCode, int maxsub, ushort bitlength, String name)=>{
|
||||
Name = name;
|
||||
DataType = dataType;
|
||||
ObjectCode = objectCode;
|
||||
|
@ -105,6 +105,7 @@ namespace ln.ethercat
|
|||
{
|
||||
public SDODescriptor Descriptor;
|
||||
public byte SubIndex { get; }
|
||||
public ushort BitLength { get; private set; }
|
||||
|
||||
public string Name { get; private set; }
|
||||
public ECDataTypes DataType { get; private set; }
|
||||
|
@ -114,13 +115,14 @@ namespace ln.ethercat
|
|||
Descriptor = descriptor;
|
||||
SubIndex = subIndex;
|
||||
|
||||
if (SubIndex == 0)
|
||||
if ((Descriptor.MaxSubIndex > 0) && (SubIndex == 0))
|
||||
{
|
||||
DataType = descriptor.DataType;
|
||||
Name = descriptor.Name;
|
||||
} else {
|
||||
if (ECMBind.ecmbind_read_objectdescription_entry(Descriptor.SlaveId, descriptor.Index, SubIndex, (int slave, int index, ECDataTypes dataType, ECObjectCodes objectCode, int si, String name)=>{
|
||||
Name = name;
|
||||
if (ECMBind.ecmbind_read_objectdescription_entry(Descriptor.SlaveId, descriptor.Index, SubIndex, (int slave, int index, ECDataTypes dataType, ECObjectCodes objectCode, int si, ushort bitlength, String name)=>{
|
||||
Name = (SubIndex == 0) ? descriptor.Name : name;
|
||||
BitLength = bitlength;
|
||||
DataType = dataType;
|
||||
}) <= 0)
|
||||
{
|
||||
|
|
|
@ -1,4 +1,5 @@
|
|||
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Threading;
|
||||
using ln.ethercat.controller.drives;
|
||||
|
@ -8,98 +9,225 @@ namespace ln.ethercat.controller
|
|||
{
|
||||
|
||||
public delegate void ControllerLogicDelegate(Controller controller);
|
||||
public delegate void ControllerStateChangeDelegate(Controller controller, ControllerStates newState);
|
||||
|
||||
public enum ControllerStates {
|
||||
NONE,
|
||||
NOTREADY,
|
||||
FAULT,
|
||||
READY,
|
||||
DISABLING,
|
||||
ENABLING,
|
||||
OPERATIONAL
|
||||
}
|
||||
|
||||
public class Controller
|
||||
{
|
||||
public event ControllerLogicDelegate ControllerLogic;
|
||||
public event ControllerStateChangeDelegate OnStateChanging;
|
||||
public event ControllerStateChangeDelegate OnStateChanged;
|
||||
|
||||
public ECMaster ECMaster { get; }
|
||||
|
||||
List<DriveController> driveControllers = new List<DriveController>();
|
||||
public DriveController[] DriveControllers => driveControllers.ToArray();
|
||||
public bool IgnoreRemoteInterface { get; set; }
|
||||
|
||||
List<ControlLoop> controlLoops = new List<ControlLoop>();
|
||||
public ControlLoop[] ControlLoops => controlLoops.ToArray();
|
||||
|
||||
public bool IsRunning => threadController?.IsAlive ?? false;
|
||||
public ControllerStates ControllerState { get; private set; }
|
||||
|
||||
public double ControllerLoopInterval { get; set; } = 0.1;
|
||||
public double ControllerLoopFrequency {
|
||||
get => 1.0 / ControllerLoopInterval;
|
||||
set => ControllerLoopInterval = 1.0 / value;
|
||||
}
|
||||
public long CycleCounter { get; private set; }
|
||||
|
||||
bool stopRequested;
|
||||
Thread threadController;
|
||||
Thread threadWatchdog;
|
||||
int wdogCounter;
|
||||
public int WatchdogReset { get; set; } = 5;
|
||||
|
||||
public Controller()
|
||||
public Controller(ECMaster ecMaster)
|
||||
{
|
||||
ECMaster = ecMaster;
|
||||
}
|
||||
|
||||
public void Start()
|
||||
{
|
||||
if (threadController?.IsAlive ?? false)
|
||||
{
|
||||
Logging.Log(LogLevel.WARNING, "Controller; Start(): already started");
|
||||
} else {
|
||||
stopRequested = false;
|
||||
|
||||
foreach (DriveController driveController in driveControllers)
|
||||
driveController.Initialize();
|
||||
|
||||
threadController = new Thread(ControllerThread);
|
||||
threadController.Start();
|
||||
}
|
||||
}
|
||||
|
||||
public void Stop()
|
||||
{
|
||||
if (threadController?.IsAlive ?? false)
|
||||
{
|
||||
stopRequested = true;
|
||||
threadController.Join();
|
||||
threadController = null;
|
||||
}
|
||||
}
|
||||
|
||||
public void Add(DriveController driveController) => driveControllers.Add(driveController);
|
||||
public void Remove(DriveController driveController) => driveControllers.Remove(driveController);
|
||||
|
||||
public void Add(ControlLoop controlLoop) => controlLoops.Add(controlLoop);
|
||||
public void Remove(ControlLoop controlLoop) => controlLoops.Remove(controlLoop);
|
||||
|
||||
public DriveStates DrivesState {
|
||||
get {
|
||||
DriveStates lowestState = DriveStates.OPERATIONAL;
|
||||
foreach (DriveController driveController in driveControllers)
|
||||
public void Initialize()
|
||||
{
|
||||
DriveStates driveState = driveController.DriveState;
|
||||
if (driveState < lowestState)
|
||||
lowestState = driveState;
|
||||
CycleCounter = 0;
|
||||
|
||||
if (!(threadWatchdog?.IsAlive ?? false))
|
||||
{
|
||||
threadWatchdog = new Thread(Watchdog);
|
||||
threadWatchdog.Start();
|
||||
}
|
||||
return lowestState;
|
||||
|
||||
foreach (DriveController driveController in ECMaster.DriveControllers)
|
||||
driveController.Initialize();
|
||||
|
||||
}
|
||||
|
||||
void UpdateControllerState()
|
||||
{
|
||||
ControllerStates nextState = ControllerStates.OPERATIONAL;
|
||||
|
||||
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());
|
||||
}
|
||||
}
|
||||
|
||||
void ControllerThread()
|
||||
public void ClearFaults()
|
||||
{
|
||||
while (!stopRequested)
|
||||
foreach (DriveController driveController in ECMaster.DriveControllers)
|
||||
{
|
||||
foreach (DriveController driveController in 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 driveControllers)
|
||||
foreach (DriveController driveController in ECMaster.DriveControllers)
|
||||
driveController.UpdateDrive();
|
||||
}
|
||||
|
||||
Thread.Sleep((int)(1000.0 * ControllerLoopInterval));
|
||||
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 svModesOfOperation;
|
||||
SDOValue svModesOfOperationDisplay;
|
||||
SDOValue svActualCurrent;
|
||||
|
||||
SDOValue svMaxSpeed;
|
||||
|
||||
UInt32 MotorMaxSpeed = 1000;
|
||||
|
||||
public CIA402Controller(ECMaster ecMaster,UInt16 slave)
|
||||
:base(ecMaster, slave)
|
||||
{
|
||||
PDOMappingRequest[] mappingRequests = new PDOMappingRequest[]{
|
||||
new PDOMappingRequest(slave, 0x603F, 0, false),
|
||||
new PDOMappingRequest(slave, 0x6040, 0, true),
|
||||
new PDOMappingRequest(slave, 0x6041, 0, false),
|
||||
new PDOMappingRequest(slave, 0x607A, 0, true),
|
||||
new PDOMappingRequest(slave, 0x60FF, 0, true),
|
||||
new PDOMappingRequest(slave, 0x6071, 0, true),
|
||||
new PDOMappingRequest(slave, 0x6064, 0, false),
|
||||
new PDOMappingRequest(slave, 0x606C, 0, false),
|
||||
new PDOMappingRequest(slave, 0x6077, 0, false),
|
||||
new PDOMappingRequest(slave, 0x6060, 0, true),
|
||||
new PDOMappingRequest(slave, 0x6061, 0, false),
|
||||
new PDOMappingRequest(slave, 0x6078, 0, false)
|
||||
};
|
||||
ecMaster.RequestPDOMapping(mappingRequests);
|
||||
}
|
||||
|
||||
public override void Initialize()
|
||||
|
@ -61,23 +80,82 @@ namespace ln.ethercat.controller.drives
|
|||
ECMaster.GetSDOValue(Slave, 0x606C, 0, out svActualSpeed) &&
|
||||
ECMaster.GetSDOValue(Slave, 0x6077, 0, out svActualTorque) &&
|
||||
ECMaster.GetSDOValue(Slave, 0x6060, 0, out svModesOfOperation) &&
|
||||
ECMaster.GetSDOValue(Slave, 0x6061, 0, out svModesOfOperationDisplay)
|
||||
ECMaster.GetSDOValue(Slave, 0x6061, 0, out svModesOfOperationDisplay) &&
|
||||
ECMaster.GetSDOValue(Slave, 0x6080, 0, out svMaxSpeed) &&
|
||||
ECMaster.GetSDOValue(Slave, 0x6078, 0, out svActualCurrent)
|
||||
))
|
||||
throw new ArgumentOutOfRangeException("CIA402Controller could not retrieve SDOvalues for CiA402 profile");
|
||||
}
|
||||
|
||||
public override void UpdateStates() { }
|
||||
public override void UpdateDrive() { }
|
||||
public override void UpdateStates() {
|
||||
CIA402State = GetCIA402State();
|
||||
}
|
||||
public override void UpdateDrive() {
|
||||
|
||||
if (CIA402State != CIA402TargetState)
|
||||
{
|
||||
switch (CIA402State)
|
||||
{
|
||||
case CIA402States.NOT_READY_TO_SWITCH_ON:
|
||||
svControlWord.SetValue((ushort)0x0000);
|
||||
ModeOfOperation = CIA402ModesOfOperation.CYCLIC_SYNC_TORQUE;
|
||||
break;
|
||||
case CIA402States.SWITCH_ON_DISABLED:
|
||||
svControlWord.SetValue((ushort)0x0006);
|
||||
break;
|
||||
case CIA402States.READY_TO_SWITCH_ON:
|
||||
switch (CIA402TargetState)
|
||||
{
|
||||
case CIA402States.SWITCH_ON_DISABLED:
|
||||
svControlWord.SetValue((ushort)0x0000);
|
||||
break;
|
||||
case CIA402States.SWITCHED_ON:
|
||||
case CIA402States.OPERATION_ENABLED:
|
||||
svControlWord.SetValue((ushort)0x0007);
|
||||
break;
|
||||
}
|
||||
break;
|
||||
case CIA402States.SWITCHED_ON:
|
||||
switch (CIA402TargetState)
|
||||
{
|
||||
case CIA402States.SWITCH_ON_DISABLED:
|
||||
case CIA402States.READY_TO_SWITCH_ON:
|
||||
svControlWord.SetValue((ushort)0x0006);
|
||||
break;
|
||||
case CIA402States.OPERATION_ENABLED:
|
||||
svControlWord.SetValue((ushort)0x000F);
|
||||
break;
|
||||
}
|
||||
break;
|
||||
case CIA402States.OPERATION_ENABLED:
|
||||
switch (CIA402TargetState)
|
||||
{
|
||||
case CIA402States.SWITCH_ON_DISABLED:
|
||||
case CIA402States.READY_TO_SWITCH_ON:
|
||||
svControlWord.SetValue((ushort)0x0006);
|
||||
break;
|
||||
case CIA402States.SWITCHED_ON:
|
||||
svControlWord.SetValue((ushort)0x0007);
|
||||
break;
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (DriveMode == DriveMode.UNDEFINED)
|
||||
DriveMode = DriveMode.TORQUE;
|
||||
}
|
||||
|
||||
public override DriveStates DriveState
|
||||
{
|
||||
get {
|
||||
switch (GetCIA402State())
|
||||
switch (CIA402State)
|
||||
{
|
||||
case CIA402States.NOT_READY_TO_SWITCH_ON:
|
||||
return DriveStates.INIT;
|
||||
case CIA402States.SWITCH_ON_DISABLED:
|
||||
case CIA402States.READY_TO_SWITCH_ON:
|
||||
return DriveStates.INIT;
|
||||
return DriveStates.READY;
|
||||
case CIA402States.SWITCHED_ON:
|
||||
case CIA402States.QUICK_STOP_ACTIVE:
|
||||
return DriveStates.POWERED;
|
||||
|
@ -93,7 +171,7 @@ namespace ln.ethercat.controller.drives
|
|||
}
|
||||
|
||||
public override string OEMDriveState {
|
||||
get => GetCIA402State().ToString();
|
||||
get => CIA402State.ToString();
|
||||
}
|
||||
public override string OEMDriveMode => ModeOfOperation.ToString();
|
||||
|
||||
|
@ -135,26 +213,27 @@ namespace ln.ethercat.controller.drives
|
|||
public void SetDriveModeTorque() => DriveMode = DriveMode.TORQUE;
|
||||
|
||||
public CIA402ModesOfOperation ModeOfOperation {
|
||||
get => (CIA402ModesOfOperation)(svModesOfOperationDisplay.GetValue<sbyte>());
|
||||
get => (ECMaster.EthercatState==ECSlaveState.OPERATIONAL) ? (CIA402ModesOfOperation)(svModesOfOperationDisplay?.GetValue<sbyte>() ?? 0) : CIA402ModesOfOperation.NO_MODE_CHANGE;
|
||||
set => svModesOfOperation.SetValue((sbyte)value);
|
||||
}
|
||||
|
||||
public override decimal ActualPosition => svActualPosition.GetValue<int>();
|
||||
public override decimal ActualSpeed => svActualSpeed.GetValue<int>();
|
||||
public override decimal ActualTorque => svActualTorque.GetValue<short>();
|
||||
public override double ActualPosition => (svActualPosition?.GetValue<int>() ?? 0) ;
|
||||
public override double ActualSpeed => (double)(svActualSpeed?.GetValue<int>() ?? 0) / (1000.0 * MotorMaxSpeed);
|
||||
public override double ActualTorque => (double)(svActualTorque?.GetValue<short>() ?? 0) / 1000.0;
|
||||
public override double ActualLoad => (double)(svActualCurrent?.GetValue<short>() ?? 0) / 1000.0;
|
||||
|
||||
public override decimal TargetPosition {
|
||||
public override double TargetPosition {
|
||||
get => svTargetPosition.GetValue<int>();
|
||||
set => svTargetPosition.SetValue((int)value);
|
||||
}
|
||||
|
||||
public override decimal TargetSpeed {
|
||||
get => svTargetSpeed.GetValue<int>();
|
||||
set => svTargetSpeed.SetValue((int)value);
|
||||
public override double TargetSpeed {
|
||||
get => (double)svTargetSpeed.GetValue<int>() / (1000.0 * MotorMaxSpeed);
|
||||
set => svTargetSpeed.SetValue((int)(value * (1000.0 * MotorMaxSpeed)));
|
||||
}
|
||||
public override decimal TargetTorque {
|
||||
get => svTargetTorque.GetValue<short>();
|
||||
set => svTargetTorque.SetValue((short)value);
|
||||
public override double TargetTorque {
|
||||
get => (double)svTargetTorque.GetValue<short>() / 1000.0;
|
||||
set => svTargetTorque.SetValue((short)(1000 * value));
|
||||
}
|
||||
|
||||
public override int ErrorCode => svErrorCode.GetValue<int>();
|
||||
|
@ -163,51 +242,12 @@ namespace ln.ethercat.controller.drives
|
|||
|
||||
public override void Enable(bool enable)
|
||||
{
|
||||
if (enable)
|
||||
{
|
||||
switch (CIA402State)
|
||||
{
|
||||
case CIA402States.SWITCHED_ON:
|
||||
svControlWord.SetValue((UInt16)0x000F);
|
||||
break;
|
||||
default:
|
||||
Logging.Log(LogLevel.WARNING, "CIA402Controller: EnableDrive(): current state is {0}, can't enable drive", CIA402State.ToString());
|
||||
break;
|
||||
}
|
||||
} else {
|
||||
switch (CIA402State)
|
||||
{
|
||||
case CIA402States.OPERATION_ENABLED:
|
||||
svControlWord.SetValue((UInt16)0x0007);
|
||||
break;
|
||||
default:
|
||||
Logging.Log(LogLevel.WARNING, "CIA402Controller: EnableDrive(): current state is {0}, can't disable drive", CIA402State.ToString());
|
||||
break;
|
||||
}
|
||||
}
|
||||
CIA402TargetState = enable ? CIA402States.OPERATION_ENABLED : CIA402States.READY_TO_SWITCH_ON;
|
||||
}
|
||||
|
||||
public override void Power(bool poweron)
|
||||
{
|
||||
if (poweron)
|
||||
{
|
||||
switch (CIA402State)
|
||||
{
|
||||
case CIA402States.FAULT:
|
||||
case CIA402States.FAULT_REACTION_ACTIVE:
|
||||
Logging.Log(LogLevel.WARNING, "CIA402Controller: Power(): Drive in fault state, not ready to switch power on");
|
||||
break;
|
||||
case CIA402States.NOT_READY_TO_SWITCH_ON:
|
||||
Logging.Log(LogLevel.WARNING, "CIA402Controller: Power(): Drive not ready to switch power on");
|
||||
break;
|
||||
case CIA402States.SWITCH_ON_DISABLED:
|
||||
case CIA402States.READY_TO_SWITCH_ON:
|
||||
svControlWord.SetValue((UInt16)0x0007); // Switch ON
|
||||
break;
|
||||
}
|
||||
} else {
|
||||
svControlWord.SetValue((UInt16)0x0006);
|
||||
}
|
||||
CIA402TargetState = poweron ? CIA402States.SWITCHED_ON : CIA402States.READY_TO_SWITCH_ON;
|
||||
}
|
||||
|
||||
public override void ClearFault()
|
||||
|
@ -220,10 +260,12 @@ namespace ln.ethercat.controller.drives
|
|||
}
|
||||
}
|
||||
|
||||
CIA402States CIA402State => GetCIA402State();
|
||||
public CIA402States CIA402State { get; private set; }
|
||||
public CIA402States CIA402TargetState { get; private set; }
|
||||
|
||||
public CIA402States GetCIA402State()
|
||||
{
|
||||
UInt16 statusword = svStatusWord.GetValue<ushort>();
|
||||
UInt16 statusword = svStatusWord?.GetValue<ushort>() ?? 0;
|
||||
|
||||
if ((statusword & 0x004F)==0)
|
||||
return CIA402States.NOT_READY_TO_SWITCH_ON;
|
||||
|
|
|
@ -1,6 +1,7 @@
|
|||
|
||||
|
||||
using System;
|
||||
using System.Net.Http.Headers;
|
||||
using System.Runtime;
|
||||
|
||||
namespace ln.ethercat.controller.drives
|
||||
|
@ -9,6 +10,7 @@ namespace ln.ethercat.controller.drives
|
|||
UNDEFINED,
|
||||
BOOT,
|
||||
INIT,
|
||||
READY,
|
||||
ERROR,
|
||||
POWERED,
|
||||
OPERATIONAL,
|
||||
|
@ -26,6 +28,9 @@ namespace ln.ethercat.controller.drives
|
|||
protected ECMaster ECMaster { get; }
|
||||
public UInt16 Slave { get; }
|
||||
|
||||
public bool IgnoredByController { get; set; }
|
||||
|
||||
|
||||
public DriveController(ECMaster ecMaster, UInt16 slave)
|
||||
{
|
||||
ECMaster = ecMaster;
|
||||
|
@ -57,13 +62,46 @@ namespace ln.ethercat.controller.drives
|
|||
public void DisableDrive() => Enable(false);
|
||||
public abstract void Enable(bool enabled);
|
||||
|
||||
public abstract decimal ActualPosition { get; }
|
||||
public abstract decimal ActualSpeed { get; }
|
||||
public abstract decimal ActualTorque { get; }
|
||||
public abstract decimal TargetPosition { get; set; }
|
||||
public abstract decimal TargetSpeed { get; set; }
|
||||
public abstract decimal TargetTorque { get; set; }
|
||||
public abstract double ActualPosition { get; }
|
||||
public abstract double ActualSpeed { get; }
|
||||
public abstract double ActualTorque { get; }
|
||||
public abstract double TargetPosition { get; set; }
|
||||
public abstract double TargetSpeed { get; set; }
|
||||
public abstract double TargetTorque { get; set; }
|
||||
|
||||
public abstract double ActualLoad { get; }
|
||||
|
||||
public virtual double TargetValue {
|
||||
get {
|
||||
switch (DriveMode)
|
||||
{
|
||||
case DriveMode.POSITION:
|
||||
return TargetPosition;
|
||||
case DriveMode.SPEED:
|
||||
return TargetSpeed;
|
||||
case DriveMode.TORQUE:
|
||||
return TargetTorque;
|
||||
default:
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
set {
|
||||
switch (DriveMode)
|
||||
{
|
||||
case DriveMode.POSITION:
|
||||
TargetPosition = value;
|
||||
break;
|
||||
case DriveMode.SPEED:
|
||||
TargetSpeed = value;
|
||||
break;
|
||||
case DriveMode.TORQUE:
|
||||
TargetTorque = value;
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
|
|
@ -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>
|
||||
<PackageReference Include="System.IO.Ports" Version="5.0.0" />
|
||||
<PackageReference Include="ln.type" Version="0.1.7-ci" />
|
||||
<PackageReference Include="ln.logging" Version="1.0.2" />
|
||||
<PackageReference Include="ln.json" Version="1.0.6" />
|
||||
|
|
Loading…
Reference in New Issue