public static void CalculateStatus(ExtendedP2PCalculatorResult ramp, double t, out double j, out double a, out double v, out double s) { var t1 = ramp.Phase1Duration; var t2 = t1 + ramp.Phase2Duration; var t3 = t2 + ramp.Phase3Duration; var jMax = ramp.Direction == RampDirection.Accelerate ? ramp.Parameters.PositiveJerk : ramp.Parameters.NegativeJerk; var v0 = ramp.vFrom; if (t <= t1) { GetStatus1(t, out j, out a, out v, out s); } else { GetStatus1(t1, out _, out var a1, out var v1, out var s1); if (t <= t2) { GetStatus2(t, a1, v1, s1, out j, out a, out v, out s); } else { GetStatus2(t2, a1, v1, s1, out _, out var a2, out var v2, out var s2); if (t <= t3) { GetStatus3(t, a2, v2, s2, out j, out a, out v, out s); } else { GetStatus3(t3, a2, v2, s2, out j, out a, out v, out s); } } } void GetStatus1(double tIn, out double jOut, out double aOut, out double vOut, out double sOut) { var tPhase = tIn; var tPhase2 = tPhase * tPhase; var tPhase3 = tPhase2 * tPhase; jOut = jMax; aOut = jMax * tPhase + ramp.aFrom; vOut = v0 + 0.5 * jMax * tPhase2 + ramp.aFrom * tPhase; sOut = v0 * tPhase + jMax / 6 * tPhase3 + 0.5 * ramp.aFrom * tPhase2; } void GetStatus2(double tIn, double a1, double v1, double s1, out double jOut, out double aOut, out double vOut, out double sOut) { var tPhase = tIn - t1; var tPhase2 = tPhase * tPhase; jOut = 0; aOut = a1; vOut = v1 + a1 * tPhase; sOut = s1 + v1 * tPhase + 0.5 * a1 * tPhase2; } void GetStatus3(double tIn, double a2, double v2, double s2, out double jOut, out double aOut, out double vOut, out double sOut) { var tPhase = tIn - t2; var tPhase2 = tPhase * tPhase; var tPhase3 = tPhase2 * tPhase; jOut = -jMax; aOut = a2 - jMax * tPhase; vOut = v2 + a2 * tPhase + 0.5 * -jMax * tPhase2; sOut = s2 + v2 * tPhase + 0.5 * a2 * tPhase2 + -jMax / 6 * tPhase3; } //double v_current; //if (t < t1) //{ // (_plotView.Model.Series[0] as LineSeries).Points.Add(new DataPoint(tTemp, jD)); //Jerk // (_plotView.Model.Series[1] as LineSeries).Points.Add(new DataPoint(tTemp, a0 + jD * t)); //Acceleration // v_current = v0 + a0 * t + 0.5 * jD * t * t; // (_plotView.Model.Series[2] as LineSeries).Points.Add(new DataPoint(tTemp, v_current)); //Velocity // (_plotView.Model.Series[3] as LineSeries).Points.Add(new DataPoint(tTemp, v0 * t + 0.5 * a0 * t * t + (1.0 / 6.0) * jD * t * t * t)); //Way //} //else if (t < t1 + t2) //{ // double tphase = t - t1; // (_plotView.Model.Series[0] as LineSeries).Points.Add(new DataPoint(tTemp, 0)); // (_plotView.Model.Series[1] as LineSeries).Points.Add(new DataPoint(tTemp, a5)); // v_current = v5 + a5 * tphase; // (_plotView.Model.Series[2] as LineSeries).Points.Add(new DataPoint(tTemp, v_current)); // (_plotView.Model.Series[3] as LineSeries).Points.Add(new DataPoint(tTemp, s5 + v5 * tphase + 0.5 * a5 * tphase * tphase)); //} //else //{ // double tphase = t - t1 - t2; // (_plotView.Model.Series[0] as LineSeries).Points.Add(new DataPoint(tTemp, jA)); // (_plotView.Model.Series[1] as LineSeries).Points.Add(new DataPoint(tTemp, a6 + jA * tphase)); // v_current = v6 + a6 * tphase + 0.5 * jA * tphase * tphase; // (_plotView.Model.Series[2] as LineSeries).Points.Add(new DataPoint(tTemp, v6 + a6 * tphase + 0.5 * jA * tphase * tphase)); // (_plotView.Model.Series[3] as LineSeries).Points.Add(new DataPoint(tTemp, s6 + v6 * tphase + 0.5 * a6 * tphase * tphase + (1.0 / 6.0) * jA * tphase * tphase * tphase)); //} }
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); }