/// <summary> /// Sends data (IPOC, correction) to the robot, raises <see cref="KUKARobot.FrameSent">FrameSent</see> event /// </summary> private void SendData() { RobotVector correction; lock (forceMoveSyncLock) { correction = generator.GetNextCorrection(position); } correction = new RobotVector(correction.X, correction.Y, correction.Z, 0, 0, 0); // CAŁY IF DO ZAKOMENTOWANIA DLA POZYCJI ABSOLUTNEJ if (!Limits.CheckCorrection(correction)) { Uninitialize(); throw new InvalidOperationException("Correction limit has been exceeded:" + $"{Environment.NewLine}{correction}"); } OutputFrame outputFrame = new OutputFrame() { Correction = correction, IPOC = IPOC }; rsiAdapter.SendData(outputFrame); FrameSent?.Invoke(outputFrame); }
private void SendData(long IPOC) { RobotVector correction; RobotVector currentVelocity; RobotVector currentAcceleration; RobotVector targetPosition; RobotVector targetVelocity; double targetDuration; lock (generatorSyncLock) { // GetNextCorrection() updates theoretical values correction = generator.GetNextCorrection(); currentVelocity = generator.Velocity; currentAcceleration = generator.Acceleration; targetPosition = generator.TargetPosition; targetVelocity = generator.TargetVelocity; targetDuration = generator.TargetDuration; } if (!Limits.CheckCorrection(correction)) { throw new InvalidOperationException("Correction limit has been exceeded:" + $"{Environment.NewLine}{correction}"); } if (!Limits.CheckVelocity(currentVelocity)) { throw new InvalidOperationException("Velocity limit has been exceeded:" + $"{Environment.NewLine}{currentVelocity}"); } if (!Limits.CheckAcceleration(currentAcceleration)) { throw new InvalidOperationException("Acceleration limit has been exceeded:" + $"{Environment.NewLine}{currentAcceleration}"); } OutputFrame outputFrame = new OutputFrame() { Correction = correction, IPOC = IPOC }; rsiAdapter.SendData(outputFrame); FrameSent?.Invoke(this, new FrameSentEventArgs { FrameSent = outputFrame, Position = position, TargetPosition = targetPosition, TargetVelocity = targetVelocity, TargetDuration = targetDuration }); }
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); } }
private void SendData(long IPOC) { correction = generator.GetNextCorrection(); if (!Limits.CheckCorrection(correction)) { Uninitialize(); throw new InvalidOperationException("Correction limit has been exceeded:" + $"{Environment.NewLine}{correction}"); } OutputFrame outputFrame = new OutputFrame() { Correction = correction, IPOC = IPOC }; FrameSent?.Invoke(this, new FrameSentEventArgs { FrameSent = outputFrame, Position = position }); }
/// <summary> /// Sends data to the remoteEndPoint (KUKA robot) /// </summary> /// <param name="data">data to sent</param> public void SendData(OutputFrame data) { byte[] bytes = Encoding.ASCII.GetBytes(data.ToString()); client.Send(bytes, bytes.Length, remoteEndPoint); }