[InlineData(5.0, 1.0, 1.0, false, 0, 0)] //Quadratic equation with negative determinant public void SolveEquationSolvesLinearEquation(double a, double b, double c, bool expectedSuccess, double expectedX1, double expectedX2) { var success = MathematicTools.SolveEquation(a, b, c, out var x1, out var x2); Assert.Equal(expectedSuccess, success); Assert.Equal(expectedX1, x1, 4); Assert.Equal(expectedX2, x2, 4); }
public void SolveEquationValidatesInput() { Assert.Throws <ArgumentOutOfRangeException>(() => MathematicTools.SolveEquation(0, 0, 0, out _, out _)); }
private ExtendedP2PCalculatorResult Calculate(double targetVelocity) { var result = new ExtendedP2PCalculatorResult { Parameters = MotionParameter, aFrom = InitialAcceleration, vFrom = InitialVelocity, vTo = targetVelocity, Direction = GetDirection(targetVelocity) }; if (result.Direction == RampDirection.Constant) { return(result); } var decMax = MotionParameter.MaximumDecceleration; var jPos = MotionParameter.PositiveJerk; var jNeg = MotionParameter.NegativeJerk; if (result.Direction == RampDirection.Accelerate) { decMax = MotionParameter.MaximumAcceleration; jPos = MotionParameter.NegativeJerk; jNeg = MotionParameter.PositiveJerk; } var a0 = InitialAcceleration; var v0 = InitialVelocity; var t1 = (decMax - a0) / jNeg; var t2 = 0.0; var t3 = -(decMax / jPos); // does profile reach constant a? var v_bya0_Ph1 = t1 * a0; var v_byjD_Ph1 = 0.5 * jNeg * t1 * t1; var v_bya1_Ph3 = t3 * (a0 + jNeg * t1); var v_byjA_Ph3 = 0.5 * jPos * t3 * t3; var vTotal = v0 + v_bya0_Ph1 + v_byjD_Ph1 + v_bya1_Ph3 + v_byjA_Ph3; if (CheckForFlatRampPart(vTotal, targetVelocity, result.Direction)) { // constant a will be reached t1 = (decMax - a0) / jNeg; t3 = Math.Abs(decMax / jPos); var v_Decc = 0.5 * jNeg * (t1 * t1) + a0 * t1; var v_Acc = -0.5 * jPos * (t3 * t3); t2 = (targetVelocity - (v_Decc + v_Acc + v0)) / decMax; } else { // constant a will not be reached // => calculate max reachable a double jerk; double t; if (a0 < 0) { jerk = MotionParameter.PositiveJerk; t = -a0 / jerk; } else if (a0 > 0) { jerk = MotionParameter.NegativeJerk; t = -a0 / jerk; } else { t = 0; jerk = 0; } //3. Bestimmung ob trotz Bremsen, beschleunigt werden muss und umgekehrt! // Geschwindigkeit die erreicht werden würde (v_bya0_to0), falls a0 auf 0 gezogen wird. Dies ist das aussschlagebende Kriterium, ob beschleunigt oder gebremst werden muss // Bsp.: v0 =200, a0= -112 , vTarget = 190 Nur durch den Abbau von a0 auf 0 wird eine Geschwindigkeit von ca. 187 erreicht --> es muss mathematisch beschleunigt werden, um die Zieglgeschwindigkeit zu erreichen var v_bya0_to0 = v0 + t * a0 + 0.5 * jerk * t * t; if (v_bya0_to0 > targetVelocity) { Inverted = result.Direction == RampDirection.Accelerate; } else { Inverted = result.Direction != RampDirection.Accelerate; } if (Inverted) // Beschleuningen falls wir bremsen ---> Bremsen falls beschleunigen { var tmp = jNeg; jNeg = jPos; jPos = tmp; } // 4. Gleichungssystem, um t1,t2 und t3 zu bestimmen var a = 0.5 * jNeg - 0.5 * (jNeg * jNeg / jPos); var b = a0 - a0 * (jNeg / jPos); var c = v0 - targetVelocity - a0 * a0 / (2 * jPos); if (MathematicTools.SolveEquation(a, b, c, out var x1, out var x2)) { CalculateAllTimes(x1, x2, jPos, jNeg, a0, out t1, out t2, out t3); } } var a1 = a0 + jNeg * t1; var v1 = v0 + a0 * t1 + 0.5 * jNeg * t1 * t1; var s1 = v0 * t1 + 0.5 * a0 * t1 * t1 + 1.0 / 6.0 * jNeg * t1 * t1 * t1; var a2 = a1; var v2 = v1 + a1 * t2; var s2 = v1 * t2 + 0.5 * a1 * t2 * t2; var s3 = v2 * t3 + 0.5 * a2 * t3 * t3 + 1.0 / 6.0 * jPos * t3 * t3 * t3; result.Length = s1 + s2 + s3; result.TotalDuration = t1 + t2 + t3; result.Phase1Duration = t1; result.Phase1Length = s1; result.Phase2Duration = t2; result.Phase2Length = s2; result.Phase3Duration = t3; result.Phase3Length = s3; return(result); }
/// <summary> /// Calculates the time [s], at which the profile has reached the given distance within ramp. /// </summary> /// <param name="ramp">Ramp, for which time should be calculated</param> /// <param name="distanceWithinRamp">Distance [mm] from beginning of the ramp. If you work with global distances /// within profiles, you may need to substract the start distance of the ramp before calling this method.</param> /// <returns>Time [s] at which the given distance is reached</returns> public static double GetTimeAt(ExtendedRampCalculationResult ramp, double distanceWithinRamp) { if (distanceWithinRamp > ramp.Length) { if (Math.Abs(distanceWithinRamp - ramp.Length) > 1e-3) { throw new ArgumentOutOfRangeException(nameof(distanceWithinRamp), $"DistanceWithinRamp ({distanceWithinRamp:2}mm) must be smaller than length of ramp ({ramp.Length:2}mm)"); } else { distanceWithinRamp = ramp.Length; } } var s = distanceWithinRamp; var j1 = ramp.Direction == RampDirection.Accelerate ? ramp.Parameters.PositiveJerk : ramp.Parameters.NegativeJerk; if (s <= ramp.Phase1Length) { // point lies within phase 1 // a t³ + c t + d =0 var a = 1.0 / 6 * j1; var c = ramp.vFrom; var d = -s; var cubicResult = Math.Abs(SolveCubicEquation(a, 0, c, d)); return(cubicResult); } else { // calculate state at end of phase 1 var t1 = ramp.Phase1Duration; var a1 = j1 * t1; var v1 = ramp.vFrom + 0.5 * j1 * t1 * t1; var s1 = ramp.vFrom * t1 + 1.0 / 6 * j1 * t1 * t1 * t1; if (s <= ramp.Phase1Length + ramp.Phase2Length) { // point lies within phase 2 // a t² + b t + c = 0 var a = 0.5 * a1; var b = v1; var c = s1 - s; if (!MathematicTools.SolveEquation(a, b, c, out var t_1, out var t_2)) { throw new JointMotionCalculationException($"Failed to solve quadratic equation with a={a:N3}, b={b:N3} and c={c:N3}"); } var quadraticResult = Math.Abs(t_1 < 0 ? t_2 : t_1); return(t1 + quadraticResult); } else { // point lies within phase 3 // calculate state at end of phase 2 var t2 = ramp.Phase2Duration; var a2 = a1; var v2 = v1 + a1 * t2; var s2 = s1 + v1 * t2 + 0.5 * a1 * t2 * t2; // calculate state within phase 3 var j3 = -j1; // a t³ + b t² + c t + d = 0 var a = 1.0 / 6 * j3; var b = 0.5 * a2; var c = v2; var d = s2 - s; var cubicResult = Math.Abs(SolveCubicEquation(a, b, c, d)); if (double.IsNaN(cubicResult)) // there are some case where method 1 fails, then call method 2 { cubicResult = SolveCubic(a, b, c, d)[0].Real; } return(t1 + t2 + cubicResult); } } }