public void Initialize(RobotVector actualRobotPosition)
{
elapsedTime = 0.0;
IsTargetPositionReached = true;
currentMovement = new RobotMovement(actualRobotPosition, RobotVector.Zero, -1.0);
polyX.Initialize(actualRobotPosition.X);
polyY.Initialize(actualRobotPosition.Y);
polyZ.Initialize(actualRobotPosition.Z);
polyA.Initialize(actualRobotPosition.A);
polyB.Initialize(actualRobotPosition.B);
polyC.Initialize(actualRobotPosition.C);
}
public void MoveTo(RobotMovement[] movementsStack)
{
lock (forceMoveSyncLock) {
if (isForceMoveModeEnabled)
{
return;
}
}
if (!isInitialized)
{
throw new InvalidOperationException("Robot is not initialized");
}
for (int i = 0; i < movementsStack.Length; i++)
{
RobotMovement movement = movementsStack[i];
if (!Limits.CheckPosition(movement.TargetPosition))
{
throw new ArgumentException("Target position is outside the available workspace:" +
$"{Environment.NewLine}{movement.TargetPosition}");
}
if (!Limits.CheckVelocity(movement.TargetVelocity))
{
throw new ArgumentException("Target velocity exceeding max value:" +
$"{Environment.NewLine}{movement.TargetVelocity}");
}
}
RobotVector currentVelocity;
RobotVector currentAcceleration;
lock (generatorSyncLock) {
generator.SetMovements(movementsStack);
currentVelocity = generator.Velocity;
currentAcceleration = generator.Acceleration;
}
MovementChanged?.Invoke(this, new MovementChangedEventArgs {
Position = Position,
Velocity = currentVelocity,
Acceleration = currentAcceleration,
MovementsStack = movementsStack
});
}
private void UpdateCurrentMovement(RobotMovement movement)
{
movement = new RobotMovement(
movement.TargetPosition,
movement.TargetVelocity,
movement.TargetDuration - 0.032
);
RobotVector targetPosition = movement.TargetPosition;
RobotVector targetVelocity = movement.TargetVelocity;
double targetDuration = movement.TargetDuration;
if (movement.TargetDuration <= 0.0)
{
throw new ArgumentException($"Duration value must be greater than 0.032s");
}
bool targetPositionChanged = !targetPosition.Compare(currentMovement.TargetPosition, 1, 0.1);
bool targetVelocityChanged = !targetVelocity.Compare(currentMovement.TargetVelocity, 1, 0.1);
bool targetDurationChanged = Math.Abs(targetDuration - currentMovement.TargetDuration) >= 0.001;
if (targetDurationChanged || targetPositionChanged || targetVelocityChanged)
{
double tmpElapsedTime = elapsedTime;
currentMovement = movement;
elapsedTime = 0.0;
IsTargetPositionReached = false;
polyX.UpdateCoefficients(targetPosition.X, targetVelocity.X, targetDuration, tmpElapsedTime);
polyY.UpdateCoefficients(targetPosition.Y, targetVelocity.Y, targetDuration, tmpElapsedTime);
polyZ.UpdateCoefficients(targetPosition.Z, targetVelocity.Z, targetDuration, tmpElapsedTime);
polyA.UpdateCoefficients(targetPosition.A, targetVelocity.A, targetDuration, tmpElapsedTime);
polyB.UpdateCoefficients(targetPosition.B, targetVelocity.B, targetDuration, tmpElapsedTime);
polyC.UpdateCoefficients(targetPosition.C, targetVelocity.C, targetDuration, tmpElapsedTime);
}
}
public void ForceMoveTo(RobotMovement movement)
{
ForceMoveTo(new RobotMovement[] { movement });
}
public void MoveTo(RobotMovement movement)
{
MoveTo(new RobotMovement[] { movement });
}
public void SetMovement(RobotMovement movement)
{
movementsStack.Clear();
UpdateCurrentMovement(movement);
}
