236 lines
9.5 KiB
C#
236 lines
9.5 KiB
C#
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);
|
|
}
|
|
}
|
|
} |