ln-dotnet
/
ln.ethercat
3
0
Fork 0
Code Issues Pull Requests Projects Releases 1 Wiki Activity

First implementation of 3 mode switch ( INDEPENDENT, SCREW, PRESS )

master
haraldwolff 2022-03-02 19:28:00 +01:00
parent 1c2a3d6daa
commit cfd40b86e5
13 changed files with 595 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

192
ln.ethercat.service/GalaTechControllerLogic.cs 100644
View File

@ -0,0 +1,192 @@
using System;
using System.IO;
using System.Linq;
using ln.ethercat.controller;
using ln.ethercat.controller.drives;
using ln.ethercat.controller.remote;
using ln.json;
using ln.json.mapping;
using ln.logging;
namespace ln.ethercat.service
{
public enum GTOperationMode { NONE, INDEPENDENT, SCREW, PRESS }
public class GalaTechControllerLogic : ControllerLogic
{
public MyParameters Parameters { get; set; }
SDOValue svRelais;
SDOValue svEnable;
private SDOValue svMode0, svMode1, svMode2;
public GalaTechControllerLogic()
{
Parameters = new MyParameters();
if (File.Exists("gtcl.json"))
{
JSONValue configValue = JSONParser.ParseFile("gtcl.json");
JSONMapper.DefaultMapper.Apply(configValue as JSONObject, Parameters);
}
}
public void Save()
{
if (JSONMapper.DefaultMapper.Serialize(Parameters, out JSONValue configObject))
{
using (StreamWriter sw = new StreamWriter("gtcl.json"))
{
sw.Write(configObject.ToString());
sw.Flush();
}
}
}
public override void Initialize(Controller controller)
{
if (!(
controller.ECMaster.GetSDOValue(1, 0x2012, 31, out svRelais) &&
controller.ECMaster.GetSDOValue(1, 0x2012, 32, out svEnable) &&
controller.ECMaster.GetSDOValue(2, 0x2008, 1, out svMode0) &&
controller.ECMaster.GetSDOValue(2, 0x2008, 2, out svMode1) &&
controller.ECMaster.GetSDOValue(2, 0x2008, 3, out svMode2)
))
throw new Exception("could not retrieve needed SDOValues");
}
public override void Cycle(Controller controller)
{
GTOperationMode operationMode = GTOperationMode.NONE;
object mm0 = svMode0.GetValue();
Logging.Log(LogLevel.DEBUG,"MT: {0}", mm0);
sbyte m0 = svMode0.GetValue<sbyte>();
sbyte m1 = svMode1.GetValue<sbyte>();
sbyte m2 = svMode2.GetValue<sbyte>();
Logging.Log(LogLevel.DEBUG, "Mode Bytes: {0} {1} {2}", m0,m1,m2);
if ((m0 == 1) && (m1 == 0) && (m2 == 0))
operationMode = GTOperationMode.INDEPENDENT;
else if ((m0 == 0) && (m1 == 1) && (m2 == 0))
operationMode = GTOperationMode.SCREW;
else if ((m0 == 0) && (m1 == 0) && (m2 == 1))
operationMode = GTOperationMode.PRESS;
else
operationMode = GTOperationMode.NONE;
switch (controller.ControllerState)
{
case ControllerStates.FAULT:
svEnable.SetValue((byte)0x00);
controller.ECMaster.DriveControllers[0].TargetTorque = 0;
controller.ECMaster.DriveControllers[0].TargetSpeed = 0;
controller.ECMaster.DriveControllers[1].TargetTorque = 0;
controller.ECMaster.DriveControllers[1].TargetSpeed = 0;
break;
case ControllerStates.NOTREADY:
svEnable.SetValue((byte)0x01);
break;
}
if (controller.ControllerState == ControllerStates.OPERATIONAL)
{
svRelais.SetValue((byte)0x01);
switch (operationMode)
{
case GTOperationMode.NONE:
controller.ECMaster.DriveControllers[0].TargetTorque = 0;
controller.ECMaster.DriveControllers[0].TargetSpeed = 0;
controller.ECMaster.DriveControllers[1].TargetTorque = 0;
controller.ECMaster.DriveControllers[1].TargetSpeed = 0;
break;
case GTOperationMode.INDEPENDENT:
controller.ECMaster.DriveControllers[0].DriveMode = DriveMode.SPEED;
controller.ECMaster.DriveControllers[0].TargetTorque = 0;
controller.ECMaster.DriveControllers[0].TargetSpeed = controller.Remotes.FirstOrDefault()?.Targets[0] ?? 0.0;
controller.ECMaster.DriveControllers[1].DriveMode = DriveMode.SPEED;
controller.ECMaster.DriveControllers[1].TargetTorque = 0;
switch (controller.Remotes.FirstOrDefault()?.FeederOperation)
{
case FeederOperation.LEFT:
controller.ECMaster.DriveControllers[1].TargetSpeed = Parameters.FeedSpeedRatio * controller.Remotes.FirstOrDefault()?.Targets[1] ?? 0.0;
break;
case FeederOperation.RIGHT:
controller.ECMaster.DriveControllers[1].TargetSpeed = Parameters.FeedSpeedRatio * -controller.Remotes.FirstOrDefault()?.Targets[1] ?? 0.0;
break;
default:
controller.ECMaster.DriveControllers[1].TargetSpeed = 0.0;
break;
}
break;
case GTOperationMode.SCREW:
controller.ECMaster.DriveControllers[0].DriveMode = DriveMode.SPEED;
controller.ECMaster.DriveControllers[0].TargetTorque = 0;
controller.ECMaster.DriveControllers[1].DriveMode = DriveMode.SPEED;
controller.ECMaster.DriveControllers[1].TargetTorque = 0;
switch (controller.Remotes.FirstOrDefault()?.FeederOperation)
{
case FeederOperation.LEFT:
controller.ECMaster.DriveControllers[0].TargetSpeed = controller.Remotes.FirstOrDefault()?.Targets[0] ?? 0.0;
controller.ECMaster.DriveControllers[1].TargetSpeed = Parameters.FeedScrewGearRatio * controller.Remotes.FirstOrDefault()?.Targets[0] ?? 0.0;
break;
case FeederOperation.RIGHT:
controller.ECMaster.DriveControllers[0].TargetSpeed = -controller.Remotes.FirstOrDefault()?.Targets[0] ?? 0.0;
controller.ECMaster.DriveControllers[1].TargetSpeed = -Parameters.FeedScrewGearRatio * controller.Remotes.FirstOrDefault()?.Targets[0] ?? 0.0;
break;
default:
controller.ECMaster.DriveControllers[0].TargetSpeed = 0.0;
controller.ECMaster.DriveControllers[1].TargetSpeed = 0.0;
break;
}
break;
case GTOperationMode.PRESS:
controller.ECMaster.DriveControllers[0].DriveMode = DriveMode.SPEED;
controller.ECMaster.DriveControllers[0].TargetTorque = 0;
controller.ECMaster.DriveControllers[0].TargetSpeed = controller.Remotes.FirstOrDefault()?.Targets[0] ?? 0.0;
controller.ECMaster.DriveControllers[1].DriveMode = DriveMode.TORQUE;
controller.ECMaster.DriveControllers[1].TargetSpeed = 0;
switch (controller.Remotes.FirstOrDefault()?.FeederOperation)
{
case FeederOperation.LEFT:
controller.ECMaster.DriveControllers[1].TargetTorque = Parameters.FeedTorqueRatio * controller.Remotes.FirstOrDefault()?.Targets[1] ?? 0.0;
break;
case FeederOperation.RIGHT:
controller.ECMaster.DriveControllers[1].TargetTorque = -Parameters.FeedTorqueRatio * controller.Remotes.FirstOrDefault()?.Targets[1] ?? 0.0;
break;
default:
controller.ECMaster.DriveControllers[1].TargetTorque = 0.0;
break;
}
break;
}
} else {
svRelais.SetValue((byte)0x00);
}
}
public class MyParameters
{
public double FeedSpeedRatio = 1.0;
public double FeedTorqueRatio = 0.5;
public double FeedScrewGearRatio = 0.3;
}
}
}

