First implementation of 3 mode switch ( INDEPENDENT, SCREW, PRESS )
parent
1c2a3d6daa
commit
cfd40b86e5
|
@ -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;
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
}
|
|
@ -39,17 +39,24 @@ namespace ln.ethercat.service
|
||||||
{
|
{
|
||||||
ECMaster.RequestPDOMapping(1, 0x2012, 31, true);
|
ECMaster.RequestPDOMapping(1, 0x2012, 31, true);
|
||||||
ECMaster.RequestPDOMapping(1, 0x2012, 32, 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();
|
ethercatService.Start();
|
||||||
|
|
||||||
if (SerialRemotePort != null)
|
if (SerialRemotePort != null)
|
||||||
{
|
{
|
||||||
StupidSerialRemote stupidSerialRemote = new StupidSerialRemote(ethercatService.ECMaster.Controller, SerialRemotePort);
|
// StupidSerialRemote stupidSerialRemote = new StupidSerialRemote(ethercatService.ECMaster.Controller, SerialRemotePort);
|
||||||
stupidSerialRemote.Start();
|
// stupidSerialRemote.Start();
|
||||||
|
NewSerialRemote serialRemote =
|
||||||
|
new NewSerialRemote(ethercatService.ECMaster.Controller, SerialRemotePort);
|
||||||
|
serialRemote.Start();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
@ -98,6 +98,7 @@ LN
|
||||||
d.socket = $ECAPP.connectWebSocket("/api/v1/sockets/controller");
|
d.socket = $ECAPP.connectWebSocket("/api/v1/sockets/controller");
|
||||||
d.socket.onmessage = (evt)=>{
|
d.socket.onmessage = (evt)=>{
|
||||||
let json = JSON.parse(evt.data);
|
let json = JSON.parse(evt.data);
|
||||||
|
console.log(json);
|
||||||
d.controller = json.value.Controller;
|
d.controller = json.value.Controller;
|
||||||
let drives = []
|
let drives = []
|
||||||
for (let n=0;n<d.controller.DriveCount;n++)
|
for (let n=0;n<d.controller.DriveCount;n++)
|
||||||
|
|
|
@ -36,6 +36,9 @@ namespace ln.ethercat.controller
|
||||||
[ESProperty]
|
[ESProperty]
|
||||||
public bool IgnoreRemoteInterface { get; set; }
|
public bool IgnoreRemoteInterface { get; set; }
|
||||||
|
|
||||||
|
private ISet<ControllerRemote> _remotes = new HashSet<ControllerRemote>();
|
||||||
|
public IEnumerable<ControllerRemote> Remotes => _remotes;
|
||||||
|
|
||||||
List<ControlLoop> controlLoops = new List<ControlLoop>();
|
List<ControlLoop> controlLoops = new List<ControlLoop>();
|
||||||
public ControlLoop[] ControlLoops => controlLoops.ToArray();
|
public ControlLoop[] ControlLoops => controlLoops.ToArray();
|
||||||
|
|
||||||
|
@ -77,6 +80,9 @@ namespace ln.ethercat.controller
|
||||||
public void Add(ControllerLogic controllerLogic) => controllerLogics.Add(controllerLogic);
|
public void Add(ControllerLogic controllerLogic) => controllerLogics.Add(controllerLogic);
|
||||||
public void Remove(ControllerLogic controllerLogic) => controllerLogics.Remove(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()
|
public void Initialize()
|
||||||
{
|
{
|
||||||
CycleCounter = 0;
|
CycleCounter = 0;
|
||||||
|
@ -184,11 +190,15 @@ namespace ln.ethercat.controller
|
||||||
|
|
||||||
public bool IsSafeToEnable()
|
public bool IsSafeToEnable()
|
||||||
{
|
{
|
||||||
foreach (ControllerIsSafeToEnable ciste in OnIsSafeToEnable.GetInvocationList())
|
if (!(OnIsSafeToEnable is null))
|
||||||
{
|
{
|
||||||
if (!ciste(this))
|
foreach (ControllerIsSafeToEnable ciste in OnIsSafeToEnable.GetInvocationList())
|
||||||
return false;
|
{
|
||||||
|
if (!ciste(this))
|
||||||
|
return false;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -3,6 +3,7 @@
|
||||||
using System;
|
using System;
|
||||||
using System.Threading;
|
using System.Threading;
|
||||||
using ln.ethercat.controller.drives;
|
using ln.ethercat.controller.drives;
|
||||||
|
using ln.ethercat.controller.remote;
|
||||||
using ln.logging;
|
using ln.logging;
|
||||||
|
|
||||||
namespace ln.ethercat.controller
|
namespace ln.ethercat.controller
|
||||||
|
@ -14,7 +15,7 @@ namespace ln.ethercat.controller
|
||||||
CLEARFAULT, // Clear fault state
|
CLEARFAULT, // Clear fault state
|
||||||
}
|
}
|
||||||
|
|
||||||
public abstract class ControllerRemote
|
public abstract class ControllerRemote : IDisposable
|
||||||
{
|
{
|
||||||
protected Controller Controller;
|
protected Controller Controller;
|
||||||
|
|
||||||
|
@ -34,7 +35,11 @@ namespace ln.ethercat.controller
|
||||||
public ControllerRemote(Controller controller)
|
public ControllerRemote(Controller controller)
|
||||||
{
|
{
|
||||||
Controller = controller;
|
Controller = controller;
|
||||||
|
Controller?.Add(this);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
public abstract double[] Targets { get; set; }
|
||||||
|
public abstract FeederOperation FeederOperation { get; }
|
||||||
|
|
||||||
public void Start()
|
public void Start()
|
||||||
{
|
{
|
||||||
|
@ -103,5 +108,9 @@ namespace ln.ethercat.controller
|
||||||
Shutdown();
|
Shutdown();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
public void Dispose()
|
||||||
|
{
|
||||||
|
Controller?.Remove(this);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
|
@ -0,0 +1,10 @@
|
||||||
|
namespace ln.ethercat.controller.remote
|
||||||
|
{
|
||||||
|
public enum FeederOperation
|
||||||
|
{
|
||||||
|
NONE,
|
||||||
|
LEFT,
|
||||||
|
RIGHT,
|
||||||
|
TORQUE
|
||||||
|
}
|
||||||
|
}
|
|
@ -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);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
|
@ -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
|
||||||
|
}
|
||||||
|
}
|
|
@ -16,32 +16,12 @@ namespace ln.ethercat.controller.remote
|
||||||
#define LED_AUX 0x07
|
#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 class StupidSerialRemote : ControllerRemote
|
||||||
{
|
{
|
||||||
public string SerialPortName { get; }
|
public string SerialPortName { get; }
|
||||||
|
|
||||||
public FeederOperation FeederOperation { get; set; }
|
private FeederOperation _feederOperation;
|
||||||
|
public override FeederOperation FeederOperation { get => _feederOperation; }
|
||||||
|
|
||||||
SerialPort serialPort;
|
SerialPort serialPort;
|
||||||
|
|
||||||
|
@ -186,13 +166,13 @@ namespace ln.ethercat.controller.remote
|
||||||
switch (av)
|
switch (av)
|
||||||
{
|
{
|
||||||
case 3:
|
case 3:
|
||||||
FeederOperation = FeederOperation.LEFT;
|
_feederOperation = FeederOperation.LEFT;
|
||||||
break;
|
break;
|
||||||
case 4:
|
case 4:
|
||||||
FeederOperation = FeederOperation.RIGHT;
|
_feederOperation = FeederOperation.RIGHT;
|
||||||
break;
|
break;
|
||||||
case 14:
|
case 14:
|
||||||
FeederOperation = FeederOperation.TORQUE;
|
_feederOperation = FeederOperation.TORQUE;
|
||||||
break; }
|
break; }
|
||||||
break;
|
break;
|
||||||
case 'U':
|
case 'U':
|
||||||
|
@ -201,7 +181,7 @@ namespace ln.ethercat.controller.remote
|
||||||
case 3:
|
case 3:
|
||||||
case 4:
|
case 4:
|
||||||
case 14:
|
case 14:
|
||||||
FeederOperation = FeederOperation.NONE;
|
_feederOperation = FeederOperation.NONE;
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
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()
|
protected override void Initialize()
|
||||||
{
|
{
|
||||||
stopReceiverThread = false;
|
stopReceiverThread = false;
|
||||||
|
|
|
@ -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; }
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
}
|
|
@ -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;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
|
@ -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;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
|
@ -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;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
Loading…
Reference in New Issue