internal static AccelerationBuilder FromTo(double initialSpeed, double endSpeed, int stepCount, double exactDuration)
{
var absoluteInitialSpeed = Math.Abs(initialSpeed);
var absoluteEndSpeed = Math.Abs(endSpeed);
var tickCount = (int)(exactDuration * Constants.TimerFrequency);
var acceleration = Math.Abs(absoluteEndSpeed - absoluteInitialSpeed) / exactDuration;
var rawC0 = Constants.TimerFrequency * Math.Sqrt(2 / acceleration);
var isDeceleration = absoluteInitialSpeed > absoluteEndSpeed;
var targetDelta = (int)Math.Abs(Math.Round(Constants.TimerFrequency / endSpeed));
if (targetDelta < 0)
{
//overflow when decelerating to stand still
targetDelta = int.MaxValue;
}
if (isDeceleration)
{
rawC0 = -rawC0;
}
var plan = new AccelerationBuilder(rawC0, targetDelta, stepCount, tickCount);
return(plan);
}
private void addRampPlan(double initialSpeed, double endSpeed, double exactDuration, double distance, List <StepInstrution> pathPlans)
{
checked
{
var profile = AccelerationBuilder.FromTo(initialSpeed, endSpeed, (int)Math.Round(distance), exactDuration);
var timeDiff = Math.Abs(profile.TotalTickCount - exactDuration * Configuration.TimerFrequency);
System.Diagnostics.Debug.WriteLine("Acceleration time diff: " + timeDiff);
System.Diagnostics.Debug.WriteLine("\t" + profile);
pathPlans.Add(profile.ToInstruction());
}
}
private StepInstrution getAccelerationInstruction(double currentSpeed, double desiredSpeed, double accelerationTime)
{
var speedDiff = desiredSpeed - currentSpeed;
var acceleration = speedDiff / accelerationTime;
var trajectory = currentSpeed * accelerationTime + 0.5 * acceleration * accelerationTime * accelerationTime;
var trajectorySteps = (int)Math.Round(trajectory / Configuration.MilimetersPerStep);
var currentSpeedSteps = currentSpeed / Configuration.MilimetersPerStep;
var desiredSpeedSteps = desiredSpeed / Configuration.MilimetersPerStep;
var builder = AccelerationBuilder.FromTo(Math.Abs(currentSpeedSteps), Math.Abs(desiredSpeedSteps), trajectorySteps, accelerationTime);
var instruction = builder.ToInstruction();
return(instruction);
}
/// <summary>
/// Adds line with specified initial and desired speed.
/// </summary>
/// <param name="xSteps">Number of steps along x.</param>
/// <param name="ySteps">Numer of steps along y.</param>
/// <param name="planeAcceleration">Acceleration used for getting desired speed out of initial.</param>
public Speed AddLineXY(int xSteps, int ySteps, Speed initialSpeed, Acceleration planeAcceleration, Speed desiredEndSpeed)
{
if (xSteps == 0 && ySteps == 0)
{
//nothing to do
return(initialSpeed);
}
Speed speedLimitX, speedLimitY;
DecomposeXY(xSteps, ySteps, desiredEndSpeed, out speedLimitX, out speedLimitY);
Speed initialSpeedX, initialSpeedY;
DecomposeXY(xSteps, ySteps, initialSpeed, out initialSpeedX, out initialSpeedY);
Acceleration accelerationX, accelerationY;
DecomposeXY(xSteps, ySteps, planeAcceleration, out accelerationX, out accelerationY);
Speed reachedX, reachedY;
int accelerationStepsX, accelerationStepsY;
var timeX = AccelerationBuilder.CalculateTime(initialSpeedX, speedLimitX, accelerationX, xSteps, out reachedX, out accelerationStepsX);
var timeY = AccelerationBuilder.CalculateTime(initialSpeedY, speedLimitY, accelerationY, ySteps, out reachedY, out accelerationStepsY);
//take acceleration time according to axis with more precision
var accelerationTime = Math.Max(timeX, timeY);
var accelerationProfileX = AccelerationBuilder.FromTo(initialSpeedX, reachedX, accelerationStepsX, accelerationTime);
var accelerationProfileY = AccelerationBuilder.FromTo(initialSpeedY, reachedY, accelerationStepsY, accelerationTime);
var reachedSpeedX = Speed.FromDeltaT(accelerationProfileX.EndDelta + accelerationProfileX.BaseDeltaT);
var reachedSpeedY = Speed.FromDeltaT(accelerationProfileY.EndDelta + accelerationProfileY.BaseDeltaT);
var reachedSpeed = ComposeSpeeds(reachedSpeedX, reachedSpeedY);
var remainingX = xSteps - accelerationProfileX.StepCount;
var remainingY = ySteps - accelerationProfileY.StepCount;
if (accelerationProfileX.StepCount == 0 && accelerationProfileY.StepCount == 0)
{
reachedSpeed = initialSpeed;
}
//send profile
AddAccelerationXY(accelerationProfileX, accelerationProfileY);
AddConstantSpeedTransitionXY(remainingX, remainingY, reachedSpeed);
return(reachedSpeed);
}
/// <summary>
/// Adds ramped line with specified number of steps.
/// </summary>
/// <param name="xSteps">Number of steps along x.</param>
/// <param name="ySteps">Numer of steps along y.</param>
/// <param name="planeAcceleration">Acceleration used for ramping - calculated for both axis combined.</param>
/// <param name="planeSpeedLimit">Maximal speed that could be achieved for both axis combined.</param>
public void AddRampedLineXY(int xSteps, int ySteps, Acceleration planeAcceleration, Speed planeSpeedLimit)
{
if (xSteps == 0 && ySteps == 0)
{
//nothing to do
return;
}
Speed speedLimitX, speedLimitY;
DecomposeXY(xSteps, ySteps, planeSpeedLimit, out speedLimitX, out speedLimitY);
Acceleration accelerationX, accelerationY;
DecomposeXY(xSteps, ySteps, planeAcceleration, out accelerationX, out accelerationY);
Speed reachedX, reachedY;
int accelerationStepsX, accelerationStepsY;
var timeX = AccelerationBuilder.CalculateTime(Speed.Zero, speedLimitX, accelerationX, xSteps / 2, out reachedX, out accelerationStepsX);
var timeY = AccelerationBuilder.CalculateTime(Speed.Zero, speedLimitY, accelerationY, ySteps / 2, out reachedY, out accelerationStepsY);
//take acceleration time according to axis with more precision
var accelerationTime = Math.Max(timeX, timeY);
var accelerationProfileX = AccelerationBuilder.FromTo(Speed.Zero, reachedX, accelerationStepsX, accelerationTime);
var accelerationProfileY = AccelerationBuilder.FromTo(Speed.Zero, reachedY, accelerationStepsY, accelerationTime);
var reachedSpeedX = Speed.FromDeltaT(accelerationProfileX.EndDelta + accelerationProfileX.BaseDeltaT);
var reachedSpeedY = Speed.FromDeltaT(accelerationProfileY.EndDelta + accelerationProfileY.BaseDeltaT);
var reachedSpeed = ComposeSpeeds(reachedSpeedX, reachedSpeedY);
var decelerationProfileX = AccelerationBuilder.FromTo(reachedX, Speed.Zero, accelerationStepsX, accelerationTime);
var decelerationProfileY = AccelerationBuilder.FromTo(reachedY, Speed.Zero, accelerationStepsY, accelerationTime);
var remainingX = xSteps - accelerationProfileX.StepCount - decelerationProfileX.StepCount;
var remainingY = ySteps - accelerationProfileY.StepCount - decelerationProfileY.StepCount;
//send ramp
AddAccelerationXY(accelerationProfileX, accelerationProfileY);
AddConstantSpeedTransitionXY(remainingX, remainingY, reachedSpeed);
AddAccelerationXY(decelerationProfileX, decelerationProfileY);
}
/// <summary>
/// Adds acceleration for x and y axes.
/// </summary>
/// <param name="accelerationProfileX">Profile for x axis acceleration.</param>
/// <param name="accelerationProfileY">Profile for y axis acceleration.</param>
public void AddAccelerationXY(AccelerationBuilder accelerationProfileX, AccelerationBuilder accelerationProfileY)
{
AddXY(accelerationProfileX.ToInstruction(), accelerationProfileY.ToInstruction());
}
/// <summary>
/// Adds ramped line with specified number of steps.
/// </summary>
/// <param name="uSteps">Number of steps along u.</param>
/// <param name="vSteps">Numer of steps along v.</param>
/// <param name="xSteps">Number of steps along x.</param>
/// <param name="ySteps">Numer of steps along y.</param>
/// <param name="planeAcceleration">Acceleration used for ramping - calculated for both axis combined.</param>
/// <param name="planeSpeedLimit">Maximal speed that could be achieved for both axis combined.</param>
public void AddRampedLineUVXY(int uSteps, int vSteps, int xSteps, int ySteps, Acceleration planeAcceleration, Speed planeSpeedLimit)
{
if (uSteps == 0 && vSteps == 0 && xSteps == 0 && ySteps == 0)
{
//nothing to do
return;
}
Speed speedLimitU, speedLimitV;
DecomposeXY(uSteps, vSteps, planeSpeedLimit, out speedLimitU, out speedLimitV);
Acceleration accelerationU, accelerationV;
DecomposeXY(uSteps, vSteps, planeAcceleration, out accelerationU, out accelerationV);
Speed speedLimitX, speedLimitY;
DecomposeXY(xSteps, ySteps, planeSpeedLimit, out speedLimitX, out speedLimitY);
Acceleration accelerationX, accelerationY;
DecomposeXY(xSteps, ySteps, planeAcceleration, out accelerationX, out accelerationY);
Speed reachedU, reachedV, reachedX, reachedY;
int accelerationStepsU, accelerationStepsV, accelerationStepsX, accelerationStepsY;
var timeU = AccelerationBuilder.CalculateTime(Speed.Zero, speedLimitU, accelerationU, uSteps / 2, out reachedU, out accelerationStepsU);
var timeV = AccelerationBuilder.CalculateTime(Speed.Zero, speedLimitV, accelerationV, vSteps / 2, out reachedV, out accelerationStepsV);
var timeX = AccelerationBuilder.CalculateTime(Speed.Zero, speedLimitX, accelerationX, xSteps / 2, out reachedX, out accelerationStepsX);
var timeY = AccelerationBuilder.CalculateTime(Speed.Zero, speedLimitY, accelerationY, ySteps / 2, out reachedY, out accelerationStepsY);
//take acceleration time according to axis with more precision
var accelerationTime = Math.Max(Math.Max(timeU, timeV), Math.Max(timeX, timeY));
var accelerationProfileU = AccelerationBuilder.FromTo(Speed.Zero, reachedU, accelerationStepsU, accelerationTime);
var accelerationProfileV = AccelerationBuilder.FromTo(Speed.Zero, reachedV, accelerationStepsV, accelerationTime);
var accelerationProfileX = AccelerationBuilder.FromTo(Speed.Zero, reachedX, accelerationStepsX, accelerationTime);
var accelerationProfileY = AccelerationBuilder.FromTo(Speed.Zero, reachedY, accelerationStepsY, accelerationTime);
var reachedSpeedU = Speed.FromDeltaT(accelerationProfileU.EndDelta + accelerationProfileU.BaseDeltaT);
var reachedSpeedV = Speed.FromDeltaT(accelerationProfileV.EndDelta + accelerationProfileV.BaseDeltaT);
var reachedSpeedX = Speed.FromDeltaT(accelerationProfileX.EndDelta + accelerationProfileX.BaseDeltaT);
var reachedSpeedY = Speed.FromDeltaT(accelerationProfileY.EndDelta + accelerationProfileY.BaseDeltaT);
var reachedSpeedUV = ComposeSpeeds(reachedSpeedU, reachedSpeedV);
var reachedSpeedXY = ComposeSpeeds(reachedSpeedX, reachedSpeedY);
var decelerationProfileU = AccelerationBuilder.FromTo(reachedU, Speed.Zero, accelerationStepsU, accelerationTime);
var decelerationProfileV = AccelerationBuilder.FromTo(reachedV, Speed.Zero, accelerationStepsV, accelerationTime);
var decelerationProfileX = AccelerationBuilder.FromTo(reachedX, Speed.Zero, accelerationStepsX, accelerationTime);
var decelerationProfileY = AccelerationBuilder.FromTo(reachedY, Speed.Zero, accelerationStepsY, accelerationTime);
var remainingU = uSteps - accelerationProfileU.StepCount - decelerationProfileU.StepCount;
var remainingV = vSteps - accelerationProfileV.StepCount - decelerationProfileV.StepCount;
var remainingX = xSteps - accelerationProfileX.StepCount - decelerationProfileX.StepCount;
var remainingY = ySteps - accelerationProfileY.StepCount - decelerationProfileY.StepCount;
//send ramp
AddAccelerationUVXY(accelerationProfileU, accelerationProfileV, accelerationProfileX, accelerationProfileY);
AddConstantSpeedTransitionUVXY(remainingU, remainingV, reachedSpeedUV, remainingX, remainingY, reachedSpeedXY);
AddAccelerationUVXY(decelerationProfileU, decelerationProfileV, decelerationProfileX, decelerationProfileY);
}
/// <summary>
/// Adds acceleration for u, v, x and y axes.
/// </summary>
public void AddAccelerationUVXY(AccelerationBuilder accelerationProfileU, AccelerationBuilder accelerationProfileV, AccelerationBuilder accelerationProfileX, AccelerationBuilder accelerationProfileY)
{
AddUVXY(accelerationProfileU.ToInstruction(), accelerationProfileV.ToInstruction(), accelerationProfileX.ToInstruction(), accelerationProfileY.ToInstruction());
}