13
ln.ethercat.service/Program.cs
View File

@ -39,17 +39,24 @@ namespace ln.ethercat.service
{
ECMaster.RequestPDOMapping(1, 0x2012, 31, true);
ECMaster.RequestPDOMapping(1, 0x2012, 32, true);
ECMaster.RequestPDOMapping(2, 0x2008, 1, false);
ECMaster.RequestPDOMapping(2, 0x2008, 2, false);
ECMaster.RequestPDOMapping(2, 0x2008, 3, false);
}
};
ethercatService.ECMaster.Controller.Add(new CLGalaTechBohrautomat());
//ethercatService.ECMaster.Controller.Add(new CLGalaTechBohrautomat());
ethercatService.ECMaster.Controller.Add(new GalaTechControllerLogic());
ethercatService.Start();
if (SerialRemotePort != null)
{
StupidSerialRemote stupidSerialRemote = new StupidSerialRemote(ethercatService.ECMaster.Controller, SerialRemotePort);
stupidSerialRemote.Start();
// StupidSerialRemote stupidSerialRemote = new StupidSerialRemote(ethercatService.ECMaster.Controller, SerialRemotePort);
// stupidSerialRemote.Start();
NewSerialRemote serialRemote =
new NewSerialRemote(ethercatService.ECMaster.Controller, SerialRemotePort);
serialRemote.Start();
}
}
}

