public override AxisTarget GetAxisTarget(TimeSpan timeSpan)
{
AxisTarget phaseStartTarget;
if (timeSpan >= phase4_EndHold.TimeSpanInTrajectory)
{
phaseStartTarget = phase4_EndHold;
}
else if (timeSpan >= phase3_Decel.TimeSpanInTrajectory)
{
phaseStartTarget = phase3_Decel;
}
else if (timeSpan >= phase2_Cruise.TimeSpanInTrajectory)
{
phaseStartTarget = phase2_Cruise;
}
else
{
phaseStartTarget = phase1_Accel;
}
TimeSpan phaseTimeSpan = timeSpan - phaseStartTarget.TimeSpanInTrajectory;
AxisTarget adjustedTarget = phaseStartTarget;
TrajectorySettings.AdvanceTargetTimestamp(ref adjustedTarget, phaseTimeSpan);
return(adjustedTarget);
}
public JogTrajectory Setup()
{
TrajectoryEndIsTimeBased = false; // trajectory never ends
double initialVelocity = InitialMeasurements.Velocity;
double velocityDelta = TrajectorySettings.Velocity - initialVelocity;
double acceleration = TrajectorySettings.Acceleration * ((velocityDelta > 0) ? 1.0 : -1.0);
phase1_AccelOrDecel = new AxisTarget()
{
TimeSpanInTrajectory = TimeSpan.Zero,
TimeStamp = InitialMeasurements.TimeStamp,
Position = InitialMeasurements.Position,
Velocity = InitialMeasurements.Velocity,
Acceleration = acceleration
};
double accelDecelPhaseDuration = velocityDelta / acceleration;
double accelDecelPhaseDeltaPosition = initialVelocity * accelDecelPhaseDuration + 0.5 * acceleration * (accelDecelPhaseDuration * accelDecelPhaseDuration);
TimeSpan accelDecelPhaseDurationTimeSpan = TimeSpan.FromSeconds(accelDecelPhaseDuration);
phase2_cruise = new AxisTarget()
{
TimeSpanInTrajectory = accelDecelPhaseDurationTimeSpan,
TimeStamp = InitialMeasurements.TimeStamp + accelDecelPhaseDurationTimeSpan,
Position = InitialMeasurements.Position + accelDecelPhaseDeltaPosition,
Velocity = TrajectorySettings.Velocity,
Acceleration = 0.0
};
FirstPhaseStartTimeStamp = InitialMeasurements.TimeStamp;
TrajectoryRunTimeSpan = accelDecelPhaseDurationTimeSpan;
return(this);
}
public void AdvanceTargetTimestamp(ref AxisTarget target, TimeSpan advanceTimeSpan)
{
double advancePeriodInSeconds = advanceTimeSpan.TotalSeconds;
target.TimeSpanInTrajectory += advanceTimeSpan;
target.TimeStamp += advancePeriodInSeconds;
target.Position += (target.Velocity * advancePeriodInSeconds);
if (target.Acceleration != 0)
{
target.Velocity += target.Acceleration * advancePeriodInSeconds;
target.Position += 0.5 * target.Acceleration * advancePeriodInSeconds * advancePeriodInSeconds;
}
UpdateTargetOutput(ref target);
}
public override AxisTarget GetAxisTarget(TimeSpan timeSpan)
{
AxisTarget phaseStartTarget;
if (timeSpan < phase2_cruise.TimeSpanInTrajectory)
{
phaseStartTarget = phase1_AccelOrDecel;
}
else
{
phaseStartTarget = phase2_cruise;
timeSpan -= phase2_cruise.TimeSpanInTrajectory;
}
AxisTarget target = phaseStartTarget;
TrajectorySettings.AdvanceTargetTimestamp(ref target, timeSpan);
return(target);
}
public PointToPointTrajectory Setup()
{
double totalPositionDelta = FinalMeasurements.Position - InitialMeasurements.Position;
double totalDistance = Math.Abs(totalPositionDelta);
// first determine if this trajectory is a velocity triangle (no cruise period) or a trapazoid (has cruise period).
double accel = TrajectorySettings.Acceleration;
double decel = TrajectorySettings.Deceleration;
double accelTime = TrajectorySettings.Velocity / accel;
double decelTime = TrajectorySettings.Velocity / decel;
double cruiseTime = 0.0;
double cruiseVelocity = TrajectorySettings.Velocity;
double accelDistance = (0.5 * accel * accelTime * accelTime);
double decelDistance = (0.5 * decel * decelTime * decelTime);
double cruiseDistance = (totalDistance - (accelDistance + decelDistance));
if (cruiseDistance < 0.0)
{
// accel and decel distances are already to large - need to recalculate shorter accel and decel times.
double accelOverDecel = accel / decel;
accelTime = Math.Sqrt(totalDistance / (0.5 * accel * (1.0 + accelOverDecel)));
decelTime = accelOverDecel * accelTime;
accelDistance = (0.5 * accel * accelTime * accelTime);
decelDistance = (0.5 * decel * decelTime * decelTime);
cruiseDistance = 0.0;
}
else
{
// have cruise period
cruiseTime = (cruiseDistance / cruiseVelocity);
}
// assign signs to the values
if (totalPositionDelta >= 0)
{
decel = -decel;
}
else
{
accel = -accel;
cruiseVelocity = -cruiseVelocity;
accelDistance = -accelDistance;
cruiseDistance = -cruiseDistance;
decelDistance = -decelDistance;
}
// phases: accel, cruise, decel, stopped.
TimeSpan phase1StartTimeSpan = TimeSpan.Zero;
TimeSpan phase2StartTimeSpan = phase1StartTimeSpan + TimeSpan.FromSeconds(accelTime);
TimeSpan phase3StartTimeSpan = phase2StartTimeSpan + TimeSpan.FromSeconds(cruiseTime);
TimeSpan phase4StartTimeSpan = phase3StartTimeSpan + TimeSpan.FromSeconds(decelTime);
phase1_Accel = new AxisTarget()
{
TimeSpanInTrajectory = phase1StartTimeSpan, TimeStamp = InitialMeasurements.TimeStamp + phase1StartTimeSpan, Position = InitialMeasurements.Position, Velocity = 0.0, Acceleration = accel
};
phase2_Cruise = new AxisTarget()
{
TimeSpanInTrajectory = phase2StartTimeSpan, TimeStamp = InitialMeasurements.TimeStamp + phase2StartTimeSpan, Position = InitialMeasurements.Position + accelDistance, Velocity = cruiseVelocity, Acceleration = 0.0
};
phase3_Decel = new AxisTarget()
{
TimeSpanInTrajectory = phase3StartTimeSpan, TimeStamp = InitialMeasurements.TimeStamp + phase3StartTimeSpan, Position = InitialMeasurements.Position + accelDistance + cruiseDistance, Velocity = cruiseVelocity, Acceleration = decel
};
phase4_EndHold = new AxisTarget()
{
TimeSpanInTrajectory = phase4StartTimeSpan, TimeStamp = InitialMeasurements.TimeStamp + phase4StartTimeSpan, Position = FinalMeasurements.Position, Velocity = 0.0, Acceleration = 0.0
};
FirstPhaseStartTimeStamp = InitialMeasurements.TimeStamp;
TrajectoryRunTimeSpan = phase4_EndHold.TimeSpanInTrajectory;
return(this);
}
public void UpdateTargetOutput(ref AxisTarget target)
{
target.Output += ConstantOutput + KVelocityOutput * target.Velocity + KAccelerationOutput * target.Acceleration;
}
public PointToPointTrajectory Setup()
{
double totalPositionDelta = FinalMeasurements.Position - InitialMeasurements.Position;
double totalDistance = Math.Abs(totalPositionDelta);
// first determine if this trajectory is a velocity triangle (no cruise period) or a trapazoid (has cruise period).
double accel = TrajectorySettings.Acceleration;
double decel = TrajectorySettings.Deceleration;
double accelTime = TrajectorySettings.Velocity / accel;
double decelTime = TrajectorySettings.Velocity / decel;
double cruiseTime = 0.0;
double cruiseVelocity = TrajectorySettings.Velocity;
double accelDistance = (0.5 * accel * accelTime * accelTime);
double decelDistance = (0.5 * decel * decelTime * decelTime);
double cruiseDistance = (totalDistance - (accelDistance + decelDistance));
if (cruiseDistance < 0.0)
{
// accel and decel distances are already to large - need to recalculate shorter accel and decel times.
double accelOverDecel = accel/decel;
accelTime = Math.Sqrt(totalPositionDelta / (0.5 * accel * (1.0 + accelOverDecel)));
decelTime = accelOverDecel * accelTime;
accelDistance = (0.5 * accel * accelTime * accelTime);
decelDistance = (0.5 * decel * decelTime * decelTime);
}
else
{
// have cruise period
cruiseTime = (cruiseDistance / cruiseVelocity);
}
// assign signs to the values
if (totalPositionDelta >= 0)
{
decel = -decel;
}
else
{
accel = -accel;
cruiseVelocity = -cruiseVelocity;
accelDistance = -accelDistance;
cruiseDistance = -cruiseDistance;
decelDistance = -decelDistance;
}
// phases: accel, cruise, decel, stopped.
TimeSpan phase1StartTimeSpan = TimeSpan.Zero;
TimeSpan phase2StartTimeSpan = phase1StartTimeSpan + TimeSpan.FromSeconds(accelTime);
TimeSpan phase3StartTimeSpan = phase2StartTimeSpan + TimeSpan.FromSeconds(cruiseTime);
TimeSpan phase4StartTimeSpan = phase3StartTimeSpan + TimeSpan.FromSeconds(decelTime);
phase1_Accel = new AxisTarget() { TimeSpanInTrajectory = phase1StartTimeSpan, TimeStamp = InitialMeasurements.TimeStamp + phase1StartTimeSpan, Position = InitialMeasurements.Position, Velocity = 0.0, Acceleration = accel };
phase2_Cruise = new AxisTarget() { TimeSpanInTrajectory = phase2StartTimeSpan, TimeStamp = InitialMeasurements.TimeStamp + phase2StartTimeSpan, Position = InitialMeasurements.Position + accelDistance, Velocity = cruiseVelocity, Acceleration = 0.0 };
phase3_Decel = new AxisTarget() { TimeSpanInTrajectory = phase3StartTimeSpan, TimeStamp = InitialMeasurements.TimeStamp + phase3StartTimeSpan, Position = InitialMeasurements.Position + accelDistance + cruiseDistance, Velocity = cruiseVelocity, Acceleration = decel };
phase4_EndHold = new AxisTarget() { TimeSpanInTrajectory = phase4StartTimeSpan, TimeStamp = InitialMeasurements.TimeStamp + phase4StartTimeSpan, Position = InitialMeasurements.Position + accelDistance + cruiseDistance + decelDistance, Velocity = 0.0, Acceleration = 0.0 };
FirstPhaseStartTimeStamp = InitialMeasurements.TimeStamp;
TrajectoryRunTimeSpan = phase4_EndHold.TimeSpanInTrajectory;
return this;
}
public JogTrajectory Setup()
{
TrajectoryEndIsTimeBased = false; // trajectory never ends
double initialVelocity = InitialMeasurements.Velocity;
double velocityDelta = TrajectorySettings.Velocity - initialVelocity;
double acceleration = TrajectorySettings.Acceleration * ((velocityDelta > 0) ? 1.0 : -1.0);
phase1_AccelOrDecel = new AxisTarget()
{
TimeSpanInTrajectory = TimeSpan.Zero,
TimeStamp = InitialMeasurements.TimeStamp,
Position = InitialMeasurements.Position,
Velocity = InitialMeasurements.Velocity,
Acceleration = acceleration
};
double accelDecelPhaseDuration = velocityDelta / acceleration;
double accelDecelPhaseDeltaPosition = initialVelocity * accelDecelPhaseDuration + 0.5 * acceleration * (accelDecelPhaseDuration * accelDecelPhaseDuration);
TimeSpan accelDecelPhaseDurationTimeSpan = TimeSpan.FromSeconds(accelDecelPhaseDuration);
phase2_cruise = new AxisTarget()
{
TimeSpanInTrajectory = accelDecelPhaseDurationTimeSpan,
TimeStamp = InitialMeasurements.TimeStamp + accelDecelPhaseDurationTimeSpan,
Position = InitialMeasurements.Position + accelDecelPhaseDeltaPosition,
Velocity = TrajectorySettings.Velocity,
Acceleration = 0.0
};
FirstPhaseStartTimeStamp = InitialMeasurements.TimeStamp;
TrajectoryRunTimeSpan = accelDecelPhaseDurationTimeSpan;
return this;
}
public void UpdateTargetOutput(ref AxisTarget target)
{
target.Output += ConstantOutput + KVelocityOutput * target.Velocity + KAccelerationOutput * target.Acceleration;
}
public void AdvanceTargetTimestamp(ref AxisTarget target, TimeSpan advanceTimeSpan)
{
double advancePeriodInSeconds = advanceTimeSpan.TotalSeconds;
target.TimeSpanInTrajectory += advanceTimeSpan;
target.TimeStamp += advancePeriodInSeconds;
target.Position += (target.Velocity * advancePeriodInSeconds);
if (target.Acceleration != 0)
{
target.Velocity += target.Acceleration * advancePeriodInSeconds;
target.Position += 0.5 * target.Acceleration * advancePeriodInSeconds * advancePeriodInSeconds;
}
UpdateTargetOutput(ref target);
}
