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