void Planner::plan_move(const std::vector<int>& steps, float length, float speed, float acceleration, float entry_speed) { PlanBlock *block = &block_buffer[block_buffer_head]; block->move.steps = steps; block->move.length = length; block->move.speed = speed; block->move.acceleration = acceleration; block->entry_speed_sqr = 0; block->nominal_speed_sqr = speed*speed; block->max_change_speed_sqr = 2*length*acceleration; if (block_buffer_head == block_buffer_tail) { block->max_entry_speed_sqr = 0; } else { // Not first block, compute entry speed float prev_nominal_speed_sqr = block_buffer[prev_block_index(block_buffer_head)].nominal_speed_sqr; block->max_entry_speed_sqr = std::min(std::min(entry_speed*entry_speed, block->nominal_speed_sqr), prev_nominal_speed_sqr); } block_buffer_head = next_buffer_head; next_buffer_head = next_block_index(block_buffer_head); // Finish up by recalculating the plan with the new block. recalculate(); }
void getHighESpeed() { static float oldt = 0; if (!autotemp_enabled) return; if (degTargetHotend0() + 2 < autotemp_min) return; // probably temperature set to zero. float high = 0.0; uint8_t block_index = block_buffer_tail; while (block_index != block_buffer_head) { block_t *block = &block_buffer[block_index]; if (block->steps[X_AXIS] || block->steps[Y_AXIS] || block->steps[Z_AXIS]) { float se = (float)block->steps[E_AXIS] / block->step_event_count * block->nominal_speed; // mm/sec; if (se > high) high = se; } block_index = next_block_index(block_index); } float t = autotemp_min + high * autotemp_factor; t = constrain(t, autotemp_min, autotemp_max); if (oldt > t) { t *= (1 - AUTOTEMP_OLDWEIGHT); t += AUTOTEMP_OLDWEIGHT * oldt; } oldt = t; setTargetHotend0(t); }
void plan_init() { block_buffer_tail = block_buffer_head; next_buffer_head = next_block_index(block_buffer_head); // block_buffer_planned = block_buffer_head; memset(&pl, 0, sizeof(pl)); // Clear planner struct }
// Recalculates the trapezoid speed profiles for all blocks in the plan according to the // entry_factor for each junction. Must be called by planner_recalculate() after // updating the blocks. void planner_recalculate_trapezoids() { int8_t block_index = block_buffer_tail; block_t *current; block_t *next = NULL; while(block_index != block_buffer_head) { current = next; next = &block_buffer[block_index]; if (current) { // Recalculate if current block entry or exit junction speed has changed. if (current->recalculate_flag || next->recalculate_flag) { // NOTE: Entry and exit factors always > 0 by all previous logic operations. calculate_trapezoid_for_block(current, current->entry_speed/current->nominal_speed, next->entry_speed/current->nominal_speed); current->recalculate_flag = false; // Reset current only to ensure next trapezoid is computed } } block_index = next_block_index( block_index ); } // Last/newest block in buffer. Exit speed is set with MINIMUM_PLANNER_SPEED. Always recalculated. if(next != NULL) { calculate_trapezoid_for_block(next, next->entry_speed/next->nominal_speed, MINIMUM_PLANNER_SPEED/next->nominal_speed); next->recalculate_flag = false; } }
float Planner::get_current_exit_speed_sqr() const { std::size_t block_index = next_block_index(block_buffer_tail); if (block_index == block_buffer_head) { // No next block return 0.0; } return block_buffer[block_index].entry_speed_sqr; }
int planner_blocks_available(void) { int next_buffer_head = next_block_index( block_buffer_head ); if (next_buffer_head == block_buffer_tail) return 0; else if (next_buffer_head >= block_buffer_tail) return BLOCK_BUFFER_SIZE - (next_buffer_head - block_buffer_tail); else return block_buffer_tail - next_buffer_head; }
block_t *planner_get_current_block() { if (block_buffer_head == block_buffer_tail) { return(NULL); } block_buffer_tail_write = next_block_index(block_buffer_tail); return(&block_buffer[block_buffer_tail]); }
uint8_t plan_queue_full (void) { int next_buffer_head = next_block_index( block_buffer_head ); if (block_buffer_tail == next_buffer_head) return 1; else return 0; }
void Planner::next_move() { if (block_buffer_head != block_buffer_tail) { // Discard non-empty buffer. std::size_t block_index = next_block_index( block_buffer_tail ); // Push block_buffer_planned pointer, if encountered. if (block_buffer_tail == block_buffer_planned) { block_buffer_planned = block_index; } block_buffer_tail = block_index; } }
/** * recalculate() needs to go over the current plan twice. * Once in reverse and once forward. This implements the forward pass. */ void Planner::forward_pass() { block_t* block[3] = { NULL, NULL, NULL }; for (uint8_t b = block_buffer_tail; b != block_buffer_head; b = next_block_index(b)) { block[0] = block[1]; block[1] = block[2]; block[2] = &block_buffer[b]; forward_pass_kernel(block[0], block[1], block[2]); } forward_pass_kernel(block[1], block[2], NULL); }
void planner_discard_current_block() { if (block_buffer_head != block_buffer_tail) { if (block_buffer[block_buffer_tail].block_type == BLOCK_TYPE_RASTER_LINE) { raster_buffer_count--; } block_buffer_tail = next_block_index( block_buffer_tail ); block_buffer_tail_write = block_buffer_tail; } }
// planner_recalculate() needs to go over the current plan twice. Once in reverse and once forward. This // implements the forward pass. void planner_forward_pass() { uint8_t block_index = block_buffer_tail; block_t *block[3] = { NULL, NULL, NULL }; while (block_index != block_buffer_head) { block[0] = block[1]; block[1] = block[2]; block[2] = &block_buffer[block_index]; planner_forward_pass_kernel(block[0], block[1], block[2]); block_index = next_block_index(block_index); } planner_forward_pass_kernel(block[1], block[2], NULL); }
void planner_command(uint8_t type) { // calculate the buffer head and check for space int next_buffer_head = next_block_index( block_buffer_head ); while(block_buffer_tail == next_buffer_head) { // buffer full condition // good! We are well ahead of the robot. Rest here until buffer has room. // sleep_mode(); } // Prepare to set up new block block_t *block = &block_buffer[block_buffer_head]; // set block type command block->block_type = type; // Move buffer head block_buffer_head = next_buffer_head; // make sure the stepper interrupt is processing stepper_wake_up(); }
void plan_buffer_wait (tActionRequest *pAction) { // Calculate the buffer head after we push this block int next_buffer_head = next_block_index( block_buffer_head ); // If the buffer is full: good! That means we are well ahead of the robot. // Rest here until there is room in the buffer. while(block_buffer_tail == next_buffer_head) { sleep_mode(); } // Prepare to set up new block block_t *block = &block_buffer[block_buffer_head]; //TODO block->action_type = pAction->ActionType; // every 50ms block->millimeters = 10; block->nominal_speed = 600; block->nominal_rate = 20*60; block->step_event_count = 1000; // Acceleration planner disabled. Set minimum that is required. block->entry_speed = block->nominal_speed; block->initial_rate = block->nominal_rate; block->final_rate = block->nominal_rate; block->accelerate_until = 0; block->decelerate_after = block->step_event_count; block->rate_delta = 0; // Move buffer head block_buffer_head = next_buffer_head; if (acceleration_manager_enabled) { planner_recalculate(); } st_wake_up(); }
void Planner::getHighESpeed() { static float oldt = 0; if (!autotemp_enabled) return; if (thermalManager.degTargetHotend(0) + 2 < autotemp_min) return; // probably temperature set to zero. float high = 0.0; for (uint8_t b = block_buffer_tail; b != block_buffer_head; b = next_block_index(b)) { block_t* block = &block_buffer[b]; if (block->steps[X_AXIS] || block->steps[Y_AXIS] || block->steps[Z_AXIS]) { float se = (float)block->steps[E_AXIS] / block->step_event_count * block->nominal_speed; // mm/sec; NOLESS(high, se); } } float t = autotemp_min + high * autotemp_factor; t = constrain(t, autotemp_min, autotemp_max); if (oldt > t) { t *= (1 - (AUTOTEMP_OLDWEIGHT)); t += (AUTOTEMP_OLDWEIGHT) * oldt; } oldt = t; thermalManager.setTargetHotend(t, 0); }
void plan_buffer_line(const float &x, const float &y, const float &z, const float &e, float feed_rate, const uint8_t extruder) #endif // AUTO_BED_LEVELING_FEATURE { // Calculate the buffer head after we push this byte int next_buffer_head = next_block_index(block_buffer_head); // If the buffer is full: good! That means we are well ahead of the robot. // Rest here until there is room in the buffer. while (block_buffer_tail == next_buffer_head) idle(); #if ENABLED(MESH_BED_LEVELING) if (mbl.active) z += mbl.get_z(x, y); #elif ENABLED(AUTO_BED_LEVELING_FEATURE) apply_rotation_xyz(plan_bed_level_matrix, x, y, z); #endif // The target position of the tool in absolute steps // 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 long target[NUM_AXIS]; target[X_AXIS] = lround(x * axis_steps_per_unit[X_AXIS]); target[Y_AXIS] = lround(y * axis_steps_per_unit[Y_AXIS]); target[Z_AXIS] = lround(z * axis_steps_per_unit[Z_AXIS]); target[E_AXIS] = lround(e * axis_steps_per_unit[E_AXIS]); float dx = target[X_AXIS] - position[X_AXIS], dy = target[Y_AXIS] - position[Y_AXIS], dz = target[Z_AXIS] - position[Z_AXIS]; // DRYRUN ignores all temperature constraints and assures that the extruder is instantly satisfied if (marlin_debug_flags & DEBUG_DRYRUN) position[E_AXIS] = target[E_AXIS]; float de = target[E_AXIS] - position[E_AXIS]; #if ENABLED(PREVENT_DANGEROUS_EXTRUDE) if (de) { if (degHotend(extruder) < extrude_min_temp) { position[E_AXIS] = target[E_AXIS]; // Behave as if the move really took place, but ignore E part de = 0; // no difference SERIAL_ECHO_START; SERIAL_ECHOLNPGM(MSG_ERR_COLD_EXTRUDE_STOP); } #if ENABLED(PREVENT_LENGTHY_EXTRUDE) if (labs(de) > axis_steps_per_unit[E_AXIS] * EXTRUDE_MAXLENGTH) { position[E_AXIS] = target[E_AXIS]; // Behave as if the move really took place, but ignore E part de = 0; // no difference SERIAL_ECHO_START; SERIAL_ECHOLNPGM(MSG_ERR_LONG_EXTRUDE_STOP); } #endif } #endif // Prepare to set up new block block_t *block = &block_buffer[block_buffer_head]; // Mark block as not busy (Not executed by the stepper interrupt) block->busy = false; // Number of steps for each axis #if ENABLED(COREXY) // corexy planning // these equations follow the form of the dA and dB equations on http://www.corexy.com/theory.html block->steps[A_AXIS] = labs(dx + dy); block->steps[B_AXIS] = labs(dx - dy); block->steps[Z_AXIS] = labs(dz); #elif ENABLED(COREXZ) // corexz planning block->steps[A_AXIS] = labs(dx + dz); block->steps[Y_AXIS] = labs(dy); block->steps[C_AXIS] = labs(dx - dz); #else // default non-h-bot planning block->steps[X_AXIS] = labs(dx); block->steps[Y_AXIS] = labs(dy); block->steps[Z_AXIS] = labs(dz); #endif block->steps[E_AXIS] = labs(de); block->steps[E_AXIS] *= volumetric_multiplier[extruder]; block->steps[E_AXIS] *= extruder_multiplier[extruder]; block->steps[E_AXIS] /= 100; block->step_event_count = max(block->steps[X_AXIS], max(block->steps[Y_AXIS], max(block->steps[Z_AXIS], block->steps[E_AXIS]))); // Bail if this is a zero-length block if (block->step_event_count <= dropsegments) return; block->fan_speed = fanSpeed; #if ENABLED(BARICUDA) block->valve_pressure = ValvePressure; block->e_to_p_pressure = EtoPPressure; #endif // Compute direction bits for this block uint8_t db = 0; #if ENABLED(COREXY) if (dx < 0) db |= BIT(X_HEAD); // Save the real Extruder (head) direction in X Axis if (dy < 0) db |= BIT(Y_HEAD); // ...and Y if (dz < 0) db |= BIT(Z_AXIS); if (dx + dy < 0) db |= BIT(A_AXIS); // Motor A direction if (dx - dy < 0) db |= BIT(B_AXIS); // Motor B direction #elif ENABLED(COREXZ) if (dx < 0) db |= BIT(X_HEAD); // Save the real Extruder (head) direction in X Axis if (dy < 0) db |= BIT(Y_AXIS); if (dz < 0) db |= BIT(Z_HEAD); // ...and Z if (dx + dz < 0) db |= BIT(A_AXIS); // Motor A direction if (dx - dz < 0) db |= BIT(C_AXIS); // Motor B direction #else if (dx < 0) db |= BIT(X_AXIS); if (dy < 0) db |= BIT(Y_AXIS); if (dz < 0) db |= BIT(Z_AXIS); #endif if (de < 0) db |= BIT(E_AXIS); block->direction_bits = db; block->active_extruder = extruder; //enable active axes #if ENABLED(COREXY) if (block->steps[A_AXIS] || block->steps[B_AXIS]) { enable_x(); enable_y(); } #if DISABLED(Z_LATE_ENABLE) if (block->steps[Z_AXIS]) enable_z(); #endif #elif ENABLED(COREXZ) if (block->steps[A_AXIS] || block->steps[C_AXIS]) { enable_x(); enable_z(); } if (block->steps[Y_AXIS]) enable_y(); #else if (block->steps[X_AXIS]) enable_x(); if (block->steps[Y_AXIS]) enable_y(); #if DISABLED(Z_LATE_ENABLE) if (block->steps[Z_AXIS]) enable_z(); #endif #endif // Enable extruder(s) if (block->steps[E_AXIS]) { if (DISABLE_INACTIVE_EXTRUDER) { //enable only selected extruder for (int i=0; i<EXTRUDERS; i++) if (g_uc_extruder_last_move[i] > 0) g_uc_extruder_last_move[i]--; switch(extruder) { case 0: enable_e0(); g_uc_extruder_last_move[0] = BLOCK_BUFFER_SIZE * 2; #if EXTRUDERS > 1 if (g_uc_extruder_last_move[1] == 0) disable_e1(); #if EXTRUDERS > 2 if (g_uc_extruder_last_move[2] == 0) disable_e2(); #if EXTRUDERS > 3 if (g_uc_extruder_last_move[3] == 0) disable_e3(); #endif #endif #endif break; #if EXTRUDERS > 1 case 1: enable_e1(); g_uc_extruder_last_move[1] = BLOCK_BUFFER_SIZE * 2; if (g_uc_extruder_last_move[0] == 0) disable_e0(); #if EXTRUDERS > 2 if (g_uc_extruder_last_move[2] == 0) disable_e2(); #if EXTRUDERS > 3 if (g_uc_extruder_last_move[3] == 0) disable_e3(); #endif #endif break; #if EXTRUDERS > 2 case 2: enable_e2(); g_uc_extruder_last_move[2] = BLOCK_BUFFER_SIZE * 2; if (g_uc_extruder_last_move[0] == 0) disable_e0(); if (g_uc_extruder_last_move[1] == 0) disable_e1(); #if EXTRUDERS > 3 if (g_uc_extruder_last_move[3] == 0) disable_e3(); #endif break; #if EXTRUDERS > 3 case 3: enable_e3(); g_uc_extruder_last_move[3] = BLOCK_BUFFER_SIZE * 2; if (g_uc_extruder_last_move[0] == 0) disable_e0(); if (g_uc_extruder_last_move[1] == 0) disable_e1(); if (g_uc_extruder_last_move[2] == 0) disable_e2(); break; #endif // EXTRUDERS > 3 #endif // EXTRUDERS > 2 #endif // EXTRUDERS > 1 } } else { // enable all enable_e0(); enable_e1(); enable_e2(); enable_e3(); } } if (block->steps[E_AXIS]) NOLESS(feed_rate, minimumfeedrate); else NOLESS(feed_rate, mintravelfeedrate); /** * This part of the code calculates the total length of the movement. * For cartesian bots, the X_AXIS is the real X movement and same for Y_AXIS. * But for corexy bots, that is not true. The "X_AXIS" and "Y_AXIS" motors (that should be named to A_AXIS * and B_AXIS) cannot be used for X and Y length, because A=X+Y and B=X-Y. * So we need to create other 2 "AXIS", named X_HEAD and Y_HEAD, meaning the real displacement of the Head. * Having the real displacement of the head, we can calculate the total movement length and apply the desired speed. */ #if ENABLED(COREXY) float delta_mm[6]; delta_mm[X_HEAD] = dx / axis_steps_per_unit[A_AXIS]; delta_mm[Y_HEAD] = dy / axis_steps_per_unit[B_AXIS]; delta_mm[Z_AXIS] = dz / axis_steps_per_unit[Z_AXIS]; delta_mm[A_AXIS] = (dx + dy) / axis_steps_per_unit[A_AXIS]; delta_mm[B_AXIS] = (dx - dy) / axis_steps_per_unit[B_AXIS]; #elif ENABLED(COREXZ) float delta_mm[6]; delta_mm[X_HEAD] = dx / axis_steps_per_unit[A_AXIS]; delta_mm[Y_AXIS] = dy / axis_steps_per_unit[Y_AXIS]; delta_mm[Z_HEAD] = dz / axis_steps_per_unit[C_AXIS]; delta_mm[A_AXIS] = (dx + dz) / axis_steps_per_unit[A_AXIS]; delta_mm[C_AXIS] = (dx - dz) / axis_steps_per_unit[C_AXIS]; #else float delta_mm[4]; delta_mm[X_AXIS] = dx / axis_steps_per_unit[X_AXIS]; delta_mm[Y_AXIS] = dy / axis_steps_per_unit[Y_AXIS]; delta_mm[Z_AXIS] = dz / axis_steps_per_unit[Z_AXIS]; #endif delta_mm[E_AXIS] = (de / axis_steps_per_unit[E_AXIS]) * volumetric_multiplier[extruder] * extruder_multiplier[extruder] / 100.0; if (block->steps[X_AXIS] <= dropsegments && block->steps[Y_AXIS] <= dropsegments && block->steps[Z_AXIS] <= dropsegments) { block->millimeters = fabs(delta_mm[E_AXIS]); } else { block->millimeters = sqrt( #if ENABLED(COREXY) square(delta_mm[X_HEAD]) + square(delta_mm[Y_HEAD]) + square(delta_mm[Z_AXIS]) #elif ENABLED(COREXZ) square(delta_mm[X_HEAD]) + square(delta_mm[Y_AXIS]) + square(delta_mm[Z_HEAD]) #else square(delta_mm[X_AXIS]) + square(delta_mm[Y_AXIS]) + square(delta_mm[Z_AXIS]) #endif ); } float inverse_millimeters = 1.0 / block->millimeters; // Inverse millimeters to remove multiple divides // Calculate speed in mm/second for each axis. No divide by zero due to previous checks. float inverse_second = feed_rate * inverse_millimeters; int moves_queued = movesplanned(); // Slow down when the buffer starts to empty, rather than wait at the corner for a buffer refill #if ENABLED(OLD_SLOWDOWN) || ENABLED(SLOWDOWN) bool mq = moves_queued > 1 && moves_queued < BLOCK_BUFFER_SIZE / 2; #if ENABLED(OLD_SLOWDOWN) if (mq) feed_rate *= 2.0 * moves_queued / BLOCK_BUFFER_SIZE; #endif #if ENABLED(SLOWDOWN) // segment time im micro seconds unsigned long segment_time = lround(1000000.0/inverse_second); if (mq) { if (segment_time < minsegmenttime) { // buffer is draining, add extra time. The amount of time added increases if the buffer is still emptied more. inverse_second = 1000000.0 / (segment_time + lround(2 * (minsegmenttime - segment_time) / moves_queued)); #ifdef XY_FREQUENCY_LIMIT segment_time = lround(1000000.0 / inverse_second); #endif } } #endif #endif block->nominal_speed = block->millimeters * inverse_second; // (mm/sec) Always > 0 block->nominal_rate = ceil(block->step_event_count * inverse_second); // (step/sec) Always > 0 #if ENABLED(FILAMENT_SENSOR) //FMM update ring buffer used for delay with filament measurements if (extruder == FILAMENT_SENSOR_EXTRUDER_NUM && delay_index2 > -1) { //only for extruder with filament sensor and if ring buffer is initialized const int MMD = MAX_MEASUREMENT_DELAY + 1, MMD10 = MMD * 10; delay_dist += delta_mm[E_AXIS]; // increment counter with next move in e axis while (delay_dist >= MMD10) delay_dist -= MMD10; // loop around the buffer while (delay_dist < 0) delay_dist += MMD10; delay_index1 = delay_dist / 10.0; // calculate index delay_index1 = constrain(delay_index1, 0, MAX_MEASUREMENT_DELAY); // (already constrained above) if (delay_index1 != delay_index2) { // moved index meas_sample = widthFil_to_size_ratio() - 100; // Subtract 100 to reduce magnitude - to store in a signed char while (delay_index1 != delay_index2) { // Increment and loop around buffer if (++delay_index2 >= MMD) delay_index2 -= MMD; delay_index2 = constrain(delay_index2, 0, MAX_MEASUREMENT_DELAY); measurement_delay[delay_index2] = meas_sample; } } } #endif // Calculate and limit speed in mm/sec for each axis float current_speed[NUM_AXIS]; float speed_factor = 1.0; //factor <=1 do decrease speed for (int i = 0; i < NUM_AXIS; i++) { current_speed[i] = delta_mm[i] * inverse_second; float cs = fabs(current_speed[i]), mf = max_feedrate[i]; if (cs > mf) speed_factor = min(speed_factor, mf / cs); } // Max segement time in us. #ifdef XY_FREQUENCY_LIMIT #define MAX_FREQ_TIME (1000000.0 / XY_FREQUENCY_LIMIT) // Check and limit the xy direction change frequency unsigned char direction_change = block->direction_bits ^ old_direction_bits; old_direction_bits = block->direction_bits; segment_time = lround((float)segment_time / speed_factor); long xs0 = axis_segment_time[X_AXIS][0], xs1 = axis_segment_time[X_AXIS][1], xs2 = axis_segment_time[X_AXIS][2], ys0 = axis_segment_time[Y_AXIS][0], ys1 = axis_segment_time[Y_AXIS][1], ys2 = axis_segment_time[Y_AXIS][2]; if ((direction_change & BIT(X_AXIS)) != 0) { xs2 = axis_segment_time[X_AXIS][2] = xs1; xs1 = axis_segment_time[X_AXIS][1] = xs0; xs0 = 0; } xs0 = axis_segment_time[X_AXIS][0] = xs0 + segment_time; if ((direction_change & BIT(Y_AXIS)) != 0) { ys2 = axis_segment_time[Y_AXIS][2] = axis_segment_time[Y_AXIS][1]; ys1 = axis_segment_time[Y_AXIS][1] = axis_segment_time[Y_AXIS][0]; ys0 = 0; } ys0 = axis_segment_time[Y_AXIS][0] = ys0 + segment_time; long max_x_segment_time = max(xs0, max(xs1, xs2)), max_y_segment_time = max(ys0, max(ys1, ys2)), min_xy_segment_time = min(max_x_segment_time, max_y_segment_time); if (min_xy_segment_time < MAX_FREQ_TIME) { float low_sf = speed_factor * min_xy_segment_time / MAX_FREQ_TIME; speed_factor = min(speed_factor, low_sf); } #endif // XY_FREQUENCY_LIMIT // Correct the speed if (speed_factor < 1.0) { for (unsigned char i = 0; i < NUM_AXIS; i++) current_speed[i] *= speed_factor; block->nominal_speed *= speed_factor; block->nominal_rate *= speed_factor; } // Compute and limit the acceleration rate for the trapezoid generator. float steps_per_mm = block->step_event_count / block->millimeters; long bsx = block->steps[X_AXIS], bsy = block->steps[Y_AXIS], bsz = block->steps[Z_AXIS], bse = block->steps[E_AXIS]; if (bsx == 0 && bsy == 0 && bsz == 0) { block->acceleration_st = ceil(retract_acceleration * steps_per_mm); // convert to: acceleration steps/sec^2 } else if (bse == 0) { block->acceleration_st = ceil(travel_acceleration * steps_per_mm); // convert to: acceleration steps/sec^2 } else { block->acceleration_st = ceil(acceleration * steps_per_mm); // convert to: acceleration steps/sec^2 } // Limit acceleration per axis unsigned long acc_st = block->acceleration_st, xsteps = axis_steps_per_sqr_second[X_AXIS], ysteps = axis_steps_per_sqr_second[Y_AXIS], zsteps = axis_steps_per_sqr_second[Z_AXIS], esteps = axis_steps_per_sqr_second[E_AXIS]; if ((float)acc_st * bsx / block->step_event_count > xsteps) acc_st = xsteps; if ((float)acc_st * bsy / block->step_event_count > ysteps) acc_st = ysteps; if ((float)acc_st * bsz / block->step_event_count > zsteps) acc_st = zsteps; if ((float)acc_st * bse / block->step_event_count > esteps) acc_st = esteps; block->acceleration_st = acc_st; block->acceleration = acc_st / steps_per_mm; block->acceleration_rate = (long)(acc_st * 16777216.0 / (F_CPU / 8.0)); #if 0 // Use old jerk for now // Compute path unit vector double unit_vec[3]; unit_vec[X_AXIS] = delta_mm[X_AXIS]*inverse_millimeters; unit_vec[Y_AXIS] = delta_mm[Y_AXIS]*inverse_millimeters; unit_vec[Z_AXIS] = delta_mm[Z_AXIS]*inverse_millimeters; // Compute maximum allowable entry speed at junction by centripetal acceleration approximation. // Let a circle be tangent to both previous and current path line segments, where the junction // deviation is defined as the distance from the junction to the closest edge of the circle, // colinear with the circle center. The circular segment joining the two paths represents the // path of centripetal acceleration. Solve for max velocity based on max acceleration about the // radius of the circle, defined indirectly by junction deviation. This may be also viewed as // path width or max_jerk in the previous grbl version. This approach does not actually deviate // from path, but used as a robust way to compute cornering speeds, as it takes into account the // nonlinearities of both the junction angle and junction velocity. double vmax_junction = MINIMUM_PLANNER_SPEED; // Set default max junction speed // Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles. if ((block_buffer_head != block_buffer_tail) && (previous_nominal_speed > 0.0)) { // Compute cosine of angle between previous and current path. (prev_unit_vec is negative) // NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity. double cos_theta = - previous_unit_vec[X_AXIS] * unit_vec[X_AXIS] - previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS] - previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ; // Skip and use default max junction speed for 0 degree acute junction. if (cos_theta < 0.95) { vmax_junction = min(previous_nominal_speed,block->nominal_speed); // 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 double sin_theta_d2 = sqrt(0.5*(1.0-cos_theta)); // Trig half angle identity. Always positive. vmax_junction = min(vmax_junction, sqrt(block->acceleration * junction_deviation * sin_theta_d2/(1.0-sin_theta_d2)) ); } } } #endif // Start with a safe speed float vmax_junction = max_xy_jerk / 2; float vmax_junction_factor = 1.0; float mz2 = max_z_jerk / 2, me2 = max_e_jerk / 2; float csz = current_speed[Z_AXIS], cse = current_speed[E_AXIS]; if (fabs(csz) > mz2) vmax_junction = min(vmax_junction, mz2); if (fabs(cse) > me2) vmax_junction = min(vmax_junction, me2); vmax_junction = min(vmax_junction, block->nominal_speed); float safe_speed = vmax_junction; if ((moves_queued > 1) && (previous_nominal_speed > 0.0001)) { float dx = current_speed[X_AXIS] - previous_speed[X_AXIS], dy = current_speed[Y_AXIS] - previous_speed[Y_AXIS], dz = fabs(csz - previous_speed[Z_AXIS]), de = fabs(cse - previous_speed[E_AXIS]), jerk = sqrt(dx * dx + dy * dy); // if ((fabs(previous_speed[X_AXIS]) > 0.0001) || (fabs(previous_speed[Y_AXIS]) > 0.0001)) { vmax_junction = block->nominal_speed; // } if (jerk > max_xy_jerk) vmax_junction_factor = max_xy_jerk / jerk; if (dz > max_z_jerk) vmax_junction_factor = min(vmax_junction_factor, max_z_jerk / dz); if (de > max_e_jerk) vmax_junction_factor = min(vmax_junction_factor, max_e_jerk / de); vmax_junction = min(previous_nominal_speed, vmax_junction * vmax_junction_factor); // Limit speed to max previous speed } block->max_entry_speed = vmax_junction; // Initialize block entry speed. Compute based on deceleration to user-defined MINIMUM_PLANNER_SPEED. double v_allowable = max_allowable_speed(-block->acceleration, MINIMUM_PLANNER_SPEED, block->millimeters); block->entry_speed = min(vmax_junction, v_allowable); // Initialize planner efficiency flags // Set flag if block will always reach maximum junction speed regardless of entry/exit speeds. // If a block can de/ac-celerate from nominal speed to zero within the length of the block, then // the current block and next block junction speeds are guaranteed to always be at their maximum // junction speeds in deceleration and acceleration, respectively. This is due to how the current // block nominal speed limits both the current and next maximum junction speeds. Hence, in both // the reverse and forward planners, the corresponding block junction speed will always be at the // the maximum junction speed and may always be ignored for any speed reduction checks. block->nominal_length_flag = (block->nominal_speed <= v_allowable); block->recalculate_flag = true; // Always calculate trapezoid for new block // Update previous path unit_vector and nominal speed for (int i = 0; i < NUM_AXIS; i++) previous_speed[i] = current_speed[i]; previous_nominal_speed = block->nominal_speed; #if ENABLED(ADVANCE) // Calculate advance rate if (!bse || (!bsx && !bsy && !bsz)) { block->advance_rate = 0; block->advance = 0; } else { long acc_dist = estimate_acceleration_distance(0, block->nominal_rate, block->acceleration_st); float advance = (STEPS_PER_CUBIC_MM_E * EXTRUDER_ADVANCE_K) * (cse * cse * EXTRUSION_AREA * EXTRUSION_AREA) * 256; block->advance = advance; block->advance_rate = acc_dist ? advance / (float)acc_dist : 0; } /* SERIAL_ECHO_START; SERIAL_ECHOPGM("advance :"); SERIAL_ECHO(block->advance/256.0); SERIAL_ECHOPGM("advance rate :"); SERIAL_ECHOLN(block->advance_rate/256.0); */ #endif // ADVANCE calculate_trapezoid_for_block(block, block->entry_speed / block->nominal_speed, safe_speed / block->nominal_speed); // Move buffer head block_buffer_head = next_buffer_head; // Update position for (int i = 0; i < NUM_AXIS; i++) position[i] = target[i]; planner_recalculate(); st_wake_up(); } // plan_buffer_line()
void check_axes_activity() { unsigned char axis_active[NUM_AXIS] = { 0 }, tail_fan_speed = fanSpeed; #if ENABLED(BARICUDA) unsigned char tail_valve_pressure = ValvePressure, tail_e_to_p_pressure = EtoPPressure; #endif block_t *block; if (blocks_queued()) { uint8_t block_index = block_buffer_tail; tail_fan_speed = block_buffer[block_index].fan_speed; #if ENABLED(BARICUDA) block = &block_buffer[block_index]; tail_valve_pressure = block->valve_pressure; tail_e_to_p_pressure = block->e_to_p_pressure; #endif while (block_index != block_buffer_head) { block = &block_buffer[block_index]; for (int i=0; i<NUM_AXIS; i++) if (block->steps[i]) axis_active[i]++; block_index = next_block_index(block_index); } } if (DISABLE_X && !axis_active[X_AXIS]) disable_x(); if (DISABLE_Y && !axis_active[Y_AXIS]) disable_y(); if (DISABLE_Z && !axis_active[Z_AXIS]) disable_z(); if (DISABLE_E && !axis_active[E_AXIS]) { disable_e0(); disable_e1(); disable_e2(); disable_e3(); } #if HAS_FAN #ifdef FAN_KICKSTART_TIME static millis_t fan_kick_end; if (tail_fan_speed) { millis_t ms = millis(); if (fan_kick_end == 0) { // Just starting up fan - run at full power. fan_kick_end = ms + FAN_KICKSTART_TIME; tail_fan_speed = 255; } else if (fan_kick_end > ms) // Fan still spinning up. tail_fan_speed = 255; } else { fan_kick_end = 0; } #endif //FAN_KICKSTART_TIME #if ENABLED(FAN_MIN_PWM) #define CALC_FAN_SPEED (tail_fan_speed ? ( FAN_MIN_PWM + (tail_fan_speed * (255 - FAN_MIN_PWM)) / 255 ) : 0) #else #define CALC_FAN_SPEED tail_fan_speed #endif // FAN_MIN_PWM #if ENABLED(FAN_SOFT_PWM) fanSpeedSoftPwm = CALC_FAN_SPEED; #else analogWrite(FAN_PIN, CALC_FAN_SPEED); #endif // FAN_SOFT_PWM #endif // HAS_FAN #if ENABLED(AUTOTEMP) getHighESpeed(); #endif #if ENABLED(BARICUDA) #if HAS_HEATER_1 analogWrite(HEATER_1_PIN,tail_valve_pressure); #endif #if HAS_HEATER_2 analogWrite(HEATER_2_PIN,tail_e_to_p_pressure); #endif #endif }
// Add a new linear movement to the buffer. x, y and z is the signed, absolute target position in // millimeters. Feed rate specifies the speed of the motion. If feed rate is inverted, the feed // rate is taken to mean "frequency" and would complete the operation in 1/feed_rate minutes. // All position data passed to the planner must be in terms of machine position to keep the planner // independent of any coordinate system changes and offsets, which are handled by the g-code parser. // NOTE: Assumes buffer is available. Buffer checks are handled at a higher level by motion_control. // Also the feed rate input value is used in three ways: as a normal feed rate if invert_feed_rate // is false, as inverse time if invert_feed_rate is true, or as seek/rapids rate if the feed_rate // value is negative (and invert_feed_rate always false). void plan_buffer_line(float x, float y, float z, float feed_rate, uint8_t invert_feed_rate) { // Prepare to set up new block block_t *block = &block_buffer[block_buffer_head]; // Calculate target position in absolute steps int32_t target[N_AXIS]; target[X_AXIS] = lround(x*settings.steps_per_mm[X_AXIS]); target[Y_AXIS] = lround(y*settings.steps_per_mm[Y_AXIS]); target[Z_AXIS] = lround(z*settings.steps_per_mm[Z_AXIS]); // Number of steps for each axis block->steps_x = labs(target[X_AXIS]-pl.position[X_AXIS]); block->steps_y = labs(target[Y_AXIS]-pl.position[Y_AXIS]); block->steps_z = labs(target[Z_AXIS]-pl.position[Z_AXIS]); block->step_event_count = max(block->steps_x, max(block->steps_y, block->steps_z)); // Bail if this is a zero-length block if (block->step_event_count == 0) { return; }; // Compute path vector in terms of absolute step target and current positions float delta_mm[N_AXIS]; delta_mm[X_AXIS] = x-pl.last_x; delta_mm[Y_AXIS] = y-pl.last_y; delta_mm[Z_AXIS] = z-pl.last_z; block->millimeters = sqrt(delta_mm[X_AXIS]*delta_mm[X_AXIS] + delta_mm[Y_AXIS]*delta_mm[Y_AXIS] + delta_mm[Z_AXIS]*delta_mm[Z_AXIS]); // Adjust feed_rate value to mm/min depending on type of rate input (normal, inverse time, or rapids) // TODO: Need to distinguish a rapids vs feed move for overrides. Some flag of some sort. if (feed_rate < 0) { feed_rate = SOME_LARGE_VALUE; } // Scaled down to absolute max/rapids rate later else if (invert_feed_rate) { feed_rate = block->millimeters/feed_rate; } // Calculate the unit vector of the line move and the block maximum feed rate and acceleration limited // by the maximum possible values. Block rapids rates are computed or feed rates are scaled down so // they don't exceed the maximum axes velocities. The block acceleration is maximized based on direction // and axes properties as well. // NOTE: This calculation assumes all axes are orthogonal (Cartesian) and works with ABC-axes, // if they are also orthogonal/independent. Operates on the absolute value of the unit vector. uint8_t i; float unit_vec[N_AXIS], inverse_unit_vec_value; float inverse_millimeters = 1.0/block->millimeters; // Inverse millimeters to remove multiple float divides block->acceleration = SOME_LARGE_VALUE; // Scaled down to maximum acceleration in loop for (i=0; i<N_AXIS; i++) { if (delta_mm[i] == 0) { unit_vec[i] = 0; // Store zero value. And avoid divide by zero. } else { // Compute unit vector and its absolute inverse value unit_vec[i] = delta_mm[i]*inverse_millimeters; inverse_unit_vec_value = abs(1.0/unit_vec[i]); // Check and limit feed rate against max axis velocities and scale accelerations to maximums feed_rate = min(feed_rate,settings.max_velocity[i]*inverse_unit_vec_value); block->acceleration = min(block->acceleration,settings.acceleration[i]*inverse_unit_vec_value); } } // Compute nominal speed and rates block->nominal_speed_sqr = feed_rate*feed_rate; // (mm/min)^2. Always > 0 block->nominal_rate = ceil(feed_rate*(RANADE_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic) // Compute the acceleration and distance traveled per step event for the stepper algorithm. block->rate_delta = ceil(block->acceleration* ((RANADE_MULTIPLIER/(60.0*60.0))/(ISR_TICKS_PER_SECOND*ACCELERATION_TICKS_PER_SECOND))); // (mult*mm/isr_tic/accel_tic) block->d_next = ceil((block->millimeters*RANADE_MULTIPLIER)/block->step_event_count); // (mult*mm/step) // Compute direction bits. Bit enabled always means direction is negative. block->direction_bits = 0; if (unit_vec[X_AXIS] < 0) { block->direction_bits |= (1<<X_DIRECTION_BIT); } if (unit_vec[Y_AXIS] < 0) { block->direction_bits |= (1<<Y_DIRECTION_BIT); } if (unit_vec[Z_AXIS] < 0) { block->direction_bits |= (1<<Z_DIRECTION_BIT); } // Compute maximum allowable entry speed at junction by centripetal acceleration approximation. // Let a circle be tangent to both previous and current path line segments, where the junction // deviation is defined as the distance from the junction to the closest edge of the circle, // colinear with the circle center. The circular segment joining the two paths represents the // path of centripetal acceleration. Solve for max velocity based on max acceleration about the // radius of the circle, defined indirectly by junction deviation. This may be also viewed as // path width or max_jerk in the previous grbl version. This approach does not actually deviate // from path, but used as a robust way to compute cornering speeds, as it takes into account the // nonlinearities of both the junction angle and junction velocity. // NOTE: If the junction deviation value is finite, Grbl executes the motions in an exact path // mode (G61). If the junction deviation value is zero, Grbl will execute the motion in an exact // stop mode (G61.1) manner. In the future, if continuous mode (G64) is desired, the math here // is exactly the same. Instead of motioning all the way to junction point, the machine will // just follow the arc circle defined here. The Arduino doesn't have the CPU cycles to perform // a continuous mode path, but ARM-based microcontrollers most certainly do. // Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles. block->max_entry_speed_sqr = MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED; if ((block_buffer_head != block_buffer_tail) && (pl.previous_nominal_speed_sqr > 0.0)) { // Compute cosine of angle between previous and current path. (prev_unit_vec is negative) // NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity. float cos_theta = - pl.previous_unit_vec[X_AXIS] * unit_vec[X_AXIS] - pl.previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS] - pl.previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ; // Skip and use default max junction speed for 0 degree acute junction. if (cos_theta < 0.95) { block->max_entry_speed_sqr = min(block->nominal_speed_sqr,pl.previous_nominal_speed_sqr); // 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. block->max_entry_speed_sqr = min(block->max_entry_speed_sqr, block->acceleration * settings.junction_deviation * sin_theta_d2/(1.0-sin_theta_d2)); } } } // Initialize block entry speed. Compute block entry velocity backwards from user-defined MINIMUM_PLANNER_SPEED. // TODO: This could be moved to the planner recalculate function. block->entry_speed_sqr = min( block->max_entry_speed_sqr, MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED + 2*block->acceleration*block->millimeters); // Set new block to be recalculated for conversion to stepper data. block->recalculate_flag = true; // Update previous path unit_vector and nominal speed (squared) memcpy(pl.previous_unit_vec, unit_vec, sizeof(unit_vec)); // pl.previous_unit_vec[] = unit_vec[] pl.previous_nominal_speed_sqr = block->nominal_speed_sqr; // Update planner position memcpy(pl.position, target, sizeof(target)); // pl.position[] = target[] pl.last_x = x; pl.last_y = y; pl.last_z = z; // Update buffer head and next buffer head indices block_buffer_head = next_buffer_head; next_buffer_head = next_block_index(block_buffer_head); planner_recalculate(); }
// Add a new linear movement to the buffer. x, y and z is the signed, absolute target position in // millimaters. Feed rate specifies the speed of the motion. If feed rate is inverted, the feed // rate is taken to mean "frequency" and would complete the operation in 1/feed_rate minutes. //void plan_buffer_line(double x, double y, double z, double feed_rate, uint8_t invert_feed_rate) void plan_buffer_line (tActionRequest *pAction) { double x; double y; double z; double feed_rate; uint8_t invert_feed_rate; bool e_only = false; double speed_x, speed_y, speed_z, speed_e; // Nominal mm/minute for each axis x = pAction->target.x; y = pAction->target.y; z = pAction->target.z; feed_rate = pAction->target.feed_rate; invert_feed_rate = pAction->target.invert_feed_rate; // Calculate target position in absolute steps int32_t target[NUM_AXES]; target[X_AXIS] = lround(x*(double)config.steps_per_mm_x); target[Y_AXIS] = lround(y*(double)config.steps_per_mm_y); target[Z_AXIS] = lround(z*(double)config.steps_per_mm_z); target[E_AXIS] = lround(pAction->target.e*(double)config.steps_per_mm_e); // Calculate the buffer head after we push this byte int next_buffer_head = next_block_index( block_buffer_head ); // If the buffer is full: good! That means we are well ahead of the robot. // Rest here until there is room in the buffer. while(block_buffer_tail == next_buffer_head) { sleep_mode(); } // Prepare to set up new block block_t *block = &block_buffer[block_buffer_head]; block->action_type = AT_MOVE; // Compute direction bits for this block block->direction_bits = 0; if (target[X_AXIS] < position[X_AXIS]) { block->direction_bits |= X_DIR_BIT; } if (target[Y_AXIS] < position[Y_AXIS]) { block->direction_bits |= Y_DIR_BIT; } if (target[Z_AXIS] < position[Z_AXIS]) { block->direction_bits |= Z_DIR_BIT; } if (target[E_AXIS] < position[E_AXIS]) { block->direction_bits |= E_DIR_BIT; } // Number of steps for each axis block->steps_x = labs(target[X_AXIS]-position[X_AXIS]); block->steps_y = labs(target[Y_AXIS]-position[Y_AXIS]); block->steps_z = labs(target[Z_AXIS]-position[Z_AXIS]); block->steps_e = labs(target[E_AXIS]-position[E_AXIS]); block->step_event_count = max(block->steps_x, max(block->steps_y, block->steps_z)); block->step_event_count = max(block->step_event_count, block->steps_e); // Bail if this is a zero-length block if (block->step_event_count == 0) { return; }; // Compute path vector in terms of absolute step target and current positions double delta_mm[NUM_AXES]; delta_mm[X_AXIS] = (target[X_AXIS]-position[X_AXIS])/(double)config.steps_per_mm_x; delta_mm[Y_AXIS] = (target[Y_AXIS]-position[Y_AXIS])/(double)config.steps_per_mm_y; delta_mm[Z_AXIS] = (target[Z_AXIS]-position[Z_AXIS])/(double)config.steps_per_mm_z; delta_mm[E_AXIS] = (target[E_AXIS]-position[E_AXIS])/(double)config.steps_per_mm_e; block->millimeters = sqrt(square(delta_mm[X_AXIS]) + square(delta_mm[Y_AXIS]) + square(delta_mm[Z_AXIS])); if (block->millimeters == 0) { e_only = true; block->millimeters = fabs(delta_mm[E_AXIS]); } double inverse_millimeters = 1.0/block->millimeters; // Inverse millimeters to remove multiple divides // // Speed limit code from Marlin firmware // //TODO: handle invert_feed_rate double microseconds; //if(feedrate<minimumfeedrate) // feedrate=minimumfeedrate; microseconds = lround((block->millimeters/feed_rate*60.0)*1000000.0); // Calculate speed in mm/minute for each axis double multiplier = 60.0*1000000.0/(double)microseconds; speed_x = delta_mm[X_AXIS] * multiplier; speed_y = delta_mm[Y_AXIS] * multiplier; speed_z = delta_mm[Z_AXIS] * multiplier; speed_e = delta_mm[E_AXIS] * multiplier; // Limit speed per axis double speed_factor = 1; //factor <=1 do decrease speed if(fabs(speed_x) > config.maximum_feedrate_x) { speed_factor = (double)config.maximum_feedrate_x / fabs(speed_x); } if(fabs(speed_y) > config.maximum_feedrate_y) { double tmp_speed_factor = (double)config.maximum_feedrate_y / fabs(speed_y); if(speed_factor > tmp_speed_factor) speed_factor = tmp_speed_factor; } if(fabs(speed_z) > config.maximum_feedrate_z) { double tmp_speed_factor = (double)config.maximum_feedrate_z / fabs(speed_z); if(speed_factor > tmp_speed_factor) speed_factor = tmp_speed_factor; } if(fabs(speed_e) > config.maximum_feedrate_e) { double tmp_speed_factor = (double)config.maximum_feedrate_e / fabs(speed_e); if(speed_factor > tmp_speed_factor) speed_factor = tmp_speed_factor; } multiplier = multiplier * speed_factor; speed_x = delta_mm[X_AXIS] * multiplier; speed_y = delta_mm[Y_AXIS] * multiplier; speed_z = delta_mm[Z_AXIS] * multiplier; speed_e = delta_mm[E_AXIS] * multiplier; block->nominal_speed = block->millimeters * multiplier; // mm per min block->nominal_rate = ceil(block->step_event_count * multiplier); // steps per minute //--- #if 0 // Calculate speed in mm/minute for each axis. No divide by zero due to previous checks. // NOTE: Minimum stepper speed is limited by MINIMUM_STEPS_PER_MINUTE in stepper.c double inverse_minute; if (!invert_feed_rate) { inverse_minute = feed_rate * inverse_millimeters; } else { inverse_minute = 1.0 / feed_rate; } block->nominal_speed = block->millimeters * inverse_minute; // (mm/min) Always > 0 block->nominal_rate = ceil(block->step_event_count * inverse_minute); // (step/min) Always > 0 #endif #if 0 double axis_speed; axis_speed = delta_mm[Z_AXIS] * inverse_minute; if (axis_speed > config.maximum_feedrate_z) { inverse_millimeters = 1.0 / delta_mm[Z_AXIS]; inverse_minute = calc_inverse_minute (false, config.maximum_feedrate_z, inverse_millimeters); block->nominal_speed = delta_mm[Z_AXIS] * inverse_minute; // (mm/min) Always > 0 block->nominal_rate = ceil(block->step_event_count * inverse_minute); // (step/min) Always > 0 } #endif // Compute the acceleration rate for the trapezoid generator. Depending on the slope of the line // average travel per step event changes. For a line along one axis the travel per step event // is equal to the travel/step in the particular axis. For a 45 degree line the steppers of both // axes might step for every step event. Travel per step event is then sqrt(travel_x^2+travel_y^2). // To generate trapezoids with contant acceleration between blocks the rate_delta must be computed // specifically for each line to compensate for this phenomenon: // Convert universal acceleration for direction-dependent stepper rate change parameter block->rate_delta = ceil( block->step_event_count*inverse_millimeters * config.acceleration*60.0 / ACCELERATION_TICKS_PER_SECOND ); // (step/min/acceleration_tick) #if 0 double rate_calc; if (delta_mm[Z_AXIS] > 0) { rate_calc = ceil( block->step_event_count / delta_mm[Z_AXIS] * 50*60.0 / ACCELERATION_TICKS_PER_SECOND ); // (step/min/acceleration_tick) if (rate_calc < block->rate_delta) block->rate_delta = rate_calc; } #endif // Perform planner-enabled calculations if (acceleration_manager_enabled /*&& !e_only*/ ) { // Compute path unit vector double unit_vec[NUM_AXES]; unit_vec[X_AXIS] = delta_mm[X_AXIS]*inverse_millimeters; unit_vec[Y_AXIS] = delta_mm[Y_AXIS]*inverse_millimeters; unit_vec[Z_AXIS] = delta_mm[Z_AXIS]*inverse_millimeters; // Compute maximum allowable entry speed at junction by centripetal acceleration approximation. // Let a circle be tangent to both previous and current path line segments, where the junction // deviation is defined as the distance from the junction to the closest edge of the circle, // colinear with the circle center. The circular segment joining the two paths represents the // path of centripetal acceleration. Solve for max velocity based on max acceleration about the // radius of the circle, defined indirectly by junction deviation. This may be also viewed as // path width or max_jerk in the previous grbl version. This approach does not actually deviate // from path, but used as a robust way to compute cornering speeds, as it takes into account the // nonlinearities of both the junction angle and junction velocity. double vmax_junction = MINIMUM_PLANNER_SPEED; // Set default max junction speed // Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles. if ((block_buffer_head != block_buffer_tail) && (previous_nominal_speed > 0.0)) { // Compute cosine of angle between previous and current path. (prev_unit_vec is negative) // NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity. double cos_theta = - previous_unit_vec[X_AXIS] * unit_vec[X_AXIS] - previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS] - previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ; // Skip and use default max junction speed for 0 degree acute junction. if (cos_theta < 0.95) { vmax_junction = min(previous_nominal_speed,block->nominal_speed); // 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 double sin_theta_d2 = sqrt(0.5*(1.0-cos_theta)); // Trig half angle identity. Always positive. vmax_junction = min(vmax_junction, sqrt(config.acceleration*60*60 * config.junction_deviation * sin_theta_d2/(1.0-sin_theta_d2)) ); } } } block->max_entry_speed = vmax_junction; // Initialize block entry speed. Compute based on deceleration to user-defined MINIMUM_PLANNER_SPEED. double v_allowable = max_allowable_speed(-config.acceleration,MINIMUM_PLANNER_SPEED,block->millimeters); block->entry_speed = min(vmax_junction, v_allowable); // Initialize planner efficiency flags // Set flag if block will always reach maximum junction speed regardless of entry/exit speeds. // If a block can de/ac-celerate from nominal speed to zero within the length of the block, then // the current block and next block junction speeds are guaranteed to always be at their maximum // junction speeds in deceleration and acceleration, respectively. This is due to how the current // block nominal speed limits both the current and next maximum junction speeds. Hence, in both // the reverse and forward planners, the corresponding block junction speed will always be at the // the maximum junction speed and may always be ignored for any speed reduction checks. if (block->nominal_speed <= v_allowable) { block->nominal_length_flag = true; } else { block->nominal_length_flag = false; } block->recalculate_flag = true; // Always calculate trapezoid for new block // Update previous path unit_vector and nominal speed memcpy(previous_unit_vec, unit_vec, sizeof(unit_vec)); // previous_unit_vec[] = unit_vec[] previous_nominal_speed = block->nominal_speed; } else { // Acceleration planner disabled. Set minimum that is required. // block->entry_speed = block->nominal_speed; block->initial_rate = block->nominal_rate; block->final_rate = block->nominal_rate; block->accelerate_until = 0; block->decelerate_after = block->step_event_count; block->rate_delta = 0; } if (pAction->ActionType == AT_MOVE) block->check_endstops = false; else block->check_endstops = true; pAction->ActionType = AT_MOVE; // Move buffer head block_buffer_head = next_buffer_head; // Update position memcpy(position, target, sizeof(target)); // position[] = target[] startpoint = pAction->target; if (acceleration_manager_enabled) { planner_recalculate(); } st_wake_up(); }
// Add a new linear movement to the buffer. x, y and z is the signed, absolute target position in // millimeters. Feed rate specifies the speed of the motion. If feed rate is inverted, the feed // rate is taken to mean "frequency" and would complete the operation in 1/feed_rate minutes. // All position data passed to the planner must be in terms of machine position to keep the planner // independent of any coordinate system changes and offsets, which are handled by the g-code parser. // NOTE: Assumes buffer is available. Buffer checks are handled at a higher level by motion_control. void plan_buffer_line(double x, double y, double z, double c, double feed_rate, uint8_t invert_feed_rate) { // Prepare to set up new block block_t *block = &block_buffer[block_buffer_head]; // Calculate target position in absolute steps int32_t target[4]; target[X_AXIS] = lround(x*settings.steps_per_mm[X_AXIS]); target[Y_AXIS] = lround(y*settings.steps_per_mm[Y_AXIS]); target[Z_AXIS] = lround(z*settings.steps_per_mm[Z_AXIS]); target[C_AXIS] = lround(c*settings.steps_per_mm[C_AXIS]); // Compute direction bits for this block block->direction_bits = 0; if (target[X_AXIS] < pl.position[X_AXIS]) { block->direction_bits |= (1<<X_DIRECTION_BIT); } if (target[Y_AXIS] < pl.position[Y_AXIS]) { block->direction_bits |= (1<<Y_DIRECTION_BIT); } if (target[Z_AXIS] < pl.position[Z_AXIS]) { block->direction_bits |= (1<<Z_DIRECTION_BIT); } if (target[C_AXIS] < pl.position[C_AXIS]) { block->direction_bits |= (1<<C_DIRECTION_BIT); } // Number of steps for each axis block->steps_x = labs(target[X_AXIS]-pl.position[X_AXIS]); block->steps_y = labs(target[Y_AXIS]-pl.position[Y_AXIS]); block->steps_z = labs(target[Z_AXIS]-pl.position[Z_AXIS]); block->steps_c = labs(target[C_AXIS]-pl.position[C_AXIS]); block->step_event_count = max(block->steps_x, max(block->steps_y, max(block->steps_z, block->steps_c))); // Bail if this is a zero-length block if (block->step_event_count == 0) { return; }; // Compute path vector in terms of absolute step target and current positions double delta_mm[4]; delta_mm[X_AXIS] = (target[X_AXIS]-pl.position[X_AXIS])/settings.steps_per_mm[X_AXIS]; delta_mm[Y_AXIS] = (target[Y_AXIS]-pl.position[Y_AXIS])/settings.steps_per_mm[Y_AXIS]; delta_mm[Z_AXIS] = (target[Z_AXIS]-pl.position[Z_AXIS])/settings.steps_per_mm[Z_AXIS]; delta_mm[C_AXIS] = (target[C_AXIS]-pl.position[C_AXIS])/settings.steps_per_mm[C_AXIS]; block->millimeters = sqrt(delta_mm[X_AXIS]*delta_mm[X_AXIS] + delta_mm[Y_AXIS]*delta_mm[Y_AXIS] + delta_mm[Z_AXIS]*delta_mm[Z_AXIS] + delta_mm[C_AXIS]*delta_mm[C_AXIS]); double inverse_millimeters = 1.0/block->millimeters; // Inverse millimeters to remove multiple divides // Calculate speed in mm/minute for each axis. No divide by zero due to previous checks. // NOTE: Minimum stepper speed is limited by MINIMUM_STEPS_PER_MINUTE in stepper.c double inverse_minute; if (!invert_feed_rate) { inverse_minute = feed_rate * inverse_millimeters; } else { inverse_minute = 1.0 / feed_rate; } block->nominal_speed = block->millimeters * inverse_minute; // (mm/min) Always > 0 block->nominal_rate = ceil(block->step_event_count * inverse_minute); // (step/min) Always > 0 // Compute the acceleration rate for the trapezoid generator. Depending on the slope of the line // average travel per step event changes. For a line along one axis the travel per step event // is equal to the travel/step in the particular axis. For a 45 degree line the steppers of both // axes might step for every step event. Travel per step event is then sqrt(travel_x^2+travel_y^2). // To generate trapezoids with contant acceleration between blocks the rate_delta must be computed // specifically for each line to compensate for this phenomenon: // Convert universal acceleration for direction-dependent stepper rate change parameter block->rate_delta = ceil( block->step_event_count*inverse_millimeters * settings.acceleration / (60 * ACCELERATION_TICKS_PER_SECOND )); // (step/min/acceleration_tick) // Compute path unit vector double unit_vec[4]; unit_vec[X_AXIS] = delta_mm[X_AXIS]*inverse_millimeters; unit_vec[Y_AXIS] = delta_mm[Y_AXIS]*inverse_millimeters; unit_vec[Z_AXIS] = delta_mm[Z_AXIS]*inverse_millimeters; unit_vec[C_AXIS] = delta_mm[C_AXIS]*inverse_millimeters; // Compute maximum allowable entry speed at junction by centripetal acceleration approximation. // Let a circle be tangent to both previous and current path line segments, where the junction // deviation is defined as the distance from the junction to the closest edge of the circle, // colinear with the circle center. The circular segment joining the two paths represents the // path of centripetal acceleration. Solve for max velocity based on max acceleration about the // radius of the circle, defined indirectly by junction deviation. This may be also viewed as // path width or max_jerk in the previous grbl version. This approach does not actually deviate // from path, but used as a robust way to compute cornering speeds, as it takes into account the // nonlinearities of both the junction angle and junction velocity. double vmax_junction = MINIMUM_PLANNER_SPEED; // Set default max junction speed // Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles. if ((block_buffer_head != block_buffer_tail) && (pl.previous_nominal_speed > 0.0)) { // Compute cosine of angle between previous and current path. (prev_unit_vec is negative) // NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity. double cos_theta = - pl.previous_unit_vec[X_AXIS] * unit_vec[X_AXIS] - pl.previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS] - pl.previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] - pl.previous_unit_vec[C_AXIS] * unit_vec[C_AXIS] ; // Skip and use default max junction speed for 0 degree acute junction. if (cos_theta < 0.95) { vmax_junction = min(pl.previous_nominal_speed,block->nominal_speed); // 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 double sin_theta_d2 = sqrt(0.5*(1.0-cos_theta)); // Trig half angle identity. Always positive. vmax_junction = min(vmax_junction, sqrt(settings.acceleration * settings.junction_deviation * sin_theta_d2/(1.0-sin_theta_d2)) ); } } } block->max_entry_speed = vmax_junction; // Initialize block entry speed. Compute based on deceleration to user-defined MINIMUM_PLANNER_SPEED. double v_allowable = max_allowable_speed(-settings.acceleration,MINIMUM_PLANNER_SPEED,block->millimeters); block->entry_speed = min(vmax_junction, v_allowable); // Initialize planner efficiency flags // Set flag if block will always reach maximum junction speed regardless of entry/exit speeds. // If a block can de/ac-celerate from nominal speed to zero within the length of the block, then // the current block and next block junction speeds are guaranteed to always be at their maximum // junction speeds in deceleration and acceleration, respectively. This is due to how the current // block nominal speed limits both the current and next maximum junction speeds. Hence, in both // the reverse and forward planners, the corresponding block junction speed will always be at the // the maximum junction speed and may always be ignored for any speed reduction checks. if (block->nominal_speed <= v_allowable) { block->nominal_length_flag = true; } else { block->nominal_length_flag = false; } block->recalculate_flag = true; // Always calculate trapezoid for new block // Update previous path unit_vector and nominal speed memcpy(pl.previous_unit_vec, unit_vec, sizeof(unit_vec)); // pl.previous_unit_vec[] = unit_vec[] pl.previous_nominal_speed = block->nominal_speed; // Update buffer head and next buffer head indices block_buffer_head = next_buffer_head; next_buffer_head = next_block_index(block_buffer_head); // Update planner position memcpy(pl.position, target, sizeof(target)); // pl.position[] = target[] planner_recalculate(); }
void plan_reset_buffer() { block_buffer_tail = block_buffer_head; next_buffer_head = next_block_index(block_buffer_head); }
void check_axes_activity() { unsigned char axis_active[NUM_AXIS], tail_fan_speed = fanSpeed; #ifdef BARICUDA unsigned char tail_valve_pressure = ValvePressure, tail_e_to_p_pressure = EtoPPressure; #endif block_t *block; if (blocks_queued()) { uint8_t block_index = block_buffer_tail; tail_fan_speed = block_buffer[block_index].fan_speed; #ifdef BARICUDA block = &block_buffer[block_index]; tail_valve_pressure = block->valve_pressure; tail_e_to_p_pressure = block->e_to_p_pressure; #endif while (block_index != block_buffer_head) { block = &block_buffer[block_index]; for (int i=0; i<NUM_AXIS; i++) if (block->steps[i]) axis_active[i]++; block_index = next_block_index(block_index); } } if (DISABLE_X && !axis_active[X_AXIS]) disable_x(); if (DISABLE_Y && !axis_active[Y_AXIS]) disable_y(); if (DISABLE_Z && !axis_active[Z_AXIS]) disable_z(); if (DISABLE_E && !axis_active[E_AXIS]) { disable_e0(); disable_e1(); disable_e2(); disable_e3(); } #if defined(FAN_PIN) && FAN_PIN > -1 // HAS_FAN #ifdef FAN_KICKSTART_TIME static unsigned long fan_kick_end; if (tail_fan_speed) { if (fan_kick_end == 0) { // Just starting up fan - run at full power. fan_kick_end = millis() + FAN_KICKSTART_TIME; tail_fan_speed = 255; } else if (fan_kick_end > millis()) // Fan still spinning up. tail_fan_speed = 255; } else { fan_kick_end = 0; } #endif//FAN_KICKSTART_TIME #ifdef FAN_SOFT_PWM fanSpeedSoftPwm = tail_fan_speed; #else analogWrite(FAN_PIN, tail_fan_speed); #endif //!FAN_SOFT_PWM #endif //FAN_PIN > -1 #ifdef AUTOTEMP getHighESpeed(); #endif #ifdef BARICUDA #if defined(HEATER_1_PIN) && HEATER_1_PIN > -1 // HAS_HEATER_1 analogWrite(HEATER_1_PIN,tail_valve_pressure); #endif #if defined(HEATER_2_PIN) && HEATER_2_PIN > -1 // HAS_HEATER_2 analogWrite(HEATER_2_PIN,tail_e_to_p_pressure); #endif #endif }
void plan_buffer_line(const float &x, const float &y, const float &z, const float &e, float feed_rate, const uint8_t &extruder) #endif //LEVEL_SENSOR { // Calculate the buffer head after we push this byte next_buffer_head = next_block_index(block_buffer_head); // If the buffer is full: good! That means we are well ahead of the robot. // Rest here until there is room in the buffer. #ifndef DOGLCD buffer_recursivity++; #endif // DOGLCD while(block_buffer_tail == next_buffer_head) { next_buffer_head = next_block_index(block_buffer_head); temp::TemperatureManager::single::instance().manageTemperatureControl(); #ifndef DOGLCD manage_inactivity(); #endif //DOGLCD lcd_update(); #ifdef DOGLCD if (stop_planner_buffer == true) { stop_planner_buffer = false; planner_buffer_stopped = true; return; } #endif // DOGLCD } #ifndef DOGLCD buffer_recursivity--; #endif // DOGLCD #ifdef LEVEL_SENSOR apply_rotation_xyz(plan_bed_level_matrix, x, y, z); #endif // LEVEL_SENSOR // The target position of the tool in absolute steps // 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 long target[4]; target[X_AXIS] = lround(x*axis_steps_per_unit[X_AXIS]); target[Y_AXIS] = lround(y*axis_steps_per_unit[Y_AXIS]); target[Z_AXIS] = lround(z*axis_steps_per_unit[Z_AXIS]); target[E_AXIS] = lround(e*axis_steps_per_unit[E_AXIS]); #ifdef PREVENT_DANGEROUS_EXTRUDE if(target[E_AXIS]!=position[E_AXIS]) { if(degHotend(active_extruder)<extrude_min_temp) { position[E_AXIS]=target[E_AXIS]; //behave as if the move really took place, but ignore E part SERIAL_ECHO_START; SERIAL_ECHOLNPGM(MSG_ERR_COLD_EXTRUDE_STOP); } #ifdef PREVENT_LENGTHY_EXTRUDE if(labs(target[E_AXIS]-position[E_AXIS])>axis_steps_per_unit[E_AXIS]*EXTRUDE_MAXLENGTH) { position[E_AXIS]=target[E_AXIS]; //behave as if the move really took place, but ignore E part SERIAL_ECHO_START; SERIAL_ECHOLNPGM(MSG_ERR_LONG_EXTRUDE_STOP); } #endif } #endif // Prepare to set up new block block_t *block = &block_buffer[block_buffer_head]; // Mark block as not busy (Not executed by the stepper interrupt) block->busy = false; // Number of steps for each axis #ifndef COREXY // default non-h-bot planning block->steps_x = labs(target[X_AXIS]-position[X_AXIS]); block->steps_y = labs(target[Y_AXIS]-position[Y_AXIS]); #else // corexy planning // these equations follow the form of the dA and dB equations on http://www.corexy.com/theory.html block->steps_x = labs((target[X_AXIS]-position[X_AXIS]) + (target[Y_AXIS]-position[Y_AXIS])); block->steps_y = labs((target[X_AXIS]-position[X_AXIS]) - (target[Y_AXIS]-position[Y_AXIS])); #endif block->steps_z = labs(target[Z_AXIS]-position[Z_AXIS]); block->steps_e = labs(target[E_AXIS]-position[E_AXIS]); block->steps_e *= volumetric_multiplier[active_extruder]; block->steps_e *= extrudemultiply; block->steps_e /= 100; block->step_event_count = max(block->steps_x, max(block->steps_y, max(block->steps_z, block->steps_e))); // Bail if this is a zero-length block if (block->step_event_count <= dropsegments) { return; } block->fan_speed = fanSpeed; #ifdef BARICUDA block->valve_pressure = ValvePressure; block->e_to_p_pressure = EtoPPressure; #endif // Compute direction bits for this block block->direction_bits = 0; #ifndef COREXY if (target[X_AXIS] < position[X_AXIS]) { block->direction_bits |= (1<<X_AXIS); } if (target[Y_AXIS] < position[Y_AXIS]) { block->direction_bits |= (1<<Y_AXIS); } #else if (target[X_AXIS] < position[X_AXIS]) { block->direction_bits |= (1<<X_HEAD); //AlexBorro: Save the real Extruder (head) direction in X Axis } if (target[Y_AXIS] < position[Y_AXIS]) { block->direction_bits |= (1<<Y_HEAD); //AlexBorro: Save the real Extruder (head) direction in Y Axis } if ((target[X_AXIS]-position[X_AXIS]) + (target[Y_AXIS]-position[Y_AXIS]) < 0) { block->direction_bits |= (1<<X_AXIS); //AlexBorro: Motor A direction (Incorrectly implemented as X_AXIS) } if ((target[X_AXIS]-position[X_AXIS]) - (target[Y_AXIS]-position[Y_AXIS]) < 0) { block->direction_bits |= (1<<Y_AXIS); //AlexBorro: Motor B direction (Incorrectly implemented as Y_AXIS) } #endif if (target[Z_AXIS] < position[Z_AXIS]) { block->direction_bits |= (1<<Z_AXIS); } if (target[E_AXIS] < position[E_AXIS]) { block->direction_bits |= (1<<E_AXIS); } block->active_extruder = extruder; //enable active axes #ifdef COREXY if((block->steps_x != 0) || (block->steps_y != 0)) { enable_x(); enable_y(); } #else if(block->steps_x != 0) enable_x(); if(block->steps_y != 0) enable_y(); #endif #ifndef Z_LATE_ENABLE if(block->steps_z != 0) enable_z(); #endif // Enable extruder(s) if(block->steps_e != 0) { if (DISABLE_INACTIVE_EXTRUDER) //enable only selected extruder { if(g_uc_extruder_last_move[0] > 0) g_uc_extruder_last_move[0]--; if(g_uc_extruder_last_move[1] > 0) g_uc_extruder_last_move[1]--; if(g_uc_extruder_last_move[2] > 0) g_uc_extruder_last_move[2]--; if(g_uc_extruder_last_move[3] > 0) g_uc_extruder_last_move[3]--; switch(extruder) { case 0: enable_e0(); g_uc_extruder_last_move[0] = BLOCK_BUFFER_SIZE*2; if(g_uc_extruder_last_move[1] == 0) disable_e1(); if(g_uc_extruder_last_move[2] == 0) disable_e2(); if(g_uc_extruder_last_move[3] == 0) disable_e3(); break; case 1: enable_e1(); g_uc_extruder_last_move[1] = BLOCK_BUFFER_SIZE*2; if(g_uc_extruder_last_move[0] == 0) disable_e0(); if(g_uc_extruder_last_move[2] == 0) disable_e2(); if(g_uc_extruder_last_move[3] == 0) disable_e3(); break; case 2: enable_e2(); g_uc_extruder_last_move[2] = BLOCK_BUFFER_SIZE*2; if(g_uc_extruder_last_move[0] == 0) disable_e0(); if(g_uc_extruder_last_move[1] == 0) disable_e1(); if(g_uc_extruder_last_move[3] == 0) disable_e3(); break; case 3: enable_e3(); g_uc_extruder_last_move[3] = BLOCK_BUFFER_SIZE*2; if(g_uc_extruder_last_move[0] == 0) disable_e0(); if(g_uc_extruder_last_move[1] == 0) disable_e1(); if(g_uc_extruder_last_move[2] == 0) disable_e2(); break; } } else //enable all { enable_e0(); enable_e1(); enable_e2(); enable_e3(); } } if (block->steps_e == 0) { if(feed_rate<mintravelfeedrate) feed_rate=mintravelfeedrate; } else { if(feed_rate<minimumfeedrate) feed_rate=minimumfeedrate; } /* This part of the code calculates the total length of the movement. For cartesian bots, the X_AXIS is the real X movement and same for Y_AXIS. But for corexy bots, that is not true. The "X_AXIS" and "Y_AXIS" motors (that should be named to A_AXIS and B_AXIS) cannot be used for X and Y length, because A=X+Y and B=X-Y. So we need to create other 2 "AXIS", named X_HEAD and Y_HEAD, meaning the real displacement of the Head. Having the real displacement of the head, we can calculate the total movement length and apply the desired speed. */ #ifndef COREXY float delta_mm[4]; delta_mm[X_AXIS] = (target[X_AXIS]-position[X_AXIS])/axis_steps_per_unit[X_AXIS]; delta_mm[Y_AXIS] = (target[Y_AXIS]-position[Y_AXIS])/axis_steps_per_unit[Y_AXIS]; #else float delta_mm[6]; delta_mm[X_HEAD] = (target[X_AXIS]-position[X_AXIS])/axis_steps_per_unit[X_AXIS]; delta_mm[Y_HEAD] = (target[Y_AXIS]-position[Y_AXIS])/axis_steps_per_unit[Y_AXIS]; delta_mm[X_AXIS] = ((target[X_AXIS]-position[X_AXIS]) + (target[Y_AXIS]-position[Y_AXIS]))/axis_steps_per_unit[X_AXIS]; delta_mm[Y_AXIS] = ((target[X_AXIS]-position[X_AXIS]) - (target[Y_AXIS]-position[Y_AXIS]))/axis_steps_per_unit[Y_AXIS]; #endif delta_mm[Z_AXIS] = (target[Z_AXIS]-position[Z_AXIS])/axis_steps_per_unit[Z_AXIS]; delta_mm[E_AXIS] = ((target[E_AXIS]-position[E_AXIS])/axis_steps_per_unit[E_AXIS])*volumetric_multiplier[active_extruder]*extrudemultiply/100.0; if ( block->steps_x <=dropsegments && block->steps_y <=dropsegments && block->steps_z <=dropsegments ) { block->millimeters = fabs(delta_mm[E_AXIS]); } else { #ifndef COREXY block->millimeters = sqrt(square(delta_mm[X_AXIS]) + square(delta_mm[Y_AXIS]) + square(delta_mm[Z_AXIS])); #else block->millimeters = sqrt(square(delta_mm[X_HEAD]) + square(delta_mm[Y_HEAD]) + square(delta_mm[Z_AXIS])); #endif } float inverse_millimeters = 1.0/block->millimeters; // Inverse millimeters to remove multiple divides // Calculate speed in mm/second for each axis. No divide by zero due to previous checks. float inverse_second = feed_rate * inverse_millimeters; int moves_queued=(block_buffer_head-block_buffer_tail + BLOCK_BUFFER_SIZE) & (BLOCK_BUFFER_SIZE - 1); // slow down when de buffer starts to empty, rather than wait at the corner for a buffer refill #ifdef OLD_SLOWDOWN if(moves_queued < (BLOCK_BUFFER_SIZE * 0.5) && moves_queued > 1) feed_rate = feed_rate*moves_queued / (BLOCK_BUFFER_SIZE * 0.5); #endif #ifdef SLOWDOWN // segment time im micro seconds unsigned long segment_time = lround(1000000.0/inverse_second); if ((moves_queued > 1) && (moves_queued < (BLOCK_BUFFER_SIZE * 0.5))) { if (segment_time < minsegmenttime) { // buffer is draining, add extra time. The amount of time added increases if the buffer is still emptied more. inverse_second=1000000.0/(segment_time+lround(2*(minsegmenttime-segment_time)/moves_queued)); #ifdef XY_FREQUENCY_LIMIT segment_time = lround(1000000.0/inverse_second); #endif } } #endif // END OF SLOW DOWN SECTION block->nominal_speed = block->millimeters * inverse_second; // (mm/sec) Always > 0 block->nominal_rate = ceil(block->step_event_count * inverse_second); // (step/sec) Always > 0 #ifdef FILAMENT_SENSOR //FMM update ring buffer used for delay with filament measurements if((extruder==FILAMENT_SENSOR_EXTRUDER_NUM) && (delay_index2 > -1)) //only for extruder with filament sensor and if ring buffer is initialized { delay_dist = delay_dist + delta_mm[E_AXIS]; //increment counter with next move in e axis while (delay_dist >= (10*(MAX_MEASUREMENT_DELAY+1))) //check if counter is over max buffer size in mm delay_dist = delay_dist - 10*(MAX_MEASUREMENT_DELAY+1); //loop around the buffer while (delay_dist<0) delay_dist = delay_dist + 10*(MAX_MEASUREMENT_DELAY+1); //loop around the buffer delay_index1=delay_dist/10.0; //calculate index //ensure the number is within range of the array after converting from floating point if(delay_index1<0) delay_index1=0; else if (delay_index1>MAX_MEASUREMENT_DELAY) delay_index1=MAX_MEASUREMENT_DELAY; if(delay_index1 != delay_index2) //moved index { meas_sample=widthFil_to_size_ratio()-100; //subtract off 100 to reduce magnitude - to store in a signed char } while( delay_index1 != delay_index2) { delay_index2 = delay_index2 + 1; if(delay_index2>MAX_MEASUREMENT_DELAY) delay_index2=delay_index2-(MAX_MEASUREMENT_DELAY+1); //loop around buffer when incrementing if(delay_index2<0) delay_index2=0; else if (delay_index2>MAX_MEASUREMENT_DELAY) delay_index2=MAX_MEASUREMENT_DELAY; measurement_delay[delay_index2]=meas_sample; } } #endif // Calculate and limit speed in mm/sec for each axis float current_speed[4]; float speed_factor = 1.0; //factor <=1 do decrease speed for(int i=0; i < 4; i++) { current_speed[i] = delta_mm[i] * inverse_second; if(fabs(current_speed[i]) > max_feedrate[i]) speed_factor = min(speed_factor, max_feedrate[i] / fabs(current_speed[i])); } // Max segement time in us. #ifdef XY_FREQUENCY_LIMIT #define MAX_FREQ_TIME (1000000.0/XY_FREQUENCY_LIMIT) // Check and limit the xy direction change frequency unsigned char direction_change = block->direction_bits ^ old_direction_bits; old_direction_bits = block->direction_bits; segment_time = lround((float)segment_time / speed_factor); if((direction_change & (1<<X_AXIS)) == 0) { x_segment_time[0] += segment_time; } else { x_segment_time[2] = x_segment_time[1]; x_segment_time[1] = x_segment_time[0]; x_segment_time[0] = segment_time; } if((direction_change & (1<<Y_AXIS)) == 0) { y_segment_time[0] += segment_time; } else { y_segment_time[2] = y_segment_time[1]; y_segment_time[1] = y_segment_time[0]; y_segment_time[0] = segment_time; } long max_x_segment_time = max(x_segment_time[0], max(x_segment_time[1], x_segment_time[2])); long max_y_segment_time = max(y_segment_time[0], max(y_segment_time[1], y_segment_time[2])); long min_xy_segment_time =min(max_x_segment_time, max_y_segment_time); if(min_xy_segment_time < MAX_FREQ_TIME) speed_factor = min(speed_factor, speed_factor * (float)min_xy_segment_time / (float)MAX_FREQ_TIME); #endif // XY_FREQUENCY_LIMIT // Correct the speed if (speed_factor < 1.0) { for (unsigned char i = 0; i < NUM_AXIS; i++) current_speed[i] *= speed_factor; block->nominal_speed *= speed_factor; block->nominal_rate *= speed_factor; } // Compute and limit the acceleration rate for the trapezoid generator. float steps_per_mm = block->step_event_count / block->millimeters; long bsx = block->steps_x, bsy = block->steps_y, bsz = block->steps_z, bse = block->steps_e; if (bsx == 0 && bsy == 0 && bsz == 0) { block->acceleration_st = ceil(retract_acceleration * steps_per_mm); // convert to: acceleration steps/sec^2 } else if (bse == 0) { block->acceleration_st = ceil(travel_acceleration * steps_per_mm); // convert to: acceleration steps/sec^2 } else { block->acceleration_st = ceil(acceleration * steps_per_mm); // convert to: acceleration steps/sec^2 } // Limit acceleration per axis unsigned long acc_st = block->acceleration_st, xsteps = axis_steps_per_sqr_second[X_AXIS], ysteps = axis_steps_per_sqr_second[Y_AXIS], zsteps = axis_steps_per_sqr_second[Z_AXIS], esteps = axis_steps_per_sqr_second[E_AXIS]; if ((float)acc_st * bsx / block->step_event_count > xsteps) acc_st = xsteps; if ((float)acc_st * bsy / block->step_event_count > ysteps) acc_st = ysteps; if ((float)acc_st * bsz / block->step_event_count > zsteps) acc_st = zsteps; if ((float)acc_st * bse / block->step_event_count > esteps) acc_st = esteps; block->acceleration_st = acc_st; block->acceleration = acc_st / steps_per_mm; block->acceleration_rate = (long)(acc_st * 16777216.0 / (F_CPU / 8.0)); #if 0 // Use old jerk for now // Compute path unit vector double unit_vec[3]; unit_vec[X_AXIS] = delta_mm[X_AXIS]*inverse_millimeters; unit_vec[Y_AXIS] = delta_mm[Y_AXIS]*inverse_millimeters; unit_vec[Z_AXIS] = delta_mm[Z_AXIS]*inverse_millimeters; // Compute maximum allowable entry speed at junction by centripetal acceleration approximation. // Let a circle be tangent to both previous and current path line segments, where the junction // deviation is defined as the distance from the junction to the closest edge of the circle, // colinear with the circle center. The circular segment joining the two paths represents the // path of centripetal acceleration. Solve for max velocity based on max acceleration about the // radius of the circle, defined indirectly by junction deviation. This may be also viewed as // path width or max_jerk in the previous grbl version. This approach does not actually deviate // from path, but used as a robust way to compute cornering speeds, as it takes into account the // nonlinearities of both the junction angle and junction velocity. double vmax_junction = MINIMUM_PLANNER_SPEED; // Set default max junction speed // Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles. if ((block_buffer_head != block_buffer_tail) && (previous_nominal_speed > 0.0)) { // Compute cosine of angle between previous and current path. (prev_unit_vec is negative) // NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity. double cos_theta = - previous_unit_vec[X_AXIS] * unit_vec[X_AXIS] - previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS] - previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ; // Skip and use default max junction speed for 0 degree acute junction. if (cos_theta < 0.95) { vmax_junction = min(previous_nominal_speed,block->nominal_speed); // 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 double sin_theta_d2 = sqrt(0.5*(1.0-cos_theta)); // Trig half angle identity. Always positive. vmax_junction = min(vmax_junction, sqrt(block->acceleration * junction_deviation * sin_theta_d2/(1.0-sin_theta_d2)) ); } } } #endif // Start with a safe speed float vmax_junction = max_xy_jerk / 2; float vmax_junction_factor = 1.0; float mz2 = max_z_jerk / 2, me2 = max_e_jerk / 2; float csz = current_speed[Z_AXIS], cse = current_speed[E_AXIS]; if (fabs(csz) > mz2) vmax_junction = min(vmax_junction, mz2); if (fabs(cse) > me2) vmax_junction = min(vmax_junction, me2); vmax_junction = min(vmax_junction, block->nominal_speed); float safe_speed = vmax_junction; if ((moves_queued > 1) && (previous_nominal_speed > 0.0001)) { float dx = current_speed[X_AXIS] - previous_speed[X_AXIS], dy = current_speed[Y_AXIS] - previous_speed[Y_AXIS], dz = fabs(csz - previous_speed[Z_AXIS]), de = fabs(cse - previous_speed[E_AXIS]), jerk = sqrt(dx * dx + dy * dy); // if ((fabs(previous_speed[X_AXIS]) > 0.0001) || (fabs(previous_speed[Y_AXIS]) > 0.0001)) { vmax_junction = block->nominal_speed; // } if (jerk > max_xy_jerk) vmax_junction_factor = max_xy_jerk / jerk; if (dz > max_z_jerk) vmax_junction_factor = min(vmax_junction_factor, max_z_jerk / dz); if (de > max_e_jerk) vmax_junction_factor = min(vmax_junction_factor, max_e_jerk / de); vmax_junction = min(previous_nominal_speed, vmax_junction * vmax_junction_factor); // Limit speed to max previous speed } block->max_entry_speed = vmax_junction; // Initialize block entry speed. Compute based on deceleration to user-defined MINIMUM_PLANNER_SPEED. double v_allowable = max_allowable_speed(-block->acceleration, MINIMUM_PLANNER_SPEED, block->millimeters); block->entry_speed = min(vmax_junction, v_allowable); // Initialize planner efficiency flags // Set flag if block will always reach maximum junction speed regardless of entry/exit speeds. // If a block can de/ac-celerate from nominal speed to zero within the length of the block, then // the current block and next block junction speeds are guaranteed to always be at their maximum // junction speeds in deceleration and acceleration, respectively. This is due to how the current // block nominal speed limits both the current and next maximum junction speeds. Hence, in both // the reverse and forward planners, the corresponding block junction speed will always be at the // the maximum junction speed and may always be ignored for any speed reduction checks. block->nominal_length_flag = (block->nominal_speed <= v_allowable); block->recalculate_flag = true; // Always calculate trapezoid for new block // Update previous path unit_vector and nominal speed for (int i = 0; i < NUM_AXIS; i++) previous_speed[i] = current_speed[i]; previous_nominal_speed = block->nominal_speed; #ifdef ADVANCE // Calculate advance rate if((block->steps_e == 0) || (block->steps_x == 0 && block->steps_y == 0 && block->steps_z == 0)) { block->advance_rate = 0; block->advance = 0; } else { long acc_dist = estimate_acceleration_distance(0, block->nominal_rate, block->acceleration_st); float advance = (STEPS_PER_CUBIC_MM_E * EXTRUDER_ADVANCE_K) * (current_speed[E_AXIS] * current_speed[E_AXIS] * EXTRUSION_AREA * EXTRUSION_AREA)*256; block->advance = advance; if(acc_dist == 0) { block->advance_rate = 0; } else { block->advance_rate = advance / (float)acc_dist; } } /* SERIAL_ECHO_START; SERIAL_ECHOPGM("advance :"); SERIAL_ECHO(block->advance/256.0); SERIAL_ECHOPGM("advance rate :"); SERIAL_ECHOLN(block->advance_rate/256.0); */ #endif // ADVANCE calculate_trapezoid_for_block(block, block->entry_speed/block->nominal_speed, safe_speed/block->nominal_speed); // Move buffer head block_buffer_head = next_buffer_head; // Update position for (int i = 0; i < NUM_AXIS; i++) position[i] = target[i]; planner_recalculate(); st_wake_up(); }
void Planner::recalculate() { // Initialize block index to the last block in the planner buffer. std::size_t block_index = prev_block_index(block_buffer_head); // Bail. Can't do anything with one only one plan-able block. if (block_index == block_buffer_planned) { return; } // Reverse Pass: Coarsely maximize all possible deceleration curves back-planning from the last // block in buffer. Cease planning when the last optimal planned or tail pointer is reached. // NOTE: Forward pass will later refine and correct the reverse pass to create an optimal plan. float entry_speed_sqr; PlanBlock *next; PlanBlock *current = &block_buffer[block_index]; // Calculate maximum entry speed for last block in buffer, where the exit speed is always zero. current->entry_speed_sqr = std::min(current->max_entry_speed_sqr, current->max_change_speed_sqr); block_index = prev_block_index(block_index); if (block_index == block_buffer_planned) { // Only two plannable blocks in buffer. Reverse pass complete. // Check if the first block is the tail. If so, notify stepper to update its current parameters. if (block_index == block_buffer_tail) { // stepper->update_plan_block_parameters(); } } else { // Three or more plan-able blocks while (block_index != block_buffer_planned) { next = current; current = &block_buffer[block_index]; block_index = prev_block_index(block_index); // Check if next block is the tail block(=planned block). If so, update current stepper parameters. if (block_index == block_buffer_tail) { // stepper->update_plan_block_parameters(); } // Compute maximum entry speed decelerating over the current block from its exit speed. if (current->entry_speed_sqr != current->max_entry_speed_sqr) { entry_speed_sqr = next->entry_speed_sqr + current->max_change_speed_sqr; if (entry_speed_sqr < current->max_entry_speed_sqr) { current->entry_speed_sqr = entry_speed_sqr; } else { current->entry_speed_sqr = current->max_entry_speed_sqr; } } } } // Forward Pass: Forward plan the acceleration curve from the planned pointer onward. // Also scans for optimal plan breakpoints and appropriately updates the planned pointer. next = &block_buffer[block_buffer_planned]; // Begin at buffer planned pointer block_index = next_block_index(block_buffer_planned); while (block_index != block_buffer_head) { current = next; next = &block_buffer[block_index]; // Any acceleration detected in the forward pass automatically moves the optimal planned // pointer forward, since everything before this is all optimal. In other words, nothing // can improve the plan from the buffer tail to the planned pointer by logic. if (current->entry_speed_sqr < next->entry_speed_sqr) { entry_speed_sqr = current->entry_speed_sqr + current->max_change_speed_sqr; // If true, current block is full-acceleration and we can move the planned pointer forward. if (entry_speed_sqr < next->entry_speed_sqr) { next->entry_speed_sqr = entry_speed_sqr; // Always <= max_entry_speed_sqr. Backward pass sets this. block_buffer_planned = block_index; // Set optimal plan pointer. } } // Any block set at its maximum entry speed also creates an optimal plan up to this // point in the buffer. When the plan is bracketed by either the beginning of the // buffer and a maximum entry speed or two maximum entry speeds, every block in between // cannot logically be further improved. Hence, we don't have to recompute them anymore. if (next->entry_speed_sqr == next->max_entry_speed_sqr) { block_buffer_planned = block_index; } block_index = next_block_index( block_index ); } }
// planner, called whenever a new block was added // All planner computations are performed with doubles (float on Arduinos) to minimize numerical round- // off errors. Only when planned values are converted to stepper rate parameters, these are integers. inline static void planner_recalculate() { //// reverse pass // Recalculate entry_speed to be (a) less or equal to vmax_junction and // (b) low enough so it can definitely reach the next entry_speed at fixed acceleration. int8_t block_index = block_buffer_head; block_t *previous = NULL; // block closer to tail (older) block_t *current = NULL; // block who's entry_speed to be adjusted block_t *next = NULL; // block closer to head (newer) while(block_index != block_buffer_tail) { block_index = prev_block_index( block_index ); next = current; current = previous; previous = &block_buffer[block_index]; if (current && next) { reduce_entry_speed_reverse(current, next); } } // skip tail/first block //// forward pass // Recalculate entry_speed to be low enough it can definitely // be reached from previous entry_speed at fixed acceleration. block_index = block_buffer_tail; previous = NULL; // block closer to tail (older) current = NULL; // block who's entry_speed to be adjusted next = NULL; // block closer to head (newer) while(block_index != block_buffer_head) { previous = current; current = next; next = &block_buffer[block_index]; if (previous && current) { reduce_entry_speed_forward(previous, current); } block_index = next_block_index(block_index); } if (current && next) { reduce_entry_speed_forward(current, next); } //// recalculate trapeziods for all flagged blocks // At this point all blocks have entry_speeds that that can be (a) reached from the prevous // entry_speed with the one and only acceleration from our settings and (b) have junction // speeds that do not exceed our limits for given direction change. // Now we only need to calculate the actual accelerate_until and decelerate_after values. block_index = block_buffer_tail; current = NULL; next = NULL; while(block_index != block_buffer_head) { current = next; next = &block_buffer[block_index]; if (current) { if (current->recalculate_flag || next->recalculate_flag) { calculate_trapezoid_for_block( current, current->entry_speed/current->nominal_speed, next->entry_speed/current->nominal_speed ); current->recalculate_flag = false; } } block_index = next_block_index( block_index ); } // always recalculate last (newest) block with zero exit speed calculate_trapezoid_for_block( next, next->entry_speed/next->nominal_speed, ZERO_SPEED/next->nominal_speed ); next->recalculate_flag = false; }
// Add a new linear movement to the buffer. steps_x, _y and _z is the absolute position in // mm. Microseconds specify how many microseconds the move should take to perform. To aid acceleration // calculation the caller must also provide the physical length of the line in millimeters. void plan_buffer_line(const float &x, const float &y, const float &z, const float &e, float feed_rate, const uint8_t &extruder) { // Calculate the buffer head after we push this byte int next_buffer_head = next_block_index(block_buffer_head); // If the buffer is full: good! That means we are well ahead of the robot. // Rest here until there is room in the buffer. while(block_buffer_tail == next_buffer_head) { manage_heater(); manage_inactivity(); lcd_update(); } // The target position of the tool in absolute steps // 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 long target[4]; target[X_AXIS] = lround(x*axis_steps_per_unit[X_AXIS]); target[Y_AXIS] = lround(y*axis_steps_per_unit[Y_AXIS]); target[Z_AXIS] = lround(z*axis_steps_per_unit[Z_AXIS]); target[E_AXIS] = lround(e*axis_steps_per_unit[E_AXIS]); #ifdef PREVENT_DANGEROUS_EXTRUDE if(target[E_AXIS]!=position[E_AXIS]) { if(degHotend(active_extruder)<extrude_min_temp) { position[E_AXIS]=target[E_AXIS]; //behave as if the move really took place, but ignore E part SERIAL_ECHO_START; SERIAL_ECHOLNPGM(MSG_ERR_COLD_EXTRUDE_STOP); } #ifdef PREVENT_LENGTHY_EXTRUDE if(labs(target[E_AXIS]-position[E_AXIS])>axis_steps_per_unit[E_AXIS]*EXTRUDE_MAXLENGTH) { #ifdef EASY_LOAD if (!allow_lengthy_extrude_once) { #endif position[E_AXIS]=target[E_AXIS]; //behave as if the move really took place, but ignore E part SERIAL_ECHO_START; SERIAL_ECHOLNPGM(MSG_ERR_LONG_EXTRUDE_STOP); #ifdef EASY_LOAD } allow_lengthy_extrude_once = false; #endif } #endif // PREVENT_LENGTHY_EXTRUDE } #endif // Prepare to set up new block block_t *block = &block_buffer[block_buffer_head]; // Mark block as not busy (Not executed by the stepper interrupt) block->busy = false; // Number of steps for each axis #ifndef COREXY // default non-h-bot planning block->steps_x = labs(target[X_AXIS]-position[X_AXIS]); block->steps_y = labs(target[Y_AXIS]-position[Y_AXIS]); #else // corexy planning // these equations follow the form of the dA and dB equations on http://www.corexy.com/theory.html block->steps_x = labs((target[X_AXIS]-position[X_AXIS]) + (target[Y_AXIS]-position[Y_AXIS])); block->steps_y = labs((target[X_AXIS]-position[X_AXIS]) - (target[Y_AXIS]-position[Y_AXIS])); #endif block->steps_z = labs(target[Z_AXIS]-position[Z_AXIS]); block->steps_e = labs(target[E_AXIS]-position[E_AXIS]); block->steps_e *= extrudemultiply; block->steps_e /= 100; block->step_event_count = max(block->steps_x, max(block->steps_y, max(block->steps_z, block->steps_e))); // Bail if this is a zero-length block if (block->step_event_count <= dropsegments) { return; } block->fan_speed = fanSpeed; #ifdef BARICUDA block->valve_pressure = ValvePressure; block->e_to_p_pressure = EtoPPressure; #endif // Compute direction bits for this block block->direction_bits = 0; #ifndef COREXY if (target[X_AXIS] < position[X_AXIS]) { block->direction_bits |= (1<<X_AXIS); } if (target[Y_AXIS] < position[Y_AXIS]) { block->direction_bits |= (1<<Y_AXIS); } #else if ((target[X_AXIS]-position[X_AXIS]) + (target[Y_AXIS]-position[Y_AXIS]) < 0) { block->direction_bits |= (1<<X_AXIS); } if ((target[X_AXIS]-position[X_AXIS]) - (target[Y_AXIS]-position[Y_AXIS]) < 0) { block->direction_bits |= (1<<Y_AXIS); } #endif if (target[Z_AXIS] < position[Z_AXIS]) { block->direction_bits |= (1<<Z_AXIS); } if (target[E_AXIS] < position[E_AXIS]) { block->direction_bits |= (1<<E_AXIS); } block->active_extruder = extruder; //enable active axes #ifdef COREXY if((block->steps_x != 0) || (block->steps_y != 0)) { enable_x(); enable_y(); } #else if(block->steps_x != 0) enable_x(); if(block->steps_y != 0) enable_y(); #endif #ifndef Z_LATE_ENABLE if(block->steps_z != 0) enable_z(); #endif // Enable all if(block->steps_e != 0) { enable_e0(); enable_e1(); enable_e2(); } if (block->steps_e == 0) { if(feed_rate<mintravelfeedrate) feed_rate=mintravelfeedrate; } else { if(feed_rate<minimumfeedrate) feed_rate=minimumfeedrate; } float delta_mm[4]; #ifndef COREXY delta_mm[X_AXIS] = (target[X_AXIS]-position[X_AXIS])/axis_steps_per_unit[X_AXIS]; delta_mm[Y_AXIS] = (target[Y_AXIS]-position[Y_AXIS])/axis_steps_per_unit[Y_AXIS]; #else delta_mm[X_AXIS] = ((target[X_AXIS]-position[X_AXIS]) + (target[Y_AXIS]-position[Y_AXIS]))/axis_steps_per_unit[X_AXIS]; delta_mm[Y_AXIS] = ((target[X_AXIS]-position[X_AXIS]) - (target[Y_AXIS]-position[Y_AXIS]))/axis_steps_per_unit[Y_AXIS]; #endif delta_mm[Z_AXIS] = (target[Z_AXIS]-position[Z_AXIS])/axis_steps_per_unit[Z_AXIS]; delta_mm[E_AXIS] = ((target[E_AXIS]-position[E_AXIS])/axis_steps_per_unit[E_AXIS])*extrudemultiply/100.0; if ( block->steps_x <=dropsegments && block->steps_y <=dropsegments && block->steps_z <=dropsegments ) { block->millimeters = fabs(delta_mm[E_AXIS]); } else { block->millimeters = sqrt(square(delta_mm[X_AXIS]) + square(delta_mm[Y_AXIS]) + square(delta_mm[Z_AXIS])); } float inverse_millimeters = 1.0/block->millimeters; // Inverse millimeters to remove multiple divides // Calculate speed in mm/second for each axis. No divide by zero due to previous checks. float inverse_second = feed_rate * inverse_millimeters; int moves_queued=(block_buffer_head-block_buffer_tail + BLOCK_BUFFER_SIZE) & (BLOCK_BUFFER_SIZE - 1); // slow down when de buffer starts to empty, rather than wait at the corner for a buffer refill #ifdef OLD_SLOWDOWN if(moves_queued < (BLOCK_BUFFER_SIZE * 0.5) && moves_queued > 1) feed_rate = feed_rate*moves_queued / (BLOCK_BUFFER_SIZE * 0.5); #endif #ifdef SLOWDOWN // segment time im micro seconds unsigned long segment_time = lround(1000000.0/inverse_second); if ((moves_queued > 1) && (moves_queued < (BLOCK_BUFFER_SIZE * 0.5))) { if (segment_time < minsegmenttime) { // buffer is draining, add extra time. The amount of time added increases if the buffer is still emptied more. inverse_second=1000000.0/(segment_time+lround(2*(minsegmenttime-segment_time)/moves_queued)); #ifdef XY_FREQUENCY_LIMIT segment_time = lround(1000000.0/inverse_second); #endif } } #endif // END OF SLOW DOWN SECTION block->nominal_speed = block->millimeters * inverse_second; // (mm/sec) Always > 0 block->nominal_rate = ceil(block->step_event_count * inverse_second); // (step/sec) Always > 0 // Calculate and limit speed in mm/sec for each axis float current_speed[4]; float speed_factor = 1.0; //factor <=1 do decrease speed for(int i=0; i < 4; i++) { current_speed[i] = delta_mm[i] * inverse_second; if(fabs(current_speed[i]) > max_feedrate[i]) speed_factor = min(speed_factor, max_feedrate[i] / fabs(current_speed[i])); } // Max segement time in us. #ifdef XY_FREQUENCY_LIMIT #define MAX_FREQ_TIME (1000000.0/XY_FREQUENCY_LIMIT) // Check and limit the xy direction change frequency unsigned char direction_change = block->direction_bits ^ old_direction_bits; old_direction_bits = block->direction_bits; segment_time = lround((float)segment_time / speed_factor); if((direction_change & (1<<X_AXIS)) == 0) { x_segment_time[0] += segment_time; } else { x_segment_time[2] = x_segment_time[1]; x_segment_time[1] = x_segment_time[0]; x_segment_time[0] = segment_time; } if((direction_change & (1<<Y_AXIS)) == 0) { y_segment_time[0] += segment_time; } else { y_segment_time[2] = y_segment_time[1]; y_segment_time[1] = y_segment_time[0]; y_segment_time[0] = segment_time; } long max_x_segment_time = max(x_segment_time[0], max(x_segment_time[1], x_segment_time[2])); long max_y_segment_time = max(y_segment_time[0], max(y_segment_time[1], y_segment_time[2])); long min_xy_segment_time =min(max_x_segment_time, max_y_segment_time); if(min_xy_segment_time < MAX_FREQ_TIME) speed_factor = min(speed_factor, speed_factor * (float)min_xy_segment_time / (float)MAX_FREQ_TIME); #endif // Correct the speed if( speed_factor < 1.0) { for(unsigned char i=0; i < 4; i++) { current_speed[i] *= speed_factor; } block->nominal_speed *= speed_factor; block->nominal_rate *= speed_factor; } // Compute and limit the acceleration rate for the trapezoid generator. float steps_per_mm = block->step_event_count/block->millimeters; if(block->steps_x == 0 && block->steps_y == 0 && block->steps_z == 0) { block->acceleration_st = ceil(retract_acceleration * steps_per_mm); // convert to: acceleration steps/sec^2 } else { block->acceleration_st = ceil(acceleration * steps_per_mm); // convert to: acceleration steps/sec^2 // Limit acceleration per axis if(((float)block->acceleration_st * (float)block->steps_x / (float)block->step_event_count) > axis_steps_per_sqr_second[X_AXIS]) block->acceleration_st = axis_steps_per_sqr_second[X_AXIS]; if(((float)block->acceleration_st * (float)block->steps_y / (float)block->step_event_count) > axis_steps_per_sqr_second[Y_AXIS]) block->acceleration_st = axis_steps_per_sqr_second[Y_AXIS]; if(((float)block->acceleration_st * (float)block->steps_e / (float)block->step_event_count) > axis_steps_per_sqr_second[E_AXIS]) block->acceleration_st = axis_steps_per_sqr_second[E_AXIS]; if(((float)block->acceleration_st * (float)block->steps_z / (float)block->step_event_count ) > axis_steps_per_sqr_second[Z_AXIS]) block->acceleration_st = axis_steps_per_sqr_second[Z_AXIS]; } block->acceleration = block->acceleration_st / steps_per_mm; block->acceleration_rate = (long)((float)block->acceleration_st * (16777216.0 / (F_CPU / 8.0))); #if 0 // Use old jerk for now // Compute path unit vector double unit_vec[3]; unit_vec[X_AXIS] = delta_mm[X_AXIS]*inverse_millimeters; unit_vec[Y_AXIS] = delta_mm[Y_AXIS]*inverse_millimeters; unit_vec[Z_AXIS] = delta_mm[Z_AXIS]*inverse_millimeters; // Compute maximum allowable entry speed at junction by centripetal acceleration approximation. // Let a circle be tangent to both previous and current path line segments, where the junction // deviation is defined as the distance from the junction to the closest edge of the circle, // colinear with the circle center. The circular segment joining the two paths represents the // path of centripetal acceleration. Solve for max velocity based on max acceleration about the // radius of the circle, defined indirectly by junction deviation. This may be also viewed as // path width or max_jerk in the previous grbl version. This approach does not actually deviate // from path, but used as a robust way to compute cornering speeds, as it takes into account the // nonlinearities of both the junction angle and junction velocity. double vmax_junction = MINIMUM_PLANNER_SPEED; // Set default max junction speed // Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles. if ((block_buffer_head != block_buffer_tail) && (previous_nominal_speed > 0.0)) { // Compute cosine of angle between previous and current path. (prev_unit_vec is negative) // NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity. double cos_theta = - previous_unit_vec[X_AXIS] * unit_vec[X_AXIS] - previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS] - previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ; // Skip and use default max junction speed for 0 degree acute junction. if (cos_theta < 0.95) { vmax_junction = min(previous_nominal_speed,block->nominal_speed); // 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 double sin_theta_d2 = sqrt(0.5*(1.0-cos_theta)); // Trig half angle identity. Always positive. vmax_junction = min(vmax_junction, sqrt(block->acceleration * junction_deviation * sin_theta_d2/(1.0-sin_theta_d2)) ); } } } #endif // Start with a safe speed float vmax_junction = max_xy_jerk/2; float vmax_junction_factor = 1.0; if(fabs(current_speed[Z_AXIS]) > max_z_jerk/2) vmax_junction = min(vmax_junction, max_z_jerk/2); if(fabs(current_speed[E_AXIS]) > max_e_jerk/2) vmax_junction = min(vmax_junction, max_e_jerk/2); vmax_junction = min(vmax_junction, block->nominal_speed); float safe_speed = vmax_junction; if ((moves_queued > 1) && (previous_nominal_speed > 0.0001)) { float jerk = sqrt(pow((current_speed[X_AXIS]-previous_speed[X_AXIS]), 2)+pow((current_speed[Y_AXIS]-previous_speed[Y_AXIS]), 2)); // if((fabs(previous_speed[X_AXIS]) > 0.0001) || (fabs(previous_speed[Y_AXIS]) > 0.0001)) { vmax_junction = block->nominal_speed; // } if (jerk > max_xy_jerk) { vmax_junction_factor = (max_xy_jerk/jerk); } if(fabs(current_speed[Z_AXIS] - previous_speed[Z_AXIS]) > max_z_jerk) { vmax_junction_factor= min(vmax_junction_factor, (max_z_jerk/fabs(current_speed[Z_AXIS] - previous_speed[Z_AXIS]))); } if(fabs(current_speed[E_AXIS] - previous_speed[E_AXIS]) > max_e_jerk) { vmax_junction_factor = min(vmax_junction_factor, (max_e_jerk/fabs(current_speed[E_AXIS] - previous_speed[E_AXIS]))); } vmax_junction = min(previous_nominal_speed, vmax_junction * vmax_junction_factor); // Limit speed to max previous speed } block->max_entry_speed = vmax_junction; // Initialize block entry speed. Compute based on deceleration to user-defined MINIMUM_PLANNER_SPEED. double v_allowable = max_allowable_speed(-block->acceleration,MINIMUM_PLANNER_SPEED,block->millimeters); block->entry_speed = min(vmax_junction, v_allowable); // Initialize planner efficiency flags // Set flag if block will always reach maximum junction speed regardless of entry/exit speeds. // If a block can de/ac-celerate from nominal speed to zero within the length of the block, then // the current block and next block junction speeds are guaranteed to always be at their maximum // junction speeds in deceleration and acceleration, respectively. This is due to how the current // block nominal speed limits both the current and next maximum junction speeds. Hence, in both // the reverse and forward planners, the corresponding block junction speed will always be at the // the maximum junction speed and may always be ignored for any speed reduction checks. if (block->nominal_speed <= v_allowable) { block->nominal_length_flag = true; } else { block->nominal_length_flag = false; } block->recalculate_flag = true; // Always calculate trapezoid for new block // Update previous path unit_vector and nominal speed memcpy(previous_speed, current_speed, sizeof(previous_speed)); // previous_speed[] = current_speed[] previous_nominal_speed = block->nominal_speed; #ifdef ADVANCE // Calculate advance rate if((block->steps_e == 0) || (block->steps_x == 0 && block->steps_y == 0 && block->steps_z == 0)) { block->advance_rate = 0; block->advance = 0; } else { long acc_dist = estimate_acceleration_distance(0, block->nominal_rate, block->acceleration_st); float advance = (STEPS_PER_CUBIC_MM_E * EXTRUDER_ADVANCE_K) * (current_speed[E_AXIS] * current_speed[E_AXIS] * EXTRUTION_AREA * EXTRUTION_AREA)*256; block->advance = advance; if(acc_dist == 0) { block->advance_rate = 0; } else { block->advance_rate = advance / (float)acc_dist; } } /* SERIAL_ECHO_START; SERIAL_ECHOPGM("advance :"); SERIAL_ECHO(block->advance/256.0); SERIAL_ECHOPGM("advance rate :"); SERIAL_ECHOLN(block->advance_rate/256.0); */ #endif // ADVANCE calculate_trapezoid_for_block(block, block->entry_speed/block->nominal_speed, safe_speed/block->nominal_speed); // Move buffer head block_buffer_head = next_buffer_head; // Update position memcpy(position, target, sizeof(target)); // position[] = target[] planner_recalculate(); st_wake_up(); }
inline void planner_discard_current_block() { if (block_buffer_head != block_buffer_tail) { block_buffer_tail = next_block_index( block_buffer_tail ); } }
/** * Maintain fans, paste extruder pressure, */ void Planner::check_axes_activity() { unsigned char axis_active[NUM_AXIS] = { 0 }, tail_fan_speed[FAN_COUNT]; #if FAN_COUNT > 0 for (uint8_t i = 0; i < FAN_COUNT; i++) tail_fan_speed[i] = fanSpeeds[i]; #endif #if ENABLED(BARICUDA) unsigned char tail_valve_pressure = baricuda_valve_pressure, tail_e_to_p_pressure = baricuda_e_to_p_pressure; #endif if (blocks_queued()) { #if FAN_COUNT > 0 for (uint8_t i = 0; i < FAN_COUNT; i++) tail_fan_speed[i] = block_buffer[block_buffer_tail].fan_speed[i]; #endif block_t* block; #if ENABLED(BARICUDA) block = &block_buffer[block_buffer_tail]; tail_valve_pressure = block->valve_pressure; tail_e_to_p_pressure = block->e_to_p_pressure; #endif for (uint8_t b = block_buffer_tail; b != block_buffer_head; b = next_block_index(b)) { block = &block_buffer[b]; for (int i = 0; i < NUM_AXIS; i++) if (block->steps[i]) axis_active[i]++; } } #if ENABLED(DISABLE_X) if (!axis_active[X_AXIS]) disable_x(); #endif #if ENABLED(DISABLE_Y) if (!axis_active[Y_AXIS]) disable_y(); #endif #if ENABLED(DISABLE_Z) if (!axis_active[Z_AXIS]) disable_z(); #endif #if ENABLED(DISABLE_E) if (!axis_active[E_AXIS]) { disable_e0(); disable_e1(); disable_e2(); disable_e3(); } #endif #if FAN_COUNT > 0 #if defined(FAN_MIN_PWM) #define CALC_FAN_SPEED(f) (tail_fan_speed[f] ? ( FAN_MIN_PWM + (tail_fan_speed[f] * (255 - FAN_MIN_PWM)) / 255 ) : 0) #else #define CALC_FAN_SPEED(f) tail_fan_speed[f] #endif #ifdef FAN_KICKSTART_TIME static millis_t fan_kick_end[FAN_COUNT] = { 0 }; #define KICKSTART_FAN(f) \ if (tail_fan_speed[f]) { \ millis_t ms = millis(); \ if (fan_kick_end[f] == 0) { \ fan_kick_end[f] = ms + FAN_KICKSTART_TIME; \ tail_fan_speed[f] = 255; \ } else { \ if (PENDING(ms, fan_kick_end[f])) { \ tail_fan_speed[f] = 255; \ } \ } \ } else { \ fan_kick_end[f] = 0; \ } #if HAS_FAN0 KICKSTART_FAN(0); #endif #if HAS_FAN1 KICKSTART_FAN(1); #endif #if HAS_FAN2 KICKSTART_FAN(2); #endif #endif //FAN_KICKSTART_TIME #if ENABLED(FAN_SOFT_PWM) #if HAS_FAN0 thermalManager.fanSpeedSoftPwm[0] = CALC_FAN_SPEED(0); #endif #if HAS_FAN1 thermalManager.fanSpeedSoftPwm[1] = CALC_FAN_SPEED(1); #endif #if HAS_FAN2 thermalManager.fanSpeedSoftPwm[2] = CALC_FAN_SPEED(2); #endif #else #if HAS_FAN0 analogWrite(FAN_PIN, CALC_FAN_SPEED(0)); #endif #if HAS_FAN1 analogWrite(FAN1_PIN, CALC_FAN_SPEED(1)); #endif #if HAS_FAN2 analogWrite(FAN2_PIN, CALC_FAN_SPEED(2)); #endif #endif #endif // FAN_COUNT > 0 #if ENABLED(AUTOTEMP) getHighESpeed(); #endif #if ENABLED(BARICUDA) #if HAS_HEATER_1 analogWrite(HEATER_1_PIN, tail_valve_pressure); #endif #if HAS_HEATER_2 analogWrite(HEATER_2_PIN, tail_e_to_p_pressure); #endif #endif }
// Add a new linear movement to the buffer. x, y and z is // the signed, absolute target position in millimeters. Feed rate specifies the speed of the motion. inline void planner_line(double x, double y, double z, double feed_rate, uint8_t nominal_laser_intensity, double pixel_width) { // calculate target position in absolute steps int32_t target[3]; target[X_AXIS] = lround(x*CONFIG_X_STEPS_PER_MM); target[Y_AXIS] = lround(y*CONFIG_Y_STEPS_PER_MM); target[Z_AXIS] = lround(z*CONFIG_Z_STEPS_PER_MM); // calculate the buffer head and check for space int next_buffer_head = next_block_index( block_buffer_head ); while(block_buffer_tail == next_buffer_head) { // buffer full condition // good! We are well ahead of the robot. Rest here until buffer has room. // sleep_mode(); protocol_idle(); } // prepare to set up new block block_t *block = &block_buffer[block_buffer_head]; // set block type to line command if (pixel_width != 0.0) { block->type = TYPE_RASTER_LINE; block->pixel_steps_x1024 = lround(pixel_width*CONFIG_X_STEPS_PER_MM*1024); } else { block->type = TYPE_LINE; } // set nominal laser intensity block->nominal_laser_intensity = nominal_laser_intensity; // compute direction bits for this block block->direction_bits = 0; if (target[X_AXIS] < position[X_AXIS]) { block->direction_bits |= (1<<X_DIRECTION_BIT); } if (target[Y_AXIS] < position[Y_AXIS]) { block->direction_bits |= (1<<Y_DIRECTION_BIT); } if (target[Z_AXIS] < position[Z_AXIS]) { block->direction_bits |= (1<<Z_DIRECTION_BIT); } // number of steps for each axis block->steps_x = labs(target[X_AXIS]-position[X_AXIS]); block->steps_y = labs(target[Y_AXIS]-position[Y_AXIS]); block->steps_z = labs(target[Z_AXIS]-position[Z_AXIS]); block->step_event_count = max(block->steps_x, max(block->steps_y, block->steps_z)); if (block->step_event_count == 0) { return; }; // bail if this is a zero-length block // compute path vector in terms of absolute step target and current positions double delta_mm[3]; delta_mm[X_AXIS] = (target[X_AXIS]-position[X_AXIS])/CONFIG_X_STEPS_PER_MM; delta_mm[Y_AXIS] = (target[Y_AXIS]-position[Y_AXIS])/CONFIG_Y_STEPS_PER_MM; delta_mm[Z_AXIS] = (target[Z_AXIS]-position[Z_AXIS])/CONFIG_Z_STEPS_PER_MM; block->millimeters = sqrt( (delta_mm[X_AXIS]*delta_mm[X_AXIS]) + (delta_mm[Y_AXIS]*delta_mm[Y_AXIS]) + (delta_mm[Z_AXIS]*delta_mm[Z_AXIS]) ); double inverse_millimeters = 1.0/block->millimeters; // store for efficency // calculate nominal_speed (mm/min) and nominal_rate (step/min) // minimum stepper speed is limited by MINIMUM_STEPS_PER_MINUTE in stepper.c double inverse_minute = feed_rate * inverse_millimeters; block->nominal_speed = feed_rate; // always > 0 block->nominal_rate = ceil(block->step_event_count * inverse_minute); // always > 0 // compute the acceleration rate for this block. (step/min/acceleration_tick) block->rate_delta = ceil( block->step_event_count * inverse_millimeters * CONFIG_ACCELERATION / (60 * ACCELERATION_TICKS_PER_SECOND) ); //// acceleeration manager calculations // Compute path unit vector double unit_vec[3]; unit_vec[X_AXIS] = delta_mm[X_AXIS]*inverse_millimeters; unit_vec[Y_AXIS] = delta_mm[Y_AXIS]*inverse_millimeters; unit_vec[Z_AXIS] = delta_mm[Z_AXIS]*inverse_millimeters; // Compute max junction speed by centripetal acceleration approximation. // Let a circle be tangent to both previous and current path line segments, where the junction // deviation is defined as the distance from the junction to the closest edge of the circle, // colinear with the circle center. The circular segment joining the two paths represents the // path of centripetal acceleration. Solve for max velocity based on max acceleration about the // radius of the circle, defined indirectly by junction deviation. This may be also viewed as // path width or max_jerk in the previous grbl version. This approach does not actually deviate // from path, but used as a robust way to compute cornering speeds, as it takes into account the // nonlinearities of both the junction angle and junction velocity. double vmax_junction = ZERO_SPEED; // prime for junctions close to 0 degree if ((block_buffer_head != block_buffer_tail) && (previous_nominal_speed > 0.0)) { // Compute cosine of angle between previous and current path. // vmax_junction is computed without sin() or acos() by trig half angle identity. double cos_theta = - previous_unit_vec[X_AXIS] * unit_vec[X_AXIS] - previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS] - previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ; if (cos_theta < 0.95) { // any junction *not* close to 0 degree vmax_junction = min(previous_nominal_speed, block->nominal_speed); // prime for close to 180 if (cos_theta > -0.95) { // any junction not close to neither 0 and 180 degree -> compute vmax double sin_theta_d2 = sqrt(0.5*(1.0-cos_theta)); // Trig half angle identity. Always positive. vmax_junction = min( vmax_junction, sqrt( CONFIG_ACCELERATION * CONFIG_JUNCTION_DEVIATION * sin_theta_d2/(1.0-sin_theta_d2) ) ); } } } block->vmax_junction = vmax_junction; // Initialize entry_speed. Compute based on deceleration to zero. // This will be updated in the forward and reverse planner passes. double v_allowable = max_allowable_speed(-CONFIG_ACCELERATION, ZERO_SPEED, block->millimeters); block->entry_speed = min(vmax_junction, v_allowable); // Set nominal_length_flag for more efficiency. // If a block can de/ac-celerate from nominal speed to zero within the length of // the block, then the speed will always be at the the maximum junction speed and // may always be ignored for any speed reduction checks. if (block->nominal_speed <= v_allowable) { block->nominal_length_flag = true; } else { block->nominal_length_flag = false; } block->recalculate_flag = true; // always calculate trapezoid for new block // update previous unit_vector and nominal speed memcpy(previous_unit_vec, unit_vec, sizeof(unit_vec)); // previous_unit_vec[] = unit_vec[] previous_nominal_speed = block->nominal_speed; //// end of acceleeration manager calculations // move buffer head and update position block_buffer_head = next_buffer_head; memcpy(position, target, sizeof(target)); // position[] = target[] planner_recalculate(); // make sure the stepper interrupt is processing stepper_start_processing(); }
/* PLANNER SPEED DEFINITION +--------+ <- current->nominal_speed / \ current->entry_speed -> + \ | + <- next->entry_speed +-------------+ time --> Recalculates the motion plan according to the following algorithm: 1. Go over every block in reverse order and calculate a junction speed reduction (i.e. block_t.entry_speed) so that: a. The junction speed is equal to or less than the maximum junction speed limit b. No speed reduction within one block requires faster deceleration than the acceleration limits. c. The last (or newest appended) block is planned from a complete stop. 2. Go over every block in chronological (forward) order and dial down junction speed values if a. The speed increase within one block would require faster acceleration than the acceleration limits. When these stages are complete, all blocks have a junction entry speed that will allow all speed changes to be performed using the overall limiting acceleration value, and where no junction speed is greater than the max limit. In other words, it just computed the fastest possible velocity profile through all buffered blocks, where the final buffered block is planned to come to a full stop when the buffer is fully executed. Finally it will: 3. Convert the plan to data that the stepper algorithm needs. Only block trapezoids adjacent to a a planner-modified junction speed with be updated, the others are assumed ok as is. All planner computations(1)(2) are performed in floating point to minimize numerical round-off errors. Only when planned values are converted to stepper rate parameters(3), these are integers. If another motion block is added while executing, the planner will re-plan and update the stored optimal velocity profile as it goes. Conceptually, the planner works like blowing up a balloon, where the balloon is the velocity profile. It's constrained by the speeds at the beginning and end of the buffer, along with the maximum junction speeds and nominal speeds of each block. Once a plan is computed, or balloon filled, this is the optimal velocity profile through all of the motions in the buffer. Whenever a new block is added, this changes some of the limiting conditions, or how the balloon is filled, so it has to be re-calculated to get the new optimal velocity profile. Also, since the planner only computes on what's in the planner buffer, some motions with lots of short line segments, like arcs, may seem to move slow. This is because there simply isn't enough combined distance traveled in the entire buffer to accelerate up to the nominal speed and then decelerate to a stop at the end of the buffer. There are a few simple solutions to this: (1) Maximize the machine acceleration. The planner will be able to compute higher speed profiles within the same combined distance. (2) Increase line segment(s) distance. The more combined distance the planner has to use, the faster it can go. (3) Increase the MINIMUM_PLANNER_SPEED. Not recommended. This will change what speed the planner plans to at the end of the buffer. Can lead to lost steps when coming to a stop. (4) [BEST] Increase the planner buffer size. The more combined distance, the bigger the balloon, or faster it can go. But this is not possible for 328p Arduinos because its limited memory is already maxed out. Future ARM versions should not have this issue, with look-ahead planner blocks numbering up to a hundred or more. NOTE: Since this function is constantly re-calculating for every new incoming block, it must be as efficient as possible. For example, in situations like arc generation or complex curves, the short, rapid line segments can execute faster than new blocks can be added, and the planner buffer will then starve and empty, leading to weird hiccup-like jerky motions. */ static void planner_recalculate() { // float entry_speed_sqr; // uint8_t block_index = block_buffer_head; // block_t *previous = NULL; // block_t *current = NULL; // block_t *next; // while (block_index != block_buffer_tail) { // block_index = prev_block_index( block_index ); // next = current; // current = previous; // previous = &block_buffer[block_index]; // // if (next && current) { // if (next != block_buffer_planned) { // if (previous == block_buffer_tail) { block_buffer_planned = next; } // else { // // if (current->entry_speed_sqr != current->max_entry_speed_sqr) { // current->recalculate_flag = true; // Almost always changes. So force recalculate. // entry_speed_sqr = next->entry_speed_sqr + 2*current->acceleration*current->millimeters; // if (entry_speed_sqr < current->max_entry_speed_sqr) { // current->entry_speed_sqr = entry_speed_sqr; // } else { // current->entry_speed_sqr = current->max_entry_speed_sqr; // } // } else { // block_buffer_planned = current; // } // } // } else { // break; // } // } // } // // block_index = block_buffer_planned; // next = &block_buffer[block_index]; // current = prev_block_index(block_index); // while (block_index != block_buffer_head) { // // // If the current block is an acceleration block, but it is not long enough to complete the // // full speed change within the block, we need to adjust the exit speed accordingly. Entry // // speeds have already been reset, maximized, and reverse planned by reverse planner. // if (current->entry_speed_sqr < next->entry_speed_sqr) { // // Compute block exit speed based on the current block speed and distance // // Computes: v_exit^2 = v_entry^2 + 2*acceleration*distance // entry_speed_sqr = current->entry_speed_sqr + 2*current->acceleration*current->millimeters; // // // If it's less than the stored value, update the exit speed and set recalculate flag. // if (entry_speed_sqr < next->entry_speed_sqr) { // next->entry_speed_sqr = entry_speed_sqr; // next->recalculate_flag = true; // } // } // // // Recalculate if current block entry or exit junction speed has changed. // if (current->recalculate_flag || next->recalculate_flag) { // // NOTE: Entry and exit factors always > 0 by all previous logic operations. // calculate_trapezoid_for_block(current, current->entry_speed_sqr, next->entry_speed_sqr); // current->recalculate_flag = false; // Reset current only to ensure next trapezoid is computed // } // // current = next; // next = &block_buffer[block_index]; // block_index = next_block_index( block_index ); // } // // // Last/newest block in buffer. Exit speed is set with MINIMUM_PLANNER_SPEED. Always recalculated. // calculate_trapezoid_for_block(next, next->entry_speed_sqr, MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED); // next->recalculate_flag = false; // TODO: No over-write protection exists for the executing block. For most cases this has proven to be ok, but // for feed-rate overrides, something like this is essential. Place a request here to the stepper driver to // find out where in the planner buffer is the a safe place to begin re-planning from. // if (block_buffer_head != block_buffer_tail) { float entry_speed_sqr; // Perform reverse planner pass. Skip the head(end) block since it is already initialized, and skip the // tail(first) block to prevent over-writing of the initial entry speed. uint8_t block_index = prev_block_index( block_buffer_head ); // Assume buffer is not empty. block_t *current = &block_buffer[block_index]; // Head block-1 = Newly appended block block_t *next; if (block_index != block_buffer_tail) { block_index = prev_block_index( block_index ); } while (block_index != block_buffer_tail) { next = current; current = &block_buffer[block_index]; // TODO: Determine maximum entry speed at junction for feedrate overrides, since they can alter // the planner nominal speeds at any time. This calc could be done in the override handler, but // this could require an additional variable to be stored to differentiate the programmed nominal // speeds, max junction speed, and override speeds/scalar. // If entry speed is already at the maximum entry speed, no need to recheck. Block is cruising. // If not, block in state of acceleration or deceleration. Reset entry speed to maximum and // check for maximum allowable speed reductions to ensure maximum possible planned speed. if (current->entry_speed_sqr != current->max_entry_speed_sqr) { current->entry_speed_sqr = current->max_entry_speed_sqr; current->recalculate_flag = true; // Almost always changes. So force recalculate. if (next->entry_speed_sqr < current->max_entry_speed_sqr) { // Computes: v_entry^2 = v_exit^2 + 2*acceleration*distance entry_speed_sqr = next->entry_speed_sqr + 2*current->acceleration*current->millimeters; if (entry_speed_sqr < current->max_entry_speed_sqr) { current->entry_speed_sqr = entry_speed_sqr; } } } block_index = prev_block_index( block_index ); } // Perform forward planner pass. Begins junction speed adjustments after tail(first) block. // Also recalculate trapezoids, block by block, as the forward pass completes the plan. block_index = next_block_index(block_buffer_tail); next = &block_buffer[block_buffer_tail]; // Places tail(first) block into current while (block_index != block_buffer_head) { current = next; next = &block_buffer[block_index]; // If the current block is an acceleration block, but it is not long enough to complete the // full speed change within the block, we need to adjust the exit speed accordingly. Entry // speeds have already been reset, maximized, and reverse planned by reverse planner. if (current->entry_speed_sqr < next->entry_speed_sqr) { // Compute block exit speed based on the current block speed and distance // Computes: v_exit^2 = v_entry^2 + 2*acceleration*distance entry_speed_sqr = current->entry_speed_sqr + 2*current->acceleration*current->millimeters; // If it's less than the stored value, update the exit speed and set recalculate flag. if (entry_speed_sqr < next->entry_speed_sqr) { next->entry_speed_sqr = entry_speed_sqr; next->recalculate_flag = true; } } // Recalculate if current block entry or exit junction speed has changed. if (current->recalculate_flag || next->recalculate_flag) { // NOTE: Entry and exit factors always > 0 by all previous logic operations. calculate_trapezoid_for_block(current, current->entry_speed_sqr, next->entry_speed_sqr); current->recalculate_flag = false; // Reset current only to ensure next trapezoid is computed } block_index = next_block_index( block_index ); } // Last/newest block in buffer. Exit speed is set with MINIMUM_PLANNER_SPEED. Always recalculated. calculate_trapezoid_for_block(next, next->entry_speed_sqr, MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED); next->recalculate_flag = false; // } }