595 lines
16 KiB
C#
595 lines
16 KiB
C#
using ACS.SPiiPlusNET;
|
|
using Aurora;
|
|
using Aurora.Api;
|
|
using Aurora.Api.Devices;
|
|
using AuroraMotor;
|
|
using Hermes;
|
|
using PropertyTools.DataAnnotations;
|
|
using System;
|
|
using System.Collections.Generic;
|
|
using System.Linq;
|
|
using System.Runtime.InteropServices;
|
|
using System.Text;
|
|
using System.Threading.Tasks;
|
|
using Axis = ACS.SPiiPlusNET.Axis;
|
|
|
|
namespace AuroraACS
|
|
{
|
|
public enum JogDirection
|
|
{
|
|
plus = 1,
|
|
minus = -1,
|
|
}
|
|
public enum Speed
|
|
{
|
|
L,
|
|
M,
|
|
H
|
|
}
|
|
|
|
public class ACSMotor : AuroraMotorBase // BaseMotor
|
|
{
|
|
private readonly Logger Log = new Logger("ACSMotor");
|
|
|
|
public bool IsInit = false;
|
|
|
|
public int BufferNo;
|
|
public Axis AxisNo;
|
|
private readonly string AxisName;
|
|
private double TargetPosition;
|
|
private double TargetPositionABS;
|
|
private TargetPosType TargetType;
|
|
ACSDevice ACSLibrary;
|
|
private ACSMotorConfig _Config;
|
|
bool IsHomeComplete = false;
|
|
|
|
public ACSMotor(string deviceId, ACSDevice acs, int bufferNo, int axisNo, string axisName = null) : base(deviceId)
|
|
{
|
|
ACSLibrary = acs;
|
|
BufferNo = bufferNo;
|
|
AxisNo = (Axis)axisNo;
|
|
AxisName = axisName;
|
|
}
|
|
|
|
protected override int DoActivate(IDeviceConfig config)
|
|
{
|
|
if (IsActivated) return E1.OK;
|
|
|
|
_Config = config as ACSMotorConfig;
|
|
if (!ACSLibrary.IsActivated) {
|
|
Log.E("ACS Not activated.");
|
|
return E1.NETWORK;
|
|
}
|
|
if (ACSLibrary.IsSimul) {
|
|
IsSimul = ACSLibrary.IsSimul;
|
|
AxisNo += 4;
|
|
ServoOn(true);
|
|
}
|
|
return E1.OK;
|
|
}
|
|
|
|
protected override int DoDeactivate()
|
|
{
|
|
if (!IsActivated) return E1.OK;
|
|
|
|
if (!ACSLibrary.IsActivated) {
|
|
return E1.OK;
|
|
}
|
|
|
|
IsInit = false;
|
|
IsHomeComplete = false;
|
|
return E1.OK;
|
|
}
|
|
|
|
public override int ClearAlarm()
|
|
{
|
|
ACSLibrary.ClearAlam(AxisNo);
|
|
return E1.OK;
|
|
}
|
|
|
|
public override bool IsAlarm()
|
|
{
|
|
var state = ACSLibrary.GetMotorAlarmCode(AxisNo);
|
|
var fault = ACSLibrary.GetFault(AxisNo);
|
|
if (state == 0 && fault == 0) {
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
public override IList<string> GetAlarm()
|
|
{
|
|
List<string> alarms = new List<string>();
|
|
if (!IsActivated) {
|
|
return alarms;
|
|
}
|
|
var mask = ACSLibrary.GetMotorAlarmCode(AxisNo);
|
|
if (mask == 0) {
|
|
return alarms;
|
|
}
|
|
|
|
alarms.Add(ACSLibrary.GetMotorAlarmDesc(mask));
|
|
|
|
return alarms;
|
|
}
|
|
|
|
public override double GetActualPos()
|
|
{
|
|
return ACSLibrary.ReadMotorPosition(AxisNo);
|
|
}
|
|
|
|
public double GetReferencePosition()
|
|
{
|
|
return ACSLibrary.ReadRPosition(AxisNo);
|
|
}
|
|
|
|
public override bool IsTargetPosLimit()
|
|
{
|
|
if (_Config == null) return true;
|
|
if (!IsActivated) return true;
|
|
|
|
return (TargetPositionABS < _Config.MoveRange.Min) || (TargetPositionABS > _Config.MoveRange.Max);
|
|
}
|
|
|
|
public override Range2<double> GetPosLimit()
|
|
{
|
|
if (_Config == null) return null;
|
|
if (!IsActivated) return null;
|
|
|
|
return new Range2<double>(_Config.MoveRange.Min, _Config.MoveRange.Max);
|
|
}
|
|
|
|
public override double GetTargetPos()
|
|
{
|
|
return TargetPositionABS;
|
|
}
|
|
|
|
public override double GetActualVel()
|
|
{
|
|
var vel = ACSLibrary.GetVelocity(AxisNo);
|
|
if (vel < 0) {
|
|
return 0;
|
|
}
|
|
|
|
return (uint)vel;
|
|
}
|
|
|
|
public double GetFeedbackVelocity()
|
|
{
|
|
return ACSLibrary.GetFeedbackVelocity(AxisNo);
|
|
}
|
|
|
|
public override Range<double> GetVelLimit()
|
|
{
|
|
return new Range<double>(_Config.VelocityRange.Min, _Config.VelocityRange.Max);
|
|
}
|
|
|
|
public override int Home()
|
|
{
|
|
if (ACSLibrary.IsSimul) {
|
|
ACSLibrary.SetZeroPosition(AxisNo);
|
|
IsHomeComplete = true;
|
|
return E1.OK;
|
|
}
|
|
|
|
if (!ACSLibrary.RunBuffer((ProgramBuffer)BufferNo)) {
|
|
Log.E("Motor initiallize fail : " + DeviceId);
|
|
return E1.GENERIC;
|
|
}
|
|
return E1.OK;
|
|
}
|
|
public int HomeStop()
|
|
{
|
|
ACSLibrary.StopBuffer((ProgramBuffer)BufferNo);
|
|
Log.E("Motor initiallize Stop : " + DeviceId);
|
|
|
|
return E1.OK;
|
|
}
|
|
|
|
public int Init()
|
|
{
|
|
if (!IsServoOn()) {
|
|
if (ServoOn(true) != E1.OK) {
|
|
Log.E("ServoOn Error : " + DeviceId);
|
|
return E1.GENERIC;
|
|
}
|
|
}
|
|
|
|
if (ACSLibrary.IsSimul) {
|
|
if (Home() != E1.OK) {
|
|
return E1.GENERIC;
|
|
}
|
|
}
|
|
|
|
IsInit = true;
|
|
return E1.OK;
|
|
|
|
//if (!IsInit) {
|
|
// if (!ACSLibrary.RunBuffer((ProgramBuffer)BufferNo)) {
|
|
// Log.E("Motor initiallize fail : " + DeviceId);
|
|
// return E1.GENERIC;
|
|
// }
|
|
// IsHomeComplete = true;
|
|
//}
|
|
|
|
//return E1.OK;
|
|
}
|
|
|
|
public override bool IsHomeCompleted()
|
|
{
|
|
if (ACSLibrary.IsSimul) {
|
|
return true;
|
|
}
|
|
|
|
return true;
|
|
|
|
//string strInit = "initComp_" + AxisName;
|
|
//return ((ACSLibrary.ReadVariableInt(strInit) == 1) ? true : false);
|
|
}
|
|
|
|
public bool IsInitiallized()
|
|
{
|
|
if (ACSLibrary.IsSimul) {
|
|
return IsInit;
|
|
}
|
|
|
|
string strInit = "initComp_" + AxisName;
|
|
IsInit = ((ACSLibrary.ReadVariableInt(strInit) == 1) ? true : false);
|
|
return IsInit;
|
|
}
|
|
|
|
public override bool IsMoving()
|
|
{
|
|
var state = ACSLibrary.GetMotorState(AxisNo);
|
|
if ((state & MotorStates.ACSC_MST_MOVE) != 0) {
|
|
return true;
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
public override bool IsServoOn()
|
|
{
|
|
var state = ACSLibrary.GetMotorState(AxisNo);
|
|
if ((state & MotorStates.ACSC_MST_ENABLE) == 0) {
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
public double GetJogSpeed(Speed speed)
|
|
{
|
|
switch (speed) {
|
|
case Speed.L: return _Config.LowJogSpeed;
|
|
case Speed.M: return _Config.MidJogSpeed;
|
|
case Speed.H: return _Config.HighJogSpeed;
|
|
default: return _Config.LowJogSpeed;
|
|
}
|
|
}
|
|
public int MoveToSWLimit(JogDirection dir, Speed speed)
|
|
{
|
|
double limit = double.NaN;
|
|
if(IsSimul)
|
|
limit = dir == JogDirection.plus ? _Config.MoveRange.Max : _Config.MoveRange.Min;
|
|
else {
|
|
limit = ACSLibrary.GetMotorSoftLimit(AxisNo, dir == JogDirection.plus);
|
|
if(limit == double.NaN)
|
|
return E2.UNKNOWN;
|
|
}
|
|
|
|
SetTargetPos(limit, TargetPosType.ABS);
|
|
SetVelocityImm(GetJogSpeed(speed));
|
|
SetAccelerationImm(GetAcceleration(speed));
|
|
SetDecelerationImm(GetAcceleration(speed));
|
|
SetJerkImm(GetJerk(speed));
|
|
return Move();
|
|
}
|
|
|
|
public override int Jog(int direction, double velocity)
|
|
{
|
|
return ACSLibrary.Jog(AxisNo, velocity * direction);
|
|
}
|
|
|
|
public int Jog(JogDirection dir, Speed speed)
|
|
{
|
|
return Jog((int)dir, GetJogSpeed(speed));
|
|
}
|
|
|
|
public override int Jog(int direction)
|
|
{
|
|
var jogSpeed = (_Config as ACSMotorConfig).MidJogSpeed;
|
|
if (SelectedJogSpeed == eJogSpeed.L) {
|
|
jogSpeed = (_Config as ACSMotorConfig).LowJogSpeed;
|
|
} else if (SelectedJogSpeed == eJogSpeed.H) {
|
|
jogSpeed = (_Config as ACSMotorConfig).HighJogSpeed;
|
|
}
|
|
|
|
//return ACSLibrary.Jog(AxisNo, (GlobalDirection)direction);
|
|
return ACSLibrary.Jog(AxisNo, jogSpeed * direction);
|
|
}
|
|
|
|
public override int JogIncrement(double increment, double velocity)
|
|
{
|
|
SetTargetSPD(velocity);
|
|
SetTargetPos(GetActualPos() + increment);
|
|
Move();
|
|
return -1;
|
|
}
|
|
|
|
public override int Move()
|
|
{
|
|
if (TargetPositionABS < _Config.MoveRange.Min) {
|
|
Log.E($"Limit Over Error : Requested[{TargetPositionABS}] < - Limit[{_Config.MoveRange.Min}]");
|
|
return -1;
|
|
} else if (TargetPositionABS > _Config.MoveRange.Max) {
|
|
Log.E($"Limit Over Error : Requested[{TargetPositionABS}] > - Limit[{_Config.MoveRange.Max}]");
|
|
return -1;
|
|
}
|
|
|
|
if (TargetType == TargetPosType.ABS) {
|
|
return ACSLibrary.MoveABS(AxisNo, TargetPosition);
|
|
} else {
|
|
return ACSLibrary.MoveINC(AxisNo, TargetPosition);
|
|
}
|
|
}
|
|
|
|
public int DoMove(int velocity)
|
|
{
|
|
SetTargetSPD(velocity);
|
|
return Move();
|
|
}
|
|
|
|
public override int ServoOn(bool onOff)
|
|
{
|
|
ACSLibrary.SetMotorEnable(AxisNo, onOff);
|
|
return E1.OK;
|
|
}
|
|
|
|
public override int SetTargetPos(double targetPos, TargetPosType type = TargetPosType.ABS)
|
|
{
|
|
TargetType = type;
|
|
TargetPosition = targetPos;
|
|
if (TargetType == TargetPosType.ABS) {
|
|
TargetPositionABS = TargetPosition;
|
|
} else {
|
|
TargetPositionABS = GetActualPos() + TargetPosition;
|
|
}
|
|
|
|
return E1.OK;
|
|
}
|
|
|
|
public override int SetTargetSPD(double tarVel)
|
|
{
|
|
if (ACSLibrary.SetVelocity(AxisNo, tarVel)) {
|
|
return E1.OK;
|
|
}
|
|
return E1.ARGUMENT;
|
|
}
|
|
|
|
public override double GetTargetSPD()
|
|
{
|
|
return ACSLibrary.GetVelocity(AxisNo);
|
|
}
|
|
|
|
private eJogSpeed SelectedJogSpeed = eJogSpeed.M;
|
|
public override int SetJogSPD(eJogSpeed jogSPD)
|
|
{
|
|
SelectedJogSpeed = jogSPD;
|
|
return 0;
|
|
}
|
|
|
|
public int SetVelocityImm(double velocity)
|
|
{
|
|
if (ACSLibrary.SetVelocityImm(AxisNo, velocity)) {
|
|
return E1.OK;
|
|
}
|
|
return E1.ARGUMENT;
|
|
}
|
|
|
|
public int SetMoveVelocityImm(Speed speed)
|
|
{
|
|
double velocity = 100;
|
|
switch (speed)
|
|
{
|
|
case Speed.L:
|
|
velocity = _Config.LowMoveSpeed;
|
|
break;
|
|
case Speed.M:
|
|
velocity = _Config.MidMoveSpeed;
|
|
break;
|
|
case Speed.H:
|
|
velocity = _Config.HighMoveSpeed;
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
return SetVelocityImm(velocity);
|
|
}
|
|
|
|
public int SetAccelerationImm(double accel)
|
|
{
|
|
if (ACSLibrary.SetAccelerationImm(AxisNo, accel)) {
|
|
return E1.OK;
|
|
}
|
|
return E1.ARGUMENT;
|
|
}
|
|
public int AccelerationImm(Speed speed)
|
|
{
|
|
return SetAccelerationImm(GetAcceleration(speed));
|
|
}
|
|
|
|
public int SetAcceleration(double accel)
|
|
{
|
|
if (ACSLibrary.setAcceleration(AxisNo, accel)) {
|
|
return E1.OK;
|
|
}
|
|
return E1.ARGUMENT;
|
|
}
|
|
|
|
public double GetAcceleration()
|
|
{
|
|
return ACSLibrary.GetAcceleration(AxisNo);
|
|
}
|
|
|
|
public int SetDeceleration(double decel)
|
|
{
|
|
if (ACSLibrary.SetDeceleration(AxisNo, decel)) {
|
|
return E1.OK;
|
|
}
|
|
return E1.ARGUMENT;
|
|
}
|
|
|
|
public double GetDeceleration()
|
|
{
|
|
return ACSLibrary.GetDeceleration(AxisNo);
|
|
}
|
|
|
|
public int SetDecelerationImm(double decel)
|
|
{
|
|
if (ACSLibrary.SetDecelerationImm(AxisNo, decel)) {
|
|
return E1.OK;
|
|
}
|
|
return E1.ARGUMENT;
|
|
}
|
|
public double GetAcceleration(Speed speed)
|
|
{
|
|
switch (speed) {
|
|
case Speed.L: return _Config.LowMoveAcc;
|
|
case Speed.M: return _Config.MidMoveAcc;
|
|
case Speed.H: return _Config.HighMoveAcc;
|
|
default: return _Config.LowMoveAcc;
|
|
}
|
|
}
|
|
|
|
public int DecelerationImm(Speed speed)
|
|
{
|
|
return SetDecelerationImm(GetAcceleration(speed));
|
|
}
|
|
|
|
public double GetKillDeceleration()
|
|
{
|
|
return ACSLibrary.GetKillDeceleration(AxisNo);
|
|
}
|
|
public double GetJerk(Speed speed)
|
|
{
|
|
switch (speed) {
|
|
case Speed.L: return _Config.LowMoveAcc * 10;
|
|
case Speed.M: return _Config.MidMoveAcc * 10;
|
|
case Speed.H: return _Config.HighMoveAcc * 10;
|
|
default: return _Config.LowMoveAcc * 10;
|
|
}
|
|
}
|
|
public bool SetJerkImm(double dVel)
|
|
{
|
|
if (!IsActivated)
|
|
return false;
|
|
|
|
return ACSLibrary.SetJerkImm(AxisNo, dVel);
|
|
}
|
|
public bool JerkImm(Speed speed)
|
|
{
|
|
return SetJerkImm(GetJerk(speed));
|
|
}
|
|
public double GetJerk()
|
|
{
|
|
return ACSLibrary.GetJerk(AxisNo);
|
|
}
|
|
|
|
public override int Stop()
|
|
{
|
|
ACSLibrary.SetHalt(AxisNo);
|
|
return E1.OK;
|
|
}
|
|
|
|
public override int StopJog()
|
|
{
|
|
ACSLibrary.SetHalt(AxisNo);
|
|
return E1.OK;
|
|
}
|
|
|
|
public int DoStopAll()
|
|
{
|
|
ACSLibrary.SetHaltAll();
|
|
return E1.OK;
|
|
}
|
|
|
|
public int EmergencyStop()
|
|
{
|
|
ACSLibrary.SetKill(AxisNo);
|
|
return E1.OK;
|
|
}
|
|
|
|
public virtual bool ReadInposition()
|
|
{
|
|
var state = ACSLibrary.GetMotorState(AxisNo);
|
|
if ((state & MotorStates.ACSC_MST_INPOS) != 0) {
|
|
return true;
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
public bool IsAccelerating()
|
|
{
|
|
var state = ACSLibrary.GetMotorState(AxisNo);
|
|
if ((state & MotorStates.ACSC_MST_ACC) != 0) {
|
|
return true;
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
public bool IsHPLimit()
|
|
{
|
|
return ACSLibrary.IsHPLimit(AxisNo);
|
|
}
|
|
|
|
|
|
public bool IsHNLimit()
|
|
{
|
|
return ACSLibrary.IsHNLimit(AxisNo);
|
|
}
|
|
|
|
public bool IsSPLimit()
|
|
{
|
|
return ACSLibrary.IsSPLimit(AxisNo);
|
|
}
|
|
|
|
public bool IsSNLimit()
|
|
{
|
|
return ACSLibrary.IsSNLimit(AxisNo);
|
|
}
|
|
|
|
|
|
public void RunBuffer(string label, Action<int> programEnd)
|
|
{
|
|
ACSLibrary.RunBuffer((ProgramBuffer)BufferNo, label, programEnd);
|
|
}
|
|
|
|
public override bool IsInpos()
|
|
{
|
|
return ACSLibrary.ReadInposition(AxisNo);
|
|
}
|
|
public double GetCurrentOverload()
|
|
{
|
|
return ACSLibrary.GetAxisCurrentOverload(AxisNo);
|
|
}
|
|
|
|
public bool IsReachedTargetPosition(double targetPosition)
|
|
{
|
|
if (ApplicationExt.IsSimulation)
|
|
return true;
|
|
|
|
if (!IsActivated)
|
|
return false;
|
|
|
|
if (Math.Abs(GetActualPos() - targetPosition) < 0.1 && IsInpos())
|
|
return true;
|
|
|
|
return false;
|
|
}
|
|
}
|
|
}
|