|
|
|
|
555
|
delta_mm[Z_AXIS] = (target[Z_AXIS]-position[Z_AXIS])/axis_steps_per_unit[Z_AXIS];
|
555
|
delta_mm[Z_AXIS] = (target[Z_AXIS]-position[Z_AXIS])/axis_steps_per_unit[Z_AXIS];
|
556
|
delta_mm[E_AXIS] = ((target[E_AXIS]-position[E_AXIS])/axis_steps_per_unit[E_AXIS])*extrudemultiply/100.0;
|
556
|
delta_mm[E_AXIS] = ((target[E_AXIS]-position[E_AXIS])/axis_steps_per_unit[E_AXIS])*extrudemultiply/100.0;
|
557
|
if ( block->steps_x == 0 && block->steps_y == 0 && block->steps_z == 0 ) {
|
557
|
if ( block->steps_x == 0 && block->steps_y == 0 && block->steps_z == 0 ) {
|
558
|
- block->millimeters = abs(delta_mm[E_AXIS]);
|
|
|
|
|
558
|
+ block->millimeters = fabs(delta_mm[E_AXIS]);
|
559
|
} else {
|
559
|
} else {
|
560
|
block->millimeters = sqrt(square(delta_mm[X_AXIS]) + square(delta_mm[Y_AXIS]) + square(delta_mm[Z_AXIS]));
|
560
|
block->millimeters = sqrt(square(delta_mm[X_AXIS]) + square(delta_mm[Y_AXIS]) + square(delta_mm[Z_AXIS]));
|
561
|
}
|
561
|
}
|
|
|
|
|
591
|
float speed_factor = 1.0; //factor <=1 do decrease speed
|
591
|
float speed_factor = 1.0; //factor <=1 do decrease speed
|
592
|
for(int i=0; i < 4; i++) {
|
592
|
for(int i=0; i < 4; i++) {
|
593
|
current_speed[i] = delta_mm[i] * inverse_second;
|
593
|
current_speed[i] = delta_mm[i] * inverse_second;
|
594
|
- if(abs(current_speed[i]) > max_feedrate[i])
|
|
|
595
|
- speed_factor = min(speed_factor, max_feedrate[i] / abs(current_speed[i]));
|
|
|
|
|
594
|
+ if(fabs(current_speed[i]) > max_feedrate[i])
|
|
|
595
|
+ speed_factor = min(speed_factor, max_feedrate[i] / fabs(current_speed[i]));
|
596
|
}
|
596
|
}
|
597
|
|
597
|
|
598
|
// Max segement time in us.
|
598
|
// Max segement time in us.
|
|
|
|
|
696
|
#endif
|
696
|
#endif
|
697
|
// Start with a safe speed
|
697
|
// Start with a safe speed
|
698
|
float vmax_junction = max_xy_jerk/2;
|
698
|
float vmax_junction = max_xy_jerk/2;
|
699
|
- if(abs(current_speed[Z_AXIS]) > max_z_jerk/2)
|
|
|
|
|
699
|
+ if(fabs(current_speed[Z_AXIS]) > max_z_jerk/2)
|
700
|
vmax_junction = max_z_jerk/2;
|
700
|
vmax_junction = max_z_jerk/2;
|
701
|
vmax_junction = min(vmax_junction, block->nominal_speed);
|
701
|
vmax_junction = min(vmax_junction, block->nominal_speed);
|
702
|
- if(abs(current_speed[E_AXIS]) > max_e_jerk/2)
|
|
|
|
|
702
|
+ if(fabs(current_speed[E_AXIS]) > max_e_jerk/2)
|
703
|
vmax_junction = min(vmax_junction, max_e_jerk/2);
|
703
|
vmax_junction = min(vmax_junction, max_e_jerk/2);
|
704
|
|
704
|
|
705
|
if ((moves_queued > 1) && (previous_nominal_speed > 0.0001)) {
|
705
|
if ((moves_queued > 1) && (previous_nominal_speed > 0.0001)) {
|
706
|
float jerk = sqrt(pow((current_speed[X_AXIS]-previous_speed[X_AXIS]), 2)+pow((current_speed[Y_AXIS]-previous_speed[Y_AXIS]), 2));
|
706
|
float jerk = sqrt(pow((current_speed[X_AXIS]-previous_speed[X_AXIS]), 2)+pow((current_speed[Y_AXIS]-previous_speed[Y_AXIS]), 2));
|
707
|
- if((abs(previous_speed[X_AXIS]) > 0.0001) || (abs(previous_speed[Y_AXIS]) > 0.0001)) {
|
|
|
|
|
707
|
+ if((fabs(previous_speed[X_AXIS]) > 0.0001) || (fabs(previous_speed[Y_AXIS]) > 0.0001)) {
|
708
|
vmax_junction = block->nominal_speed;
|
708
|
vmax_junction = block->nominal_speed;
|
709
|
}
|
709
|
}
|
710
|
if (jerk > max_xy_jerk) {
|
710
|
if (jerk > max_xy_jerk) {
|
711
|
vmax_junction *= (max_xy_jerk/jerk);
|
711
|
vmax_junction *= (max_xy_jerk/jerk);
|
712
|
}
|
712
|
}
|
713
|
- if(abs(current_speed[Z_AXIS] - previous_speed[Z_AXIS]) > max_z_jerk) {
|
|
|
714
|
- vmax_junction *= (max_z_jerk/abs(current_speed[Z_AXIS] - previous_speed[Z_AXIS]));
|
|
|
|
|
713
|
+ if(fabs(current_speed[Z_AXIS] - previous_speed[Z_AXIS]) > max_z_jerk) {
|
|
|
714
|
+ vmax_junction *= (max_z_jerk/fabs(current_speed[Z_AXIS] - previous_speed[Z_AXIS]));
|
715
|
}
|
715
|
}
|
716
|
- if(abs(current_speed[E_AXIS] - previous_speed[E_AXIS]) > max_e_jerk) {
|
|
|
717
|
- vmax_junction *= (max_e_jerk/abs(current_speed[E_AXIS] - previous_speed[E_AXIS]));
|
|
|
|
|
716
|
+ if(fabs(current_speed[E_AXIS] - previous_speed[E_AXIS]) > max_e_jerk) {
|
|
|
717
|
+ vmax_junction *= (max_e_jerk/fabs(current_speed[E_AXIS] - previous_speed[E_AXIS]));
|
718
|
}
|
718
|
}
|
719
|
}
|
719
|
}
|
720
|
block->max_entry_speed = vmax_junction;
|
720
|
block->max_entry_speed = vmax_junction;
|