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