public VelocityGenerator(double max_vel, double max_acc, double max_jerk, ControlPointDirection direction, double dt)
{
this.max_vel = max_vel;
this.max_acc = max_acc;
this.max_jerk = max_jerk;
this.direction = direction;
this.dt = dt;
s_curve[0] = new S_Curve(0.0, 0.0, max_jerk, 0.0, 0.0, 0.0); // curve1
s_curve[1] = new S_Curve(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); // curve2
s_curve[2] = new S_Curve(0.0, 0.0, -max_jerk, 0.0, 0.0, 0.0); // curve3
s_curve[3] = new S_Curve(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); // curve4
s_curve[4] = new S_Curve(0.0, 0.0, -max_jerk, 0.0, 0.0, 0.0); // curve5
s_curve[5] = new S_Curve(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); // curve6
s_curve[6] = new S_Curve(0.0, 0.0, max_jerk, 0.0, 0.0, 0.0); // curve7
}
private double s_curve_part_jerk(S_Curve part, double time)
{
return(part.j);
}
private double s_curve_part_acc(S_Curve part, double time)
{
return(part.a + part.j * time);
}
private double s_curve_part_vel(S_Curve part, double time)
{
return(part.v + part.a * time +
1.0 / 2.0 * part.j * Math.Pow(time, 2));
}
private double s_curve_part_pos(S_Curve part, double time)
{
return(part.p + part.v * time +
1.0 / 2.0 * part.a * Math.Pow(time, 2) +
1.0 / 6.0 * part.j * Math.Pow(time, 3));
}