/// <summary>
/// Receives data (IPOC, cartesian and axis position) from the robot asynchronously,
/// raises <see cref="KUKARobot.FrameRecived">FrameReceived</see> event
/// </summary>
private async Task ReceiveDataAsync()
{
InputFrame receivedFrame = await rsiAdapter.ReceiveDataAsync();
RobotVector correction = receivedFrame.Position - position;
if (!Limits.CheckCorrection(correction))
{
Uninitialize();
throw new InvalidOperationException("Correction limit has been exceeded:" +
$"{Environment.NewLine}{correction}");
}
if (!Limits.CheckAxisPosition(receivedFrame.AxisPosition))
{
Uninitialize();
throw new InvalidOperationException("Axis position limit has been exceeded:" +
$"{Environment.NewLine}{receivedFrame.AxisPosition}");
}
if (!Limits.CheckPosition(receivedFrame.Position))
{
Uninitialize();
throw new InvalidOperationException("Available workspace limit has been exceeded:" +
$"{Environment.NewLine}{receivedFrame.Position}");
}
lock (receivedDataSyncLock) {
IPOC = receivedFrame.IPOC;
position = receivedFrame.Position;
axisPosition = receivedFrame.AxisPosition;
}
FrameReceived?.Invoke(receivedFrame);
}
private async Task <long> ReceiveDataAsync()
{
InputFrame receivedFrame = await rsiAdapter.ReceiveDataAsync();
if (!Limits.CheckAxisPosition(receivedFrame.AxisPosition))
{
Uninitialize();
throw new InvalidOperationException("Axis position limit has been exceeded:" +
$"{Environment.NewLine}{receivedFrame.AxisPosition}");
}
if (!Limits.CheckPosition(receivedFrame.Position))
{
Uninitialize();
throw new InvalidOperationException("Available workspace limit has been exceeded:" +
$"{Environment.NewLine}{receivedFrame.Position}");
}
lock (receivedDataSyncLock) {
position = receivedFrame.Position;
axisPosition = receivedFrame.AxisPosition;
}
FrameReceived?.Invoke(this, new FrameReceivedEventArgs {
ReceivedFrame = receivedFrame
});
return(receivedFrame.IPOC);
}
private async Task Connect()
{
cts = new CancellationTokenSource();
IsCancellationRequested = false;
InputFrame receivedFrame = null;
isInitialized = true;
// Connect with the robot
try {
Task.Run(async() => {
receivedFrame = await rsiAdapter.Connect(Config.Port);
}).Wait(cts.Token);
} catch (OperationCanceledException) {
rsiAdapter.Disconnect();
return;
}
lock (generatorSyncLock) {
generator.Initialize(receivedFrame.Position);
}
lock (receivedDataSyncLock) {
position = receivedFrame.Position;
HomePosition = receivedFrame.Position;
}
// Send first response
OutputFrame response = new OutputFrame()
{
Correction = RobotVector.Zero,
IPOC = receivedFrame.IPOC
};
rsiAdapter.SendData(response);
Initialized?.Invoke(this, EventArgs.Empty);
// Start loop for receiving and sending data
while (!IsCancellationRequested)
{
long IPOC = await ReceiveDataAsync();
SendData(IPOC);
}
}
/// <param name="port">Port defined in RSI_EthernetConfig.xml</param>
/// <param name="limits">robot limits</param>
public KUKARobot(int port, RobotLimits limits)
{
rsiAdapter = new RSIAdapter(port);
Limits = limits;
worker = new BackgroundWorker()
{
WorkerSupportsCancellation = true
};
worker.DoWork += async(sender, args) => {
// Connect with the robot
InputFrame receivedFrame = await rsiAdapter.Connect();
generator = new TrajectoryGenerator5(receivedFrame.Position);
lock (receivedDataSyncLock) {
IPOC = receivedFrame.IPOC;
position = receivedFrame.Position;
HomePosition = receivedFrame.Position;
}
// Send first response (prevent connection timeout)
rsiAdapter.SendData(new OutputFrame()
{
Correction = new RobotVector(),
IPOC = IPOC
});
isInitialized = true;
Initialized?.Invoke();
// Start loop for receiving and sending data
while (!worker.CancellationPending)
{
await ReceiveDataAsync();
SendData();
}
isInitialized = false;
rsiAdapter.Disconnect();
};
}
private long ReceiveDataAsync()
{
correctionBuffor.Add(correction);
RobotVector currentCorrection = RobotVector.Zero;
if (correctionBuffor.Count == 8)
{
currentCorrection = correctionBuffor[0];
correctionBuffor.RemoveAt(0);
}
InputFrame receivedFrame = new InputFrame {
Position = position + currentCorrection,
AxisPosition = RobotAxisVector.Zero,
IPOC = 0
};
if (!Limits.CheckAxisPosition(receivedFrame.AxisPosition))
{
Uninitialize();
throw new InvalidOperationException("Axis position limit has been exceeded:" +
$"{Environment.NewLine}{receivedFrame.AxisPosition}");
}
if (!Limits.CheckPosition(receivedFrame.Position))
{
Uninitialize();
throw new InvalidOperationException("Available workspace limit has been exceeded:" +
$"{Environment.NewLine}{receivedFrame.Position}");
}
lock (receivedDataSyncLock) {
position = receivedFrame.Position;
axisPosition = receivedFrame.AxisPosition;
}
FrameReceived?.Invoke(this, new FrameReceivedEventArgs {
ReceivedFrame = receivedFrame
});
return(receivedFrame.IPOC);
}
private void Connect()
{
IsCancellationRequested = false;
InputFrame receivedFrame = new InputFrame {
Position = HomePosition,
AxisPosition = RobotAxisVector.Zero,
IPOC = 0
};
generator.Initialize(receivedFrame.Position);
lock (receivedDataSyncLock) {
position = receivedFrame.Position;
HomePosition = receivedFrame.Position;
}
isInitialized = true;
Initialized?.Invoke(this, EventArgs.Empty);
// Start loop for receiving and sending data
try {
while (!IsCancellationRequested)
{
long IPOC = ReceiveDataAsync();
SendData(IPOC);
Thread.Sleep(4);
}
} catch (Exception e) {
var args = new ErrorOccuredEventArgs {
RobotIp = ToString(),
Exception = e
};
ErrorOccured?.Invoke(this, args);
}
isInitialized = false;
Uninitialized?.Invoke(this, EventArgs.Empty);
}