1
ln.ethercat.service/www/html/spa.html
View File

@ -98,6 +98,7 @@ LN
d.socket = $ECAPP.connectWebSocket("/api/v1/sockets/controller");
d.socket.onmessage = (evt)=>{
let json = JSON.parse(evt.data);
console.log(json);
d.controller = json.value.Controller;
let drives = []
for (let n=0;n<d.controller.DriveCount;n++)

16
ln.ethercat/controller/Controller.cs
View File

@ -36,6 +36,9 @@ namespace ln.ethercat.controller
[ESProperty]
public bool IgnoreRemoteInterface { get; set; }
private ISet<ControllerRemote> _remotes = new HashSet<ControllerRemote>();
public IEnumerable<ControllerRemote> Remotes => _remotes;
List<ControlLoop> controlLoops = new List<ControlLoop>();
public ControlLoop[] ControlLoops => controlLoops.ToArray();
@ -77,6 +80,9 @@ namespace ln.ethercat.controller
public void Add(ControllerLogic controllerLogic) => controllerLogics.Add(controllerLogic);
public void Remove(ControllerLogic controllerLogic) => controllerLogics.Remove(controllerLogic);
public void Add(ControllerRemote remote) => _remotes.Add(remote);
public void Remove(ControllerRemote remote) => _remotes.Remove(remote);
public void Initialize()
{
CycleCounter = 0;
@ -184,11 +190,15 @@ namespace ln.ethercat.controller
public bool IsSafeToEnable()
{
foreach (ControllerIsSafeToEnable ciste in OnIsSafeToEnable.GetInvocationList())
if (!(OnIsSafeToEnable is null))
{
if (!ciste(this))
return false;
foreach (ControllerIsSafeToEnable ciste in OnIsSafeToEnable.GetInvocationList())
{
if (!ciste(this))
return false;
}
}
return true;
}

11
ln.ethercat/controller/ControllerRemote.cs
View File

@ -3,6 +3,7 @@
using System;
using System.Threading;
using ln.ethercat.controller.drives;
using ln.ethercat.controller.remote;
using ln.logging;
namespace ln.ethercat.controller
@ -14,7 +15,7 @@ namespace ln.ethercat.controller
CLEARFAULT, // Clear fault state
}
public abstract class ControllerRemote
public abstract class ControllerRemote : IDisposable
{
protected Controller Controller;
@ -34,7 +35,11 @@ namespace ln.ethercat.controller
public ControllerRemote(Controller controller)
{
Controller = controller;
Controller?.Add(this);
}
public abstract double[] Targets { get; set; }
public abstract FeederOperation FeederOperation { get; }
public void Start()
{
@ -103,5 +108,9 @@ namespace ln.ethercat.controller
Shutdown();
}
public void Dispose()
{
Controller?.Remove(this);
}
}
}

