public RobotVector GetNextValue(RobotVector currentPosition)
{
lock (syncLock) {
if (!targetPositionReached && timeLeft >= Ts && !currentPosition.Compare(targetPosition, 0.02, 0.004))
{
double nx = polyX.GetNextValue(currentPosition.X, targetPosition.X, targetVelocity.X, timeLeft, Ts);
double ny = polyY.GetNextValue(currentPosition.Y, targetPosition.Y, targetVelocity.Y, timeLeft, Ts);
double nz = polyZ.GetNextValue(currentPosition.Z, targetPosition.Z, targetVelocity.Z, timeLeft, Ts);
double na = polyA.GetNextValue(currentPosition.A, targetPosition.A, targetVelocity.A, timeLeft, Ts);
double nb = polyB.GetNextValue(currentPosition.B, targetPosition.B, targetVelocity.B, timeLeft, Ts);
double nc = polyC.GetNextValue(currentPosition.C, targetPosition.C, targetVelocity.C, timeLeft, Ts);
timeLeft -= Ts;
return(new RobotVector(nx, ny, nz, na, nb, nc) - homePosition);
}
else
{
if (!targetPositionReached)
{
reachedPosition = currentPosition - homePosition;
}
targetPositionReached = true;
polyX.Reset(targetVelocity.X);
polyY.Reset(targetVelocity.Y);
polyZ.Reset(targetVelocity.Z);
polyA.Reset(targetVelocity.A);
polyB.Reset(targetVelocity.B);
polyC.Reset(targetVelocity.C);
return(reachedPosition);
}
}
}
public void SetTargetPosition(RobotVector currentPosition, RobotVector targetPosition, RobotVector targetVelocity, double targetDuration)
{
if (targetDuration <= 0.0)
{
throw new ArgumentException($"Duration value must be greater than 0, get {targetDuration}");
}
bool targetPositionChanged = !targetPosition.Compare(this.targetPosition, 0.1, 1);
bool targetVelocityChanged = !targetVelocity.Compare(this.targetVelocity, 0.1, 1);
bool targetDurationChanged = targetDuration != this.targetDuration;
if (targetDurationChanged || targetPositionChanged || targetVelocityChanged)
{
lock (syncLock) {
targetPositionReached = false;
this.targetPosition = targetPosition;
this.targetVelocity = targetVelocity;
this.targetDuration = targetDuration;
elapsedTime = 0.0;
}
polyX.UpdateCoefficients(currentPosition.X, this.targetPosition.X, this.targetVelocity.X, this.targetDuration);
polyY.UpdateCoefficients(currentPosition.Y, this.targetPosition.Y, this.targetVelocity.Y, this.targetDuration);
polyZ.UpdateCoefficients(currentPosition.Z, this.targetPosition.Z, this.targetVelocity.Z, this.targetDuration);
polyA.UpdateCoefficients(currentPosition.A, this.targetPosition.A, this.targetVelocity.A, this.targetDuration);
polyB.UpdateCoefficients(currentPosition.B, this.targetPosition.B, this.targetVelocity.B, this.targetDuration);
polyC.UpdateCoefficients(currentPosition.C, this.targetPosition.C, this.targetVelocity.C, this.targetDuration);
}
}
public void SetTargetPosition(RobotVector targetPosition, double time)
{
if (time <= 0.0)
{
throw new ArgumentException($"Duration value must be greater than 0, get {time}");
}
if (totalTime2Dest != time || !targetPosition.Compare(this.targetPosition, 0.1, 1))
{
lock (syncLock) {
this.targetPosition = targetPosition.Clone() as RobotVector;
totalTime2Dest = time;
time2Dest = time;
targetPositionReached = false;
}
}
}
public void SetTargetPosition(RobotVector targetPosition, RobotVector targetVelocity, double targetDuration)
{
if (targetDuration <= 0.0)
{
throw new ArgumentException($"Duration value must be greater than 0, get {targetDuration}");
}
bool targetPositionChanged = !targetPosition.Compare(this.targetPosition, 0.1, 0.1);
bool targetVelocityChanged = !targetVelocity.Compare(this.targetVelocity, 0.1, 0.1);
bool targetDurationChanged = targetDuration != this.targetDuration;
if (targetDurationChanged || targetPositionChanged || targetVelocityChanged)
{
lock (syncLock) {
targetPositionReached = false;
this.targetPosition = targetPosition;
this.targetVelocity = targetVelocity;
this.targetDuration = targetDuration;
timeLeft = targetDuration;
}
}
}
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);
}
}
