/// <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); }