public override void MoveRelAxis(RealAxis axis, MLengthSpeed speed, bool waitForCompletion)
{
Millimeters mmSpeed = new Millimeters(speed);
try
{
lock (this)
{
int axisNumber = axis.AxisNo + 1;
double percent = mmSpeed;
double dPos = axis.TargetPosition;
string cmd = string.Format("DRIVEI({0},{1:F2}),S={2:F2}", axisNumber, dPos, percent);
SendCommand(cmd);
string ret = WaitForCommand();
CheckOKReplied("DRIVEI", ret, "RUN");
ret = WaitForCommand(30000);
CheckOKReplied("WaitMoveDone", ret, "END");
}
lock (this)
{
OnUpdateCurrentPositions();
}
}
catch (Exception ex)
{
//U.LogAlarmPopup(ex, "Move Absolute Command fail {0}", this.Nickname);
throw new MCoreExceptionPopup(ex, "Move Absolute Command fail {0}", this.Nickname);
}
}
private void OnUpdateCurrentPositions()
{
// We assume only one axis
RealAxis axis = _listRealAxis[0] as RealAxis;
if (axis != null)
{
try
{
string ret;
ret = SendPortCommand("?D0", PortSetting.ReadWriteTimeOut);
if (ret != null)
{
string[] recvPos = new string[4];
recvPos = ret.Split('=');
axis.SetCurrentPosition(double.Parse(recvPos[1]) / 100.0);
}
}
catch (Exception ex)
{
//U.LogError(ex, "Error for set current position '{0}'", this.Nickname);
throw new MCoreExceptionPopup(ex, "Error for set current position '{0}'", this.Nickname);
}
}
}
/// <summary>
/// Home an Axis
/// </summary>
/// <param name="axis"></param>
public override void HomeAxis(RealAxis axis)
{
if (HomeAxisDS402(axis, axis.HomeMethod))
{
axis.Homed = true;
}
}
public override void HomeAxis(RealAxis axis)
{
int axisNumber = axis.AxisNo + 1;
//lock (this)
//{
try
{
String ret;
SendPortCommand("|." + axisNumber.ToString(), PortSetting.ReadWriteTimeOut, false);
ret = SendPortCommand("?99.1", PortSetting.ReadWriteTimeOut, true);
if (!ret.Contains("Ux"))
{
axis.Homed = false;
throw new MCoreExceptionPopup("Origin command failed {0}", this.Nickname);
}
}
catch (Exception ex)
{
axis.Homed = false;
throw new MCoreExceptionPopup(ex, "Origin command failed {0}", this.Nickname);
}
WaitForHomeDone(60000);
axis.Homed = true;
System.Threading.Thread.Sleep(2000);
OnUpdateCurrentPositions();
//}
}
/// <summary>
/// MoveAbsAxis
/// </summary>
/// <param name="axis"></param>
public override void MoveAbsAxis(RealAxis axis, MLengthSpeed speed, bool waitForCompletion)
{
try
{
string ret = "";
ret = SendPortCommand(string.Format("P1={0}", (int)(axis.TargetPosition * 100)), PortSetting.ReadWriteTimeOut);
//System.Threading.Thread.Sleep(10);
ret = SendPortCommand(string.Format("S1={0}", (int)speed), PortSetting.ReadWriteTimeOut);
//System.Threading.Thread.Sleep(10);
ret = SendPortCommand(string.Format("AC1={0}", (int)axis.AccelDecel), PortSetting.ReadWriteTimeOut);
//System.Threading.Thread.Sleep(10);
ret = SendPortCommand(string.Format("DC1={0}", (int)axis.AccelDecel), PortSetting.ReadWriteTimeOut);
//System.Threading.Thread.Sleep(10);
ret = SendPortCommand(string.Format("M1=1"), PortSetting.ReadWriteTimeOut);
//System.Threading.Thread.Sleep(10);
ret = SendPortCommand(String.Format("START1"), PortSetting.ReadWriteTimeOut);
System.Threading.Thread.Sleep(10);
if (!ret.Contains("RUN") && !ret.Contains("OK"))
{
throw new MCoreExceptionPopup("Move Absolute Command fail {0}", this.Nickname);
}
WaitForMoveDone(10000);
System.Threading.Thread.Sleep(25);
OnUpdateCurrentPositions();
}
catch (Exception ex)
{
//U.LogError(ex, "Move Absolute Command fail {0}", this.Nickname);
throw new MCoreExceptionPopup(ex, "Move Absolute Command fail {0}", this.Nickname);
}
}
//private void UpdateLoop(object sender, DoWorkEventArgs e)
//{
// do
// {
// try
// {
// OnUpdateCurrentPositions();
// Thread.Sleep(100);
// }
// catch (Exception ex)
// {
// U.Log(ex);
// return;
// }
// } while (!_destroy);
//}
private void OnUpdateCurrentPositions()
{
// We assume only one axis
RealAxis axis = _listRealAxis[0] as RealAxis;
if (axis != null)
{
try
{
string ret;
ret = SendPortCommand("?96." + axis.AxisNo.ToString(), PortSetting.ReadWriteTimeOut, true);
if (ret != null & ret.Contains("="))
{
string pulsePos = ret.Substring((ret.IndexOf("=") + 1), ret.Length - (ret.IndexOf("=") + 1));
double dPulsePos = double.Parse(pulsePos);
double linearPos = PulsePosConvertToRealPos(dPulsePos);
axis.SetCurrentPosition(linearPos);
}
}
catch (Exception ex)
{
throw new MCoreExceptionPopup(ex, "Error for set current position '{0}'", this.Nickname);
}
}
}
public override void HomeAxis(RealAxis axis)
{
int axisNumber = axis.AxisNo + 1;
try
{
lock (this)
{
_waitDataRecived.Reset();
_modbusClient.WriteSingleCoil(IAIConstAddr.HOME, false);
WaitDataReceived(ReadTimeOut);
_waitDataRecived.Reset();
_modbusClient.WriteSingleCoil(IAIConstAddr.HOME, true);
WaitDataReceived(ReadTimeOut);
}
}
catch (Exception ex)
{
axis.Homed = false;
//U.LogAlarmPopup(ex, "Origin command failed {0}", this.Nickname);
throw new MCoreExceptionPopup(ex, "origin command failed {0}", this.Nickname);
}
WaitForHomeDone(20000);
axis.Homed = true;
System.Threading.Thread.Sleep(100);
lock (this)
{
OnUpdateCurrentPositions();
}
}
public override void EnableAxis(RealAxis axis, bool bEnable)
{
bool servoOnOff = true;
if (bEnable == true)
{
servoOnOff = true;
}
else
{
servoOnOff = false;
}
try
{
int retryCount = 3;
string ret = "";
for (int i = 0; i < retryCount; i++)
{
lock (this)
{
_waitDataRecived.Reset();
_modbusClient.WriteSingleCoil(IAIConstAddr.SON, servoOnOff);
WaitDataReceived(ReadTimeOut);
}
}
axis.Enabled = bEnable;
OnUpdateCurrentPositions();
}
catch (Exception ex)
{
//U.LogAlarmPopup(ex, "Enable Axis Command fail {0}", this.Nickname);
throw new MCoreExceptionPopup(ex, "Enable Axis Command fail {0}", this.Nickname);
}
}
public override void HomeAxis(RealAxis axis)
{
int axisNumber = axis.AxisNo + 1;
lock (this)
{
try
{
String ret = SendPortCommand("ORG", PortSetting.ReadWriteTimeOut);
if (!ret.Contains("RUN") && !ret.Contains("OK"))
{
axis.Homed = false;
// U.LogError("Origin command failed {0}", this.Nickname);
throw new MCoreExceptionPopup("Origin command failed {0}", this.Nickname);
}
}
catch (Exception ex)
{
axis.Homed = false;
//U.LogError(ex, "Origin command failed {0}", this.Nickname);
throw new MCoreExceptionPopup(ex, "Origin command failed {0}", this.Nickname);
}
WaitForHomeDone(60000);
axis.Homed = true;
System.Threading.Thread.Sleep(2000);
OnUpdateCurrentPositions();
}
}
private void OnUpdateCurrentPositions()
{
_retryUpdate = 10;
bool readSuccess = false;
do
{
// We assume only one axis
RealAxis axis = _listRealAxis[0] as RealAxis;
if (axis != null)
{
int lenght = 2;
try
{
lock (this)
{
Int32[] values = _modbusClient.ReadHoldingRegisters(IAIConstAddr.PNOW, lenght);
string sHighBytePos = ((Int16)values[0]).ToString("X4");
string sLowBytePos = ((Int16)values[1]).ToString("X4");
string sPos = sHighBytePos + sLowBytePos;
Int32 iPos = Int32.Parse(sPos, System.Globalization.NumberStyles.AllowHexSpecifier);
double factor = 0.01;
double dPos = (double)iPos * factor;
axis.SetCurrentPosition(dPos);
readSuccess = true;
}
}
catch (Exception ex)
{
_retryUpdate--;
readSuccess = false;
System.Threading.Thread.Sleep(100);
//U.LogAlarmPopup(ex, "Error for set current position '{0}'", this.Nickname);
if (_retryUpdate <= 0)
{
throw new MCoreExceptionPopup(ex, "Error for set current position '{0}'", this.Nickname);
}
}
}
} while (!readSuccess && _retryUpdate > 0);
}
public override void HomeAxis(RealAxis axis)
{
int axisNumber = axis.AxisNo + 1;
axis.Homed = false;
try
{
lock (this)
{
string ret = string.Empty;
SendCommand("STOP");
ret = WaitForCommand();
CheckOKReplied("STOP", ret, "OK");
SendCommand("ALMRST");
ret = WaitForCommand();
CheckOKReplied("ALMRST", ret, "RUN");
ret = WaitForCommand();
CheckOKReplied("ALMRST", ret, "END");
SendCommand("ASPEED 100");
ret = WaitForCommand();
CheckOKReplied("ASPEED", ret, "OK");
SendCommand(string.Format("DRIVE ({0},0.0),S=1", axisNumber));
ret = WaitForCommand();
CheckOKReplied("WaitHomeDone", ret, "RUN");
ret = WaitForCommand(60000);
CheckOKReplied("WaitHomeDone", ret, "END");
axis.Homed = true;
}
lock (this)
{
OnUpdateCurrentPositions();
}
}
catch (Exception ex)
{
axis.Homed = false;
//U.LogAlarmPopup(ex, "Origin command failed {0}", this.Nickname);
throw new MCoreExceptionPopup(ex, "origin command failed {0}", this.Nickname);
}
}
public override void EnableAxis(RealAxis axis, bool bEnable)
{
string servoOnOff = "";
if (bEnable == true)
{
servoOnOff = string.Format("(.{0}", axis.AxisNo);
}
else
{
servoOnOff = string.Format(").{0}", axis.AxisNo);
}
try
{
int retryCount = 3;
string ret = "";
for (int i = 0; i < retryCount; i++)
{
string command =
SendPortCommand(servoOnOff, PortSetting.ReadWriteTimeOut, false);
ret = SendPortCommand("?99.1", PortSetting.ReadWriteTimeOut, true);
if (ret.Contains("Ux."))
{
break;
}
}
if (!ret.Contains("Ux."))
{
throw new MCoreExceptionPopup("Enable Axis Command fail {0}", this.Nickname);
}
axis.Enabled = bEnable;
System.Threading.Thread.Sleep(100);
OnUpdateCurrentPositions();
}
catch (Exception ex)
{
throw new MCoreExceptionPopup(ex, "Enable Axis Command fail {0}", this.Nickname);
}
}
public override void EnableAxis(RealAxis axis, bool bEnable)
{
int servoOnOff = 1;
if (bEnable == true)
{
servoOnOff = 1;
}
else
{
servoOnOff = 0;
}
try
{
int retryCount = 3;
string ret = "";
for (int i = 0; i < retryCount; i++)
{
ret = SendPortCommand("SRVO" + servoOnOff, PortSetting.ReadWriteTimeOut);
if (ret.Contains("OK"))
{
break;
}
}
if (!ret.Contains("OK"))
{
//U.LogError("Enable Axis Command fail {0}", this.Nickname);
throw new MCoreExceptionPopup("Enable Axis Command fail {0}", this.Nickname);
}
axis.Enabled = bEnable;
System.Threading.Thread.Sleep(100);
OnUpdateCurrentPositions();
}
catch (Exception ex)
{
//U.LogAlarmPopup(ex, "Enable Axis Command fail {0}", this.Nickname);
throw new MCoreExceptionPopup(ex, "Enable Axis Command fail {0}", this.Nickname);
}
}
/// <summary>
/// MoveAbsAxis
/// </summary>
/// <param name="axis"></param>
public override void MoveAbsAxis(RealAxis axis, MLengthSpeed speed, bool waitForCompletion)
{
try
{
string ret = "";
double dTargetPos = RealPosionConvertToPulsePos(axis.TargetPosition);
ret = SendPortCommand(string.Format("P.{0}={1}", axis.AxisNo, (int)(dTargetPos)), PortSetting.ReadWriteTimeOut, false);
//System.Threading.Thread.Sleep(10);
double dTargetSpeed = RealSpeedConvertToPulseSpeed(speed);
ret = SendPortCommand(string.Format("S.{0}={1}", axis.AxisNo, (int)dTargetSpeed), PortSetting.ReadWriteTimeOut, false);
//System.Threading.Thread.Sleep(10);
double dTargetAceel = RealSpeedConvertToPulseSpeed(axis.AccelDecel);
ret = SendPortCommand(string.Format("A.{0}={1}", axis.AxisNo, (int)dTargetAceel), PortSetting.ReadWriteTimeOut, false);
//System.Threading.Thread.Sleep(10);
ret = SendPortCommand(string.Format("^.{0}", axis.AxisNo), PortSetting.ReadWriteTimeOut, false);
//System.Threading.Thread.Sleep(10);
ret = SendPortCommand(string.Format("?99.{0}", axis.AxisNo), PortSetting.ReadWriteTimeOut, true);
System.Threading.Thread.Sleep(10);
if (!ret.Contains("Ux"))
{
throw new MCoreExceptionPopup("Move Absolute Command fail {0}", this.Nickname);
}
WaitForMoveDone(10000);
System.Threading.Thread.Sleep(30);
OnUpdateCurrentPositions();
}
catch (Exception ex)
{
//U.LogError(ex, "Move Absolute Command fail {0}", this.Nickname);
throw new MCoreExceptionPopup(ex, "Move Absolute Command fail {0}", this.Nickname);
}
}
/// <summary>
/// Home an Axis
/// </summary>
/// <param name="axis"></param>
public virtual void HomeAxis(RealAxis axis)
{
// Simulation
axis.SimulateHome();
}
/// <summary>
/// Home an Axis
/// </summary>
/// <param name="axis"></param>
public override void StopAxis(RealAxis axis)
{
StopAxis(axis);
}
/// <summary>
/// EnableAxis an Axis
/// </summary>
/// <param name="axis"></param>
/// <param name="bEnable"></param>
public override void EnableAxis(RealAxis axis, bool bEnable)
{
EnableAxis(axis, bEnable);
axis.Enabled = bEnable;
}
/// <summary>
/// Home an Axis
/// </summary>
/// <param name="axis"></param>
public override void MoveAbsAxis(RealAxis axis, MLengthSpeed speed, bool waitForCompletion)
{
Millimeters mmSpeed = new Millimeters(speed);
MoveAbsAxis(axis, axis.TargetPosition, (float)mmSpeed, waitForCompletion);
}
/// <summary>
/// MoveAbsAxis
/// </summary>
/// <param name="axis"></param>
public override void MoveAbsAxis(RealAxis axis, MLengthSpeed speed, bool waitForCompletion)
{
try
{
int iPos = (int)(axis.TargetPosition / 0.01);
int iInpos = 1;
int iSpeed = (int)(speed / 0.01);
int iAccel = (int)(axis.AccelDecel / 0.01);
string sPos = iPos.ToString("X8");
string sInpos = iInpos.ToString("X8");
string sSPeed = iSpeed.ToString("X8");
string s = sPos.Substring(4, 4);
int iPos1 = int.Parse(sPos.Substring(0, 4), System.Globalization.NumberStyles.AllowHexSpecifier);
int iPos2 = int.Parse(sPos.Substring(4, 4), System.Globalization.NumberStyles.AllowHexSpecifier);
int iInpos1 = int.Parse(sInpos.Substring(0, 4), System.Globalization.NumberStyles.AllowHexSpecifier);
int iInpos2 = int.Parse(sInpos.Substring(4, 4), System.Globalization.NumberStyles.AllowHexSpecifier);
int iSpeed1 = int.Parse(sSPeed.Substring(0, 4), System.Globalization.NumberStyles.AllowHexSpecifier);
int iSpeed2 = int.Parse(sSPeed.Substring(4, 4), System.Globalization.NumberStyles.AllowHexSpecifier);
int[] regValue = new int[7];
regValue[0] = iPos1;
regValue[1] = iPos2;
regValue[2] = iInpos1;
regValue[3] = iInpos2;
regValue[4] = iSpeed1;
regValue[5] = iSpeed2;
regValue[6] = iAccel;
lock (this)
{
_waitDataRecived.Reset();
try
{
_modbusClient.WriteMultipleRegisters(IAIConstAddr.PCMD, regValue);
}
catch
{
}
finally
{
}
WaitDataReceived(ReadTimeOut);
}
WaitForMoveDone(10000);
lock (this)
{
System.Threading.Thread.Sleep(100);
OnUpdateCurrentPositions();
}
}
catch (Exception ex)
{
//U.LogAlarmPopup(ex, "Move Absolute Command fail {0}", this.Nickname);
throw new MCoreExceptionPopup(ex, "Move Absolute Command fail {0}", this.Nickname);
}
}
public virtual void MoveRelAxis(RealAxis axis, MLengthSpeed speed, bool waitForCompletion)
{
// Simulation
axis.SimulateMoveRel(speed);
}
/// <summary>
/// EnableAxis an Axis
/// </summary>
/// <param name="axis"></param>
/// <param name="bEnable"></param>
public virtual void EnableAxis(RealAxis axis, bool bEnable)
{
// Simulation
axis.Enabled = bEnable;
}
public override void EnableAxis(RealAxis axis, bool bEnable)
{
string ret = string.Empty;
try
{
lock (this)
{
// Check current status
SendCommand(string.Format("?SERVO"));
ret = WaitForCommand();
if (ret.Length < 9)
{
throw new Exception("Unexpected response");
}
string[] split = ret.Split(',');
if (split.Length < 4)
{
throw new Exception("Drive is not ON");
}
int axisNumber = axis.AxisNo + 1;
if (bEnable)
{
//Reset alarm
SendCommand("STOP");
ret = WaitForCommand();
CheckOKReplied("STOP", ret, "OK");
SendCommand("ALMRST");
ret = WaitForCommand();
CheckOKReplied("ALMRST", ret, "RUN");
ret = WaitForCommand();
CheckOKReplied("ALMRST", ret, "END");
SendCommand(string.Format("SERVO ON({0})", axisNumber));
ret = WaitForCommand();
CheckOKReplied("Enable", ret, "RUN");
ret = WaitForCommand();
CheckOKReplied("Enable", ret, "END");
}
else
{
SendCommand(string.Format("SERVO OFF({0})", axisNumber));
ret = WaitForCommand();
CheckOKReplied("Enable", ret, "RUN");
ret = WaitForCommand();
CheckOKReplied("Enable", ret, "END");
}
axis.Enabled = bEnable;
}
}
catch (Exception ex)
{
//U.LogError(ex, "Failed to enable '{0}'", axis.Name);
throw new MCoreExceptionPopup(ex, "Failed to enable '{0}'", axis.Name);
}
}
/// <summary>
/// Stop Axis if moving
/// </summary>
/// <param name="axis"></param>
public virtual void StopAxis(RealAxis axis)
{
// Simulation
axis.TargetMotorCounts = axis.CurrentMotorCounts;
}
