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