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