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