10
ln.ethercat/controller/remote/FeederOperation.cs 100644
View File

@ -0,0 +1,10 @@
namespace ln.ethercat.controller.remote
{
public enum FeederOperation
{
NONE,
LEFT,
RIGHT,
TORQUE
}
}

236
ln.ethercat/controller/remote/NewSerialRemote.cs 100644
View File

@ -0,0 +1,236 @@
using System;
using System.IO.Ports;
using System.Threading;
using ln.ethercat.controller.drives;
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
*/
public class NewSerialRemote : ControllerRemote
{
public string SerialPortName { get; }
private FeederOperation _feederOperation;
public override FeederOperation FeederOperation { get => _feederOperation; }
SerialPort serialPort;
bool stopReceiverThread;
Thread threadReceiver;
public double MainTarget { get; set; }
public double FeedTarget { get; set; }
public override double[] Targets
{
get => new[] {MainTarget, FeedTarget};
set => throw new NotImplementedException();
}
public double FeedRatio { get; set; } = 0.5;
public double TorqueRatio { get; set; } = 1.0;
public NewSerialRemote(Controller controller)
:this(controller, SerialPort.GetPortNames()[0]){}
public NewSerialRemote(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);
//Controller.RemoteUpdateTarget(0, MainTarget);
// switch (FeederOperation)
// {
// case FeederOperation.NONE:
// Controller.RemoteUpdateTarget(1, 0);
// break;
// case FeederOperation.LEFT:
// if (Controller.ECMaster.DriveControllers[1].DriveMode == DriveMode.SPEED)
// {
// Controller.RemoteUpdateTarget(1, FeedTarget * FeedRatio);
// } else {
// Controller.ECMaster.DriveControllers[1].DriveMode = DriveMode.SPEED;
// Controller.RemoteUpdateTarget(1, 0);
// }
// break;
// case FeederOperation.RIGHT:
// if (Controller.ECMaster.DriveControllers[1].DriveMode == DriveMode.SPEED)
// {
// Controller.RemoteUpdateTarget(1, -(FeedTarget * FeedRatio));
// } else {
// Controller.ECMaster.DriveControllers[1].DriveMode = DriveMode.SPEED;
// Controller.RemoteUpdateTarget(1, 0);
// }
// break;
// case FeederOperation.TORQUE:
// if (Controller.ECMaster.DriveControllers[1].DriveMode == DriveMode.TORQUE)
// {
// Controller.RemoteUpdateTarget(1, FeedTarget * TorqueRatio);
// } else {
// Controller.ECMaster.DriveControllers[1].DriveMode = DriveMode.TORQUE;
// Controller.RemoteUpdateTarget(1, 0);
// }
// break;
// }
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 = 0;
ushort.TryParse(rxLine.Substring(2,4), System.Globalization.NumberStyles.HexNumber, null, out av);
switch (rxLine[0])
{
case 'A':
int drive = rxLine[1] - '0';
double rel = (double)av / 65535;
switch (drive)
{
case 0:
FeedTarget = rel;
break;
case 2:
MainTarget = rel;
break;
}
Controller.RemoteTriggerWatchdog();
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.ECMaster.DriveControllers[0].DriveMode = DriveMode.SPEED;
Controller.ECMaster.DriveControllers[1].DriveMode = DriveMode.SPEED;
Controller.RemoteAction(CRActions.ENABLE);
}
break;
case 1:
Controller.RemoteAction(CRActions.DISABLE);
break;
}
break;
case 'D':
switch (av)
{
case 3:
_feederOperation = FeederOperation.LEFT;
break;
case 4:
_feederOperation = FeederOperation.RIGHT;
break;
case 14:
_feederOperation = FeederOperation.TORQUE;
break; }
break;
case 'U':
switch (av)
{
case 3:
case 4:
case 14:
_feederOperation = FeederOperation.NONE;
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();
}
public override bool IsSafeToEnable()
{
return (MainTarget==0) && (FeedTarget == 0);
}
}
}

