3DPrinting/Prusa-Firmware
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Code Cleanup

Browse Source
This commit is contained in:
FormerLurker 2021-01-18 13:51:08 -06:00 committed by Alex Voinea
parent 623c029bfa
commit 02b0307307
1 changed files with 23 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
Firmware/motion_control.cpp
View File

@ -86,7 +86,7 @@ void mc_arc(float* position, float* target, float* offset, float feed_rate, floa
// calculating here // calculating here
const float millimeters_of_travel_arc = hypot(angular_travel_total * radius, fabs(travel_z)); const float millimeters_of_travel_arc = hypot(angular_travel_total * radius, fabs(travel_z));
if (millimeters_of_travel_arc < 0.001) { return; } if (millimeters_of_travel_arc < 0.001) { return; }
// Calculate the total travel per segment
// Calculate the number of arc segments // Calculate the number of arc segments
uint16_t segments = static_cast<uint16_t>(ceil(millimeters_of_travel_arc / mm_per_arc_segment)); uint16_t segments = static_cast<uint16_t>(ceil(millimeters_of_travel_arc / mm_per_arc_segment));
@ -115,17 +115,18 @@ void mc_arc(float* position, float* target, float* offset, float feed_rate, floa
// If there is only one segment, no need to do a bunch of work since this is a straight line! // If there is only one segment, no need to do a bunch of work since this is a straight line!
if (segments > 1) if (segments > 1)
{ {
// Calculate theta per segments, and linear (z) travel per segment, e travel per segment
// Calculate theta per segments and linear (z) travel per segment // as well as the small angle approximation for sin and cos.
const float theta_per_segment = angular_travel_total / segments, const float theta_per_segment = angular_travel_total / segments,
linear_per_segment = travel_z / (segments), linear_per_segment = travel_z / (segments),
segment_extruder_travel = (target[E_AXIS] - position[E_AXIS]) / (segments), segment_extruder_travel = (target[E_AXIS] - position[E_AXIS]) / (segments),
sq_theta_per_segment = theta_per_segment * theta_per_segment, sq_theta_per_segment = theta_per_segment * theta_per_segment,
sin_T = theta_per_segment - sq_theta_per_segment * theta_per_segment / 6, sin_T = theta_per_segment - sq_theta_per_segment * theta_per_segment / 6,
cos_T = 1 - 0.5f * sq_theta_per_segment; cos_T = 1 - 0.5f * sq_theta_per_segment;
// Loop through all but one of the segments. The last one can be done simply
for (uint16_t i = 1; i < segments; i++) { // Increment (segments-1) // by moving to the target.
if (n_arc_correction--<1) { for (uint16_t i = 1; i < segments; i++) {
if (n_arc_correction-- == 0) {
// Calculate the actual position for r_axis_x and r_axis_y // Calculate the actual position for r_axis_x and r_axis_y
const float cos_Ti = cos(i * theta_per_segment), sin_Ti = sin(i * theta_per_segment); const float cos_Ti = cos(i * theta_per_segment), sin_Ti = sin(i * theta_per_segment);
r_axis_x = -offset[X_AXIS] * cos_Ti + offset[Y_AXIS] * sin_Ti; r_axis_x = -offset[X_AXIS] * cos_Ti + offset[Y_AXIS] * sin_Ti;
@ -134,24 +135,25 @@ void mc_arc(float* position, float* target, float* offset, float feed_rate, floa
n_arc_correction = cs.n_arc_correction; n_arc_correction = cs.n_arc_correction;
} }
else { else {
// Calculate X and Y using the small angle approximation
const float r_axisi = r_axis_x * sin_T + r_axis_y * cos_T; const float r_axisi = r_axis_x * sin_T + r_axis_y * cos_T;
r_axis_x = r_axis_x * cos_T - r_axis_y * sin_T; r_axis_x = r_axis_x * cos_T - r_axis_y * sin_T;
r_axis_y = r_axisi; r_axis_y = r_axisi;
} }
// Update arc_target location // Update Position
position[X_AXIS] = center_axis_x + r_axis_x; position[X_AXIS] = center_axis_x + r_axis_x;
position[Y_AXIS] = center_axis_y + r_axis_y; position[Y_AXIS] = center_axis_y + r_axis_y;
position[Z_AXIS] += linear_per_segment; position[Z_AXIS] += linear_per_segment;
position[E_AXIS] += segment_extruder_travel; position[E_AXIS] += segment_extruder_travel;
// We can't clamp to the target because we are interpolating! We would need to update a position, clamp to it // Clamp to the calculated position.
// after updating from calculated values.
clamp_to_software_endstops(position); clamp_to_software_endstops(position);
plan_buffer_line(position[X_AXIS], position[Y_AXIS], position[Z_AXIS], position[E_AXIS], feed_rate, extruder); // Insert the segment into the buffer
plan_buffer_line(position[X_AXIS], position[Y_AXIS], position[Z_AXIS], position[E_AXIS], feed_rate, extruder, position);
} }
} }
// Ensure last segment arrives at target location. // Clamp to the target position.
// Here we could clamp, but why bother. We would need to update our current position, clamp to it
clamp_to_software_endstops(target); clamp_to_software_endstops(target);
plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], feed_rate, extruder); // Ensure last segment arrives at target location.
plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], feed_rate, extruder, target);
} }