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