17
ln.ethercat/controller/remote/StupidLEDs.cs 100644
View File

@ -0,0 +1,17 @@
using System;
namespace ln.ethercat.controller.remote
{
[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
}
}

34
ln.ethercat/controller/remote/StupidSerialRemote.cs
View File

@ -16,32 +16,12 @@ namespace ln.ethercat.controller.remote
#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 enum FeederOperation
{
NONE,
LEFT,
RIGHT,
TORQUE
}
public class StupidSerialRemote : ControllerRemote
{
public string SerialPortName { get; }
public FeederOperation FeederOperation { get; set; }
private FeederOperation _feederOperation;
public override FeederOperation FeederOperation { get => _feederOperation; }
SerialPort serialPort;
@ -186,13 +166,13 @@ namespace ln.ethercat.controller.remote
switch (av)
{
case 3:
FeederOperation = FeederOperation.LEFT;
_feederOperation = FeederOperation.LEFT;
break;
case 4:
FeederOperation = FeederOperation.RIGHT;
_feederOperation = FeederOperation.RIGHT;
break;
case 14:
FeederOperation = FeederOperation.TORQUE;
_feederOperation = FeederOperation.TORQUE;
break; }
break;
case 'U':
@ -201,7 +181,7 @@ namespace ln.ethercat.controller.remote
case 3:
case 4:
case 14:
FeederOperation = FeederOperation.NONE;
_feederOperation = FeederOperation.NONE;
break;
}
break;
@ -218,6 +198,8 @@ namespace ln.ethercat.controller.remote
}
public override double[] Targets { get => new []{ MainTarget, FeedTarget }; set => throw new NotSupportedException(); }
protected override void Initialize()
{
stopReceiverThread = false;

22
ln.ethercat/kinematics/Joint.cs 100644
View File

@ -0,0 +1,22 @@
using System.Numerics;
namespace ln.ethercat.kinematics
{
public abstract class Joint
{
public Joint()
{
ForwardMatrix = Matrix4x4.Identity;
}
public Joint(Matrix4x4 forwardMatrix)
{
ForwardMatrix = forwardMatrix;
}
public Matrix4x4 ForwardMatrix { get; protected set; }
}
}

20
ln.ethercat/kinematics/Position.cs 100644
View File

@ -0,0 +1,20 @@
using System.Numerics;
namespace ln.ethercat.kinematics
{
public struct Position
{
public readonly Vector3 Base;
public readonly Vector3 X;
public readonly Vector3 Y;
public readonly Vector3 Z;
public Position(Vector3 b, Vector3 x, Vector3 y, Vector3 z)
{
Base = b;
X = x;
Y = y;
Z = z;
}
}
}

34
ln.ethercat/kinematics/RotationalJoint.cs 100644
View File

@ -0,0 +1,34 @@
using System.Numerics;
namespace ln.ethercat.kinematics
{
public class RotationalJoint : Joint
{
private Vector3 axis;
private float angle;
public RotationalJoint()
{
}
public Vector3 Axis
{
get => axis;
set
{
ForwardMatrix = Matrix4x4.CreateFromAxisAngle(value, angle);
axis = value;
}
}
public float Angle
{
get => angle;
set
{
ForwardMatrix = Matrix4x4.CreateFromAxisAngle(axis, value);
angle = value;
}
}
}
}

22
ln.ethercat/kinematics/TranslatingJoint.cs 100644
View File

@ -0,0 +1,22 @@
using System.Numerics;
namespace ln.ethercat.kinematics
{
public class TranslatingJoint : Joint
{
public TranslatingJoint() :base(Matrix4x4.Identity)
{
}
public Vector3 Translation
{
get => ForwardMatrix.Translation;
set
{
Matrix4x4 m = ForwardMatrix;
m.Translation = value;
ForwardMatrix = m;
}
}
}
}