private async Task incMove(IMove iMove, double speedSP)
{
if (isSafeToMove(iMove))
{
short ret = 0;
StringBuilder vType = new StringBuilder("V"); // VR;
double speed = (speedSP > Limits.maxLinearSpeed) ? Limits.maxLinearSpeed : ((speedSP < 0) ? 0 : (double)speedSP); // is VJ=20.00
StringBuilder framename = new StringBuilder("TOOL"); // ROBOT BASE
short toolNo = 0;
double[] iCoordinates = iMove.ToArray();
if (await IsReadyToOperate())
{
await Task.Delay(commsDelay_mm);
MovingToPosition?.Invoke(this, null);
ret = CMotocom.BscImov(m_Handle, vType, speed, framename, toolNo, ref iCoordinates[0]);
if (ret != 0)
{
SetError("Error MoveToPosition:BscImov");
}
}
}
else
{
SetError("Not safe to move to X=" + movingTo[0] + ", Y=" + movingTo[1] + ", Z=" + movingTo[2] + ", Rz=" + movingTo[5]);
}
}
public async Task ReadPositionVariable(short Index, CRobPosVar PosVar)
{
if (m_commsError)
{
return;
}
await TaskEx.WaitUntil(isIdle);
SetEvent("ReadPositionVariable");
StringBuilder StringVal = new StringBuilder(256);
double[] PosVarArray = new double[12];
short ret = CMotocom.BscHostGetVarData(m_Handle, 4, Index, ref PosVarArray[0], StringVal);
if (ret != 0)
{
SetError("Error executing ReadPositionVariable: " + ret.ToString());
}
//throw new Exception("Error executing BscHostGetVarData");
if (PosVar != null)
{
PosVar.HostGetVarDataArray = PosVarArray;
}
await SetEvent(EnumEvent.Idle);
}
public async Task Disconnect()
{
if (m_Handle < 0)
{
return;
}
SetConnection(EnumComms.Disconnecting);
await TaskEx.WaitUntil(isIdle);
CMotocom.BscDisConnect(m_Handle);
CMotocom.BscClose(m_Handle);
SetConnection(EnumComms.Disconnected);
}
private async Task <bool> ReadGeneralStatus()
{
await TaskEx.WaitUntil(isIdle);
SetEvent("ReadGeneralStatus");
bool isError = false;
short sw1 = -1, sw2 = -1;
short ret = CMotocom.BscGetStatus(m_Handle, ref sw1, ref sw2);
if (ret != 0)
{
isError = true;
SetError("Error calling BscGetStatus");
}
else
{
if (robotStatus.SetAndCheckSWs(sw1, sw2))
{
StatusChanged(this, null);
}
StringBuilder framename = new StringBuilder("ROBOT"); // BASE ROBOT
int formCode = 0;
short isExternal = 0;
short toolNo = 0;
short ret1 = CMotocom.BscGetCartPos(
//short ret1 = CMotocom.BscIsRobotPos(
m_Handle, framename, isExternal, ref formCode, ref toolNo, ref currentPosition[0]);
if (ret1 == -1)
{
isError = true;
SetError("Error executing BscGetCartPos");
}
else
{
currentPosition[13] = formCode;
currentPosition[14] = toolNo;
DispatchCurrentPosition(this, null);
m_previousStatus_ms = DateTimeOffset.Now.ToUnixTimeMilliseconds();
}
}
await SetEvent(EnumEvent.Idle);
return(isError);
}
public async Task StartJob(string JobName)
{
if (m_commsError)
{
return;
}
if (!JobName.ToLower().Contains(".jbi"))
{
SetError("Error *.jbi jobname extension is missing !");
return;
}
await TaskEx.WaitUntil(isIdle);
SetEvent("StartJob");
short ret;
ret = CMotocom.BscSelectJob(m_Handle, JobName);
if (ret == 0)
{
if (await IsReadyToOperate())
{
ret = CMotocom.BscStartJob(m_Handle);
if (ret != 0)
{
SetError("Error starting job !");
}
}
//ret = CMotocom.BscServoOn(m_Handle);
//if (ret != 0) SetError("Error executing BscServoON err:" + ret.ToString());
//else
//{
// ret = CMotocom.BscStartJob(m_Handle);
// if (ret != 0) SetError("Error starting job !");
//}
}
else
{
SetError("Error selecting job !");
}
SetEvent(EnumEvent.Idle);
}
public void Connect()
{
SetConnection(EnumComms.Connecting);
//** Initialize communication **
short ret;
// try to get a handle
// m_Handle = CMotocom.BscOpen(m_CommDir, 256);
m_Handle = CMotocom.BscOpen(m_CommDir, 1); // 1 - serial comms
if (m_Handle >= 0)
{
//set IP Address
// ret = CMotocom.BscSetEServer(m_Handle, m_IPAddress);
ret = CMotocom.BscSetCom(m_Handle, portNumber, 9600, 2, 8, 0); //9600, Even parity, 8 data bits, 1 stop bit
//if (ret!=1) throw new Exception("Could not set IP address !");
if (ret != 1)
{
SetError("Could not set COM port");
return;
//throw new Exception("Could not set COM port");
}
ret = CMotocom.BscConnect(m_Handle);
if (ret == 0)
{
SetError("Error on connecting!");
return;
//throw new Exception("Error on connecting!");
}
SetError("");
SetConnection(EnumComms.Connected);
AutoStatusUpdate = true;
}
else
{
SetError("Could not get a handle. Check Hardware key!");
}
//throw new Exception("Could not get a handle. Check Hardware key !");
}
private async Task <bool> IsReadyToOperate()
{
short ret = 0;
if (robotStatus.isServoOn)
{
if (robotStatus.isOperating)
{
// stop any current jobs
await Task.Delay(commsDelay_mm);
ret = CMotocom.BscHoldOn(m_Handle);
if (ret != 0)
{
SetError("Error IsReadyToOperate:BscHoldOn");
return(false);
}
else
{
await Task.Delay(commsDelay_mm);
ret = CMotocom.BscHoldOff(m_Handle);
if (ret != 0)
{
SetError("Error IsReadyToOperate:BscHoldOff");
return(false);
}
else
{
return(true);
}
}
}
else
{
return(true);
}
}
else
{
SetError("Error MoveToPosition: SERVO is OFF");
return(false);
}
}
public async void ReadByteVariable(short index, double[] posVarArray)
{
if (m_commsError)
{
return;
}
await TaskEx.WaitUntil(isIdle);
SetEvent("ReadByteVariable");
short ret = CMotocom.BscGetVarData(m_Handle, 0, index, ref posVarArray[0]);
if (ret != 0)
{
SetError("Error executing ReadByteVariable");
}
await SetEvent(EnumEvent.Idle);
}
public async Task MoveToHome1(double speedSP = 20)
{
if (m_commsError)
{
return;
}
await TaskEx.WaitUntil(isIdle);
SetEvent("MoveToHome1");
short ret = 0;
double speed = (speedSP > Limits.maxJointSpeed) ? Limits.maxJointSpeed : ((speedSP < 0) ? 0 : speedSP);
short toolNo = 0;
if (await IsReadyToOperate())
{
if (currentPosition[2] < homePosXYZ[2] - 500)
{
IMove iMove = new IMove(0, 0, -300, 0);
await incMove(iMove, Limits.maxLinearSpeed);
}
movingTo[0] = homePosXYZ[0];
movingTo[1] = homePosXYZ[1];
movingTo[2] = homePosXYZ[2];
movingTo[3] = homePosXYZ[3];
movingTo[4] = homePosXYZ[4];
movingTo[5] = homePosXYZ[5];
await Task.Delay(commsDelay_mm);
MovingToPosition?.Invoke(this, null);
ret = CMotocom.BscPMovj(m_Handle, speed, toolNo, ref homePosPulse[0]);
if (ret != 0)
{
SetError("Error MoveToHome1:BscPMovj");
}
}
await SetEvent(EnumEvent.Idle);
}
//private async Task MoveToPosition(double[] posVar, decimal speedSP)
//{
// await TaskEx.WaitUntil(isIdle);
// SetEvent("MoveToPosition");
// short ret = 0;
// StringBuilder vType = new StringBuilder("V"); // VR;
// double speed = (speedSP > 300) ? 300 : ((speedSP < 0) ? 0 : (double)speedSP); // is VJ=20.00
// StringBuilder framename = new StringBuilder("TOOL"); // ROBOT BASE
// short toolNo = 0;
// double[] pos = (posVar != null) ? posVar : new double[]{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
// //if (posVar != null) pos = new double[] { posVar.X, posVar.Y, posVar.Z, posVar.Rx, posVar.Ry, posVar.Rz, 0, 0, 0, 0, 0, 0 };
// //short form = (short)Convert.ToInt32("0", 2); ; // Bits: 0 No-flip, 1 elbow under, 2 Back side, 3 R>=180, 4 T>=180, S>=180
// if (IsReadyToOperate())
// {
// ret = CMotocom.BscImov(m_Handle, vType, speed, framename, toolNo, ref pos[0]);
// if (ret != 0) SetError("Error MoveToPosition:BscMovj");
// }
// SetError("NOT ERROR: finished moving.");
// SetEvent(EnumEvent.Idle);
// //void moveRobot() {
// // ret = (isHome) ?
// // CMotocom.BscPMovj(m_Handle, 30, toolNo, ref homePosPulse[0]) :
// // CMotocom.BscImov(m_Handle, vType, speed, framename, toolNo, ref pos[0]);
// // if (ret != 0) SetError("Error MoveToPosition:BscMovj");
// //}
//}
public async Task CancelOperation()
{
await TaskEx.WaitUntil(isIdle);
SetEvent("CancelOperation");
short ret = 0;
ret = CMotocom.BscHoldOn(m_Handle);
if (ret != 0)
{
SetError("Error CancelOperation:BscHoldOn");
}
else
{
ret = CMotocom.BscHoldOff(m_Handle);
if (ret != 0)
{
SetError("Error CancelOperation:BscHoldOff");
}
}
await SetEvent(EnumEvent.Idle);
}
private async Task WritePositionVariable(short Index, CRobPosVar PosVar)
{
if (m_commsError)
{
return;
}
await TaskEx.WaitUntil(isIdle);
SetEvent("WritePositionVariable");
Console.WriteLine("X is " + PosVar.X.ToString());
StringBuilder StringVal = new StringBuilder(256);
double[] PosVarArray = PosVar.HostGetVarDataArray;
short ret = CMotocom.BscHostPutVarData(m_Handle, 4, Index, ref PosVarArray[0], StringVal);
if (ret != 0)
{
SetError("Error executing WritePositionVariable");
}
await SetEvent(EnumEvent.Idle);
}
public async Task MoveByJob(bool isHome, CRobPosVar position = null)
{
if (isHome)
{
await MoveToHome1();
return;
}
if (position == null)
{
return;
}
await TaskEx.WaitUntil(isIdle);
SetEvent("MoveByJob");
short ret = 0;
string moveHomeJob = "TO_POS_0.jbi";
double[] speed = new double[10];
speed[0] = 2000; // is VJ=20.00
short varNo = 0;
//// stop any current jobs
//ret = CMotocom.BscHoldOn(m_Handle);
//if (ret != 0) SetError("Error MoveToPosition:BscHoldOn");
//else
//{
// write position at varNo
StringBuilder StringVal = new StringBuilder(256);
double[] PosVarArray = position.HostGetVarDataArray;
ret = CMotocom.BscHostPutVarData(m_Handle, 4, varNo, ref PosVarArray[0], StringVal);
if (ret != 0)
{
SetError("Error executing MoveToPosition:WritePositionVariable");
}
else
{
// read position at varNo
StringVal = new StringBuilder(256);
double[] readPosition = new double[10];
ret = CMotocom.BscHostGetVarData(m_Handle, 4, varNo, ref readPosition[0], StringVal);
if (ret != 0)
{
SetError("Error executing MoveToPosition:ReadPositionVariable");
}
else
{
// compare the set home position with read from robot
if (!readPosition.SequenceEqual(position.HostGetVarDataArray))
{
SetError("MoveToPosition: arrays are not equal.");
}
else
{
ret = CMotocom.BscPutVarData(m_Handle, 1, varNo, ref speed[0]);
if (ret != 0)
{
SetError("Error executing MoveToPosition:ReadByteVariable");
}
else
{
// select job
ret = CMotocom.BscSelectJob(m_Handle, moveHomeJob);
if (ret != 0)
{
SetError("Error selecting job at MoveToPosition");
}
else
{
// run the job
ret = CMotocom.BscStartJob(m_Handle);
if (ret != 0)
{
SetError("Error starting job at MoveToPosition!");
}
}
}
}
}
}
//}
await SetEvent(EnumEvent.Idle);
}