private MotionInfo GetMotionInfoAtIndex(int motionIndex)
{
var getMotionInfoAtIndexRequest = new GetMotionInfoAtIndexRequest(
RobotPacket.PacketID.GetInfoMotionAtIndex, motionIndex);
RobotReply reply = Connection.SendAndReceivePacket(getMotionInfoAtIndexRequest.BuildRequest());
if (reply == null)
{
return(null);
}
reply.RequestID = RobotPacket.PacketID.GetInfoMotionAtIndex;
RobotReplyData robotReplyData = reply.Process();
if (robotReplyData.Type == RobotReplyData.ReplyType.Failed)
{
return(null);
}
return(new MotionInfo(robotReplyData.Data));
}
private int GetMotionCount()
{
int motionCount = -1;
var getMotionCountRequest = new GetMotionCountRequest(RobotPacket.PacketID.CountMotions);
RobotReply reply = Connection.SendAndReceivePacket(getMotionCountRequest.BuildRequest());
if (reply == null)
{
Debug.Fail("Reply from robot is null");
return(0);
}
reply.RequestID = RobotPacket.PacketID.CountMotions;
RobotReplyData robotReplyData = reply.Process();
//if (robotReplyData.Type == RobotReplyData.ReplyType.Success)
//{
// return (int)GlobalFunction.LE4ToDec(robotReplyData.Data);
//}
switch (robotReplyData.Type)
{
case RobotReplyData.ReplyType.Success:
if (robotReplyData.Data.Length != 4)
{
Debug.Fail("ListAllMotionRequest: data reply is not sufficient");
return(-1);
}
return((int)GlobalFunction.LE4ToDec(robotReplyData.Data));
//case RobotReplyData.ReplyType.Failed:
// OnProcessError(ErrorCode.ReplyFailed, "Reply error");
// return -2;
case RobotReplyData.ReplyType.WrongID:
OnProcessError(ErrorCode.WrongSessionID, "Wrong session ID");
return(-2);
}
return(motionCount);
}
/// <summary>
/// Request and reply process
/// </summary>
/// <returns>Data of reply</returns>
public object Process()
{
RobotReplyData replyData = null;
if (!SetupConnection())
{
OnProcessError(ErrorCode.SetupConnection, "Setup connection error");
_logger.Error("Cannot connect to robot");
return(null);
}
byte[] packet = BuildRequest();
RobotReply reply = null;
int crcCount = 0;
bool crcError = false;
do
{
crcCount++;
//if (GlobalVariables.Sw != null && GlobalVariables.Sw.IsRunning)
//{
// GlobalVariables.Sw.Stop();
// _logger.Info("1 processing session duration: " + GlobalVariables.Sw.ElapsedMilliseconds);
//}
try
{
Conn.SendPacket(packet);
}
catch (SocketException se)
{
OnProcessError(ErrorCode.SocketException, se.Message);
_logger.Error("SendPacket socket exception: " + se.Message);
return(replyData);
}
// For testing transfer speed
//OnProcessSuccessfully(null);
//return replyData;
try
{
reply = Conn.ReceiveReply();
}
catch (SocketException se)
{
OnProcessError(ErrorCode.SocketException, se.Message);
_logger.Error("ReceiveReply socket exception: " + se.Message);
return(replyData);
}
//if (GlobalVariables.Sw == null || !GlobalVariables.Sw.IsRunning) GlobalVariables.Sw = Stopwatch.StartNew();
if (reply != null)
{
reply.RequestID = RequestID;
replyData = reply.Process();
switch (replyData.Type)
{
case RobotReplyData.ReplyType.Success:
OnProcessSuccessfully(replyData);
_logger.Info("Receive reply from robot: type successfully");
break;
case RobotReplyData.ReplyType.Failed:
OnProcessError(ErrorCode.ReplyFailed, "Reply error");
_logger.Error("Receive reply from robot: type failed");
break;
case RobotReplyData.ReplyType.WrongID:
OnProcessError(ErrorCode.WrongSessionID, "Wrong session ID");
_logger.Error("Receive reply from robot: type wrong session ID");
break;
case RobotReplyData.ReplyType.CRC:
crcError = true;
_logger.Error("Receive reply from robot: type CRC error");
break;
}
}
else
{
OnProcessError(ErrorCode.ReplyNull, "Reply cannot be null");
_logger.Error("Receive reply from robot: reply null");
}
} while (crcCount < WirelessConnection.MaxCrcRetry && crcError);
//ReleaseConnection();
return(replyData);
}