192 lines
8.7 KiB
C#
192 lines
8.7 KiB
C#
|
|
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.015;
|
|
|
|
}
|
|
|
|
}
|
|
} |