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