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