bissen21/Marlin_Firmware
1
0
Fork 0
Code Issues Pull Requests Projects Releases 4 Wiki Activity

Apply maths macros and type changes ahead of HAL

Browse Source
This commit is contained in:
Scott Lahteine
2017-06-19 22:39:23 -05:00
parent 0c616700f3
commit 6c45d0fd81
26 changed files with 181 additions and 166 deletions
Download Patch File Download Diff File Expand all files Collapse all files

64
Marlin/planner.cpp
View File

@ -178,23 +178,23 @@ void Planner::init() {
* by the provided factors.
*/
void Planner::calculate_trapezoid_for_block(block_t* const block, const float &entry_factor, const float &exit_factor) {
uint32_t initial_rate = ceil(block->nominal_rate * entry_factor),
final_rate = ceil(block->nominal_rate * exit_factor); // (steps per second)
uint32_t initial_rate = CEIL(block->nominal_rate * entry_factor),
final_rate = CEIL(block->nominal_rate * exit_factor); // (steps per second)
// Limit minimal step rate (Otherwise the timer will overflow.)
NOLESS(initial_rate, MINIMAL_STEP_RATE);
NOLESS(final_rate, MINIMAL_STEP_RATE);
int32_t accel = block->acceleration_steps_per_s2,
accelerate_steps = ceil(estimate_acceleration_distance(initial_rate, block->nominal_rate, accel)),
decelerate_steps = floor(estimate_acceleration_distance(block->nominal_rate, final_rate, -accel)),
accelerate_steps = CEIL(estimate_acceleration_distance(initial_rate, block->nominal_rate, accel)),
decelerate_steps = FLOOR(estimate_acceleration_distance(block->nominal_rate, final_rate, -accel)),
plateau_steps = block->step_event_count - accelerate_steps - decelerate_steps;
// Is the Plateau of Nominal Rate smaller than nothing? That means no cruising, and we will
// have to use intersection_distance() to calculate when to abort accel and start braking
// in order to reach the final_rate exactly at the end of this block.
if (plateau_steps < 0) {
accelerate_steps = ceil(intersection_distance(initial_rate, final_rate, accel, block->step_event_count));
accelerate_steps = CEIL(intersection_distance(initial_rate, final_rate, accel, block->step_event_count));
NOLESS(accelerate_steps, 0); // Check limits due to numerical round-off
accelerate_steps = min((uint32_t)accelerate_steps, block->step_event_count);//(We can cast here to unsigned, because the above line ensures that we are above zero)
plateau_steps = 0;
@ -221,8 +221,8 @@ void Planner::calculate_trapezoid_for_block(block_t* const block, const float &e
// This method will calculate the junction jerk as the euclidean distance between the nominal
// velocities of the respective blocks.
//inline float junction_jerk(block_t *before, block_t *after) {
// return sqrt(
// pow((before->speed_x-after->speed_x), 2)+pow((before->speed_y-after->speed_y), 2));
// return SQRT(
// POW((before->speed_x-after->speed_x), 2)+POW((before->speed_y-after->speed_y), 2));
//}
@ -693,22 +693,22 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const
// Calculate target position in absolute steps
//this should be done after the wait, because otherwise a M92 code within the gcode disrupts this calculation somehow
const long target[XYZE] = {
lround(a * axis_steps_per_mm[X_AXIS]),
lround(b * axis_steps_per_mm[Y_AXIS]),
lround(c * axis_steps_per_mm[Z_AXIS]),
lround(e * axis_steps_per_mm[E_AXIS_N])
LROUND(a * axis_steps_per_mm[X_AXIS]),
LROUND(b * axis_steps_per_mm[Y_AXIS]),
LROUND(c * axis_steps_per_mm[Z_AXIS]),
LROUND(e * axis_steps_per_mm[E_AXIS_N])
};
// When changing extruders recalculate steps corresponding to the E position
#if ENABLED(DISTINCT_E_FACTORS)
if (last_extruder != extruder && axis_steps_per_mm[E_AXIS_N] != axis_steps_per_mm[E_AXIS + last_extruder]) {
position[E_AXIS] = lround(position[E_AXIS] * axis_steps_per_mm[E_AXIS_N] * steps_to_mm[E_AXIS + last_extruder]);
position[E_AXIS] = LROUND(position[E_AXIS] * axis_steps_per_mm[E_AXIS_N] * steps_to_mm[E_AXIS + last_extruder]);
last_extruder = extruder;
}
#endif
#if ENABLED(LIN_ADVANCE)
const float mm_D_float = sqrt(sq(a - position_float[X_AXIS]) + sq(b - position_float[Y_AXIS]));
const float mm_D_float = SQRT(sq(a - position_float[X_AXIS]) + sq(b - position_float[Y_AXIS]));
#endif
const long da = target[X_AXIS] - position[X_AXIS],
@ -1036,10 +1036,10 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const
delta_mm[E_AXIS] = esteps_float * steps_to_mm[E_AXIS_N];
if (block->steps[X_AXIS] < MIN_STEPS_PER_SEGMENT && block->steps[Y_AXIS] < MIN_STEPS_PER_SEGMENT && block->steps[Z_AXIS] < MIN_STEPS_PER_SEGMENT) {
block->millimeters = fabs(delta_mm[E_AXIS]);
block->millimeters = FABS(delta_mm[E_AXIS]);
}
else {
block->millimeters = sqrt(
block->millimeters = SQRT(
#if CORE_IS_XY
sq(delta_mm[X_HEAD]) + sq(delta_mm[Y_HEAD]) + sq(delta_mm[Z_AXIS])
#elif CORE_IS_XZ
@ -1061,15 +1061,15 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const
// Slow down when the buffer starts to empty, rather than wait at the corner for a buffer refill
#if ENABLED(SLOWDOWN) || ENABLED(ULTRA_LCD) || defined(XY_FREQUENCY_LIMIT)
// Segment time im micro seconds
unsigned long segment_time = lround(1000000.0 / inverse_mm_s);
unsigned long segment_time = LROUND(1000000.0 / inverse_mm_s);
#endif
#if ENABLED(SLOWDOWN)
if (WITHIN(moves_queued, 2, (BLOCK_BUFFER_SIZE) / 2 - 1)) {
if (segment_time < min_segment_time) {
// buffer is draining, add extra time. The amount of time added increases if the buffer is still emptied more.
inverse_mm_s = 1000000.0 / (segment_time + lround(2 * (min_segment_time - segment_time) / moves_queued));
inverse_mm_s = 1000000.0 / (segment_time + LROUND(2 * (min_segment_time - segment_time) / moves_queued));
#if defined(XY_FREQUENCY_LIMIT) || ENABLED(ULTRA_LCD)
segment_time = lround(1000000.0 / inverse_mm_s);
segment_time = LROUND(1000000.0 / inverse_mm_s);
#endif
}
}
@ -1082,7 +1082,7 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const
#endif
block->nominal_speed = block->millimeters * inverse_mm_s; // (mm/sec) Always > 0
block->nominal_rate = ceil(block->step_event_count * inverse_mm_s); // (step/sec) Always > 0
block->nominal_rate = CEIL(block->step_event_count * inverse_mm_s); // (step/sec) Always > 0
#if ENABLED(FILAMENT_WIDTH_SENSOR)
static float filwidth_e_count = 0, filwidth_delay_dist = 0;
@ -1121,7 +1121,7 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const
// Calculate and limit speed in mm/sec for each axis
float current_speed[NUM_AXIS], speed_factor = 1.0; // factor <1 decreases speed
LOOP_XYZE(i) {
const float cs = fabs(current_speed[i] = delta_mm[i] * inverse_mm_s);
const float cs = FABS(current_speed[i] = delta_mm[i] * inverse_mm_s);
#if ENABLED(DISTINCT_E_FACTORS)
if (i == E_AXIS) i += extruder;
#endif
@ -1134,7 +1134,7 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const
// Check and limit the xy direction change frequency
const unsigned char direction_change = block->direction_bits ^ old_direction_bits;
old_direction_bits = block->direction_bits;
segment_time = lround((float)segment_time / speed_factor);
segment_time = LROUND((float)segment_time / speed_factor);
long xs0 = axis_segment_time[X_AXIS][0],
xs1 = axis_segment_time[X_AXIS][1],
@ -1178,7 +1178,7 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const
uint32_t accel;
if (!block->steps[X_AXIS] && !block->steps[Y_AXIS] && !block->steps[Z_AXIS]) {
// convert to: acceleration steps/sec^2
accel = ceil(retract_acceleration * steps_per_mm);
accel = CEIL(retract_acceleration * steps_per_mm);
}
else {
#define LIMIT_ACCEL_LONG(AXIS,INDX) do{ \
@ -1196,7 +1196,7 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const
}while(0)
// Start with print or travel acceleration
accel = ceil((esteps ? acceleration : travel_acceleration) * steps_per_mm);
accel = CEIL((esteps ? acceleration : travel_acceleration) * steps_per_mm);
#if ENABLED(DISTINCT_E_FACTORS)
#define ACCEL_IDX extruder
@ -1267,8 +1267,8 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const
// Skip and avoid divide by zero for straight junctions at 180 degrees. Limit to min() of nominal speeds.
if (cos_theta > -0.95) {
// Compute maximum junction velocity based on maximum acceleration and junction deviation
float sin_theta_d2 = sqrt(0.5 * (1.0 - cos_theta)); // Trig half angle identity. Always positive.
NOMORE(vmax_junction, sqrt(block->acceleration * junction_deviation * sin_theta_d2 / (1.0 - sin_theta_d2)));
float sin_theta_d2 = SQRT(0.5 * (1.0 - cos_theta)); // Trig half angle identity. Always positive.
NOMORE(vmax_junction, SQRT(block->acceleration * junction_deviation * sin_theta_d2 / (1.0 - sin_theta_d2)));
}
}
}
@ -1286,7 +1286,7 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const
float safe_speed = block->nominal_speed;
uint8_t limited = 0;
LOOP_XYZE(i) {
const float jerk = fabs(current_speed[i]), maxj = max_jerk[i];
const float jerk = FABS(current_speed[i]), maxj = max_jerk[i];
if (jerk > maxj) {
if (limited) {
const float mjerk = maxj * block->nominal_speed;
@ -1395,7 +1395,7 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const
&& (uint32_t)esteps != block->step_event_count
&& de_float > 0.0;
if (block->use_advance_lead)
block->abs_adv_steps_multiplier8 = lround(
block->abs_adv_steps_multiplier8 = LROUND(
extruder_advance_k
* (UNEAR_ZERO(advance_ed_ratio) ? de_float / mm_D_float : advance_ed_ratio) // Use the fixed ratio, if set
* (block->nominal_speed / (float)block->nominal_rate)
@ -1458,10 +1458,10 @@ void Planner::_set_position_mm(const float &a, const float &b, const float &c, c
#else
#define _EINDEX E_AXIS
#endif
long na = position[X_AXIS] = lround(a * axis_steps_per_mm[X_AXIS]),
nb = position[Y_AXIS] = lround(b * axis_steps_per_mm[Y_AXIS]),
nc = position[Z_AXIS] = lround(c * axis_steps_per_mm[Z_AXIS]),
ne = position[E_AXIS] = lround(e * axis_steps_per_mm[_EINDEX]);
long na = position[X_AXIS] = LROUND(a * axis_steps_per_mm[X_AXIS]),
nb = position[Y_AXIS] = LROUND(b * axis_steps_per_mm[Y_AXIS]),
nc = position[Z_AXIS] = LROUND(c * axis_steps_per_mm[Z_AXIS]),
ne = position[E_AXIS] = LROUND(e * axis_steps_per_mm[_EINDEX]);
#if ENABLED(LIN_ADVANCE)
position_float[X_AXIS] = a;
position_float[Y_AXIS] = b;
@ -1514,7 +1514,7 @@ void Planner::set_position_mm(const AxisEnum axis, const float &v) {
#else
const uint8_t axis_index = axis;
#endif
position[axis] = lround(v * axis_steps_per_mm[axis_index]);
position[axis] = LROUND(v * axis_steps_per_mm[axis_index]);
#if ENABLED(LIN_ADVANCE)
position_float[axis] = v;
#endif