Fix segmentation, must not use last_milestone as it gets set afterwards not in append_milestone anymore
fix typo in the nmotors PR
void Block::debug()
{
- THEKERNEL->streams->printf("%p: steps:X%04d Y%04d Z%04d(max:%4d) nominal:r%10d/s%6.1f mm:%9.6f rdelta:%8f acc:%5d dec:%5d rates:%10d>%10d entry/max: %10.4f/%10.4f taken:%d ready:%d recalc:%d nomlen:%d\r\n",
+ THEKERNEL->streams->printf("%p: steps:X%04lu Y%04lu Z%04lu(max:%4lu) nominal:r%10lu/s%6.1f mm:%9.6f rdelta:%8f acc:%5lu dec:%5lu rates:%10lu>%10lu entry/max: %10.4f/%10.4f taken:%d ready:%d recalc:%d nomlen:%d\r\n",
this,
- this->steps[0],
- this->steps[1],
- this->steps[2],
- this->steps_event_count,
- this->nominal_rate,
- this->nominal_speed,
- this->millimeters,
- this->rate_delta,
- this->accelerate_until,
- this->decelerate_after,
- this->initial_rate,
- this->final_rate,
- this->entry_speed,
- this->max_entry_speed,
- this->times_taken,
- this->is_ready,
- recalculate_flag?1:0,
- nominal_length_flag?1:0
- );
+ this->steps[0],
+ this->steps[1],
+ this->steps[2],
+ this->steps_event_count,
+ this->nominal_rate,
+ this->nominal_speed,
+ this->millimeters,
+ this->rate_delta,
+ this->accelerate_until,
+ this->decelerate_after,
+ this->initial_rate,
+ this->final_rate,
+ this->entry_speed,
+ this->max_entry_speed,
+ this->times_taken,
+ this->is_ready,
+ recalculate_flag ? 1 : 0,
+ nominal_length_flag ? 1 : 0
+ );
}
// 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 (this->entry_speed != this->max_entry_speed)
- {
+ if (this->entry_speed != this->max_entry_speed) {
// If nominal length true, max junction speed is guaranteed to be reached. Only compute
// for max allowable speed if block is decelerating and nominal length is false.
- if ((!this->nominal_length_flag) && (this->max_entry_speed > exit_speed))
- {
+ if ((!this->nominal_length_flag) && (this->max_entry_speed > exit_speed)) {
float max_entry_speed = max_allowable_speed(-this->acceleration, exit_speed, this->millimeters);
this->entry_speed = min(max_entry_speed, this->max_entry_speed);
return this->entry_speed;
- }
- else
+ } else
this->entry_speed = this->max_entry_speed;
}
if (prev_max_exit_speed > max_entry_speed)
prev_max_exit_speed = max_entry_speed;
- if (prev_max_exit_speed <= entry_speed)
- {
+ if (prev_max_exit_speed <= entry_speed) {
// accel limited
entry_speed = prev_max_exit_speed;
// since we're now acceleration or cruise limited
std::vector<Gcode> gcodes;
- std::array<int, k_max_actuators> steps; // Number of steps for each axis for this block
- unsigned int steps_event_count; // Steps for the longest axis
- unsigned int nominal_rate; // Nominal rate in steps per second
- float nominal_speed; // Nominal speed in mm per second
- float millimeters; // Distance for this move
- float entry_speed;
- float exit_speed;
- float rate_delta; // Number of steps to add to the speed for each acceleration tick
- float acceleration; // the acceleratoin for this block
- unsigned int initial_rate; // Initial speed in steps per second
- unsigned int final_rate; // Final speed in steps per second
- unsigned int accelerate_until; // Stop accelerating after this number of steps
- unsigned int decelerate_after; // Start decelerating after this number of steps
+ std::array<uint32_t, k_max_actuators> steps; // Number of steps for each axis for this block
+ uint32_t steps_event_count; // Steps for the longest axis
+ uint32_t nominal_rate; // Nominal rate in steps per second
+ float nominal_speed; // Nominal speed in mm per second
+ float millimeters; // Distance for this move
+ float entry_speed;
+ float exit_speed;
+ float rate_delta; // Number of steps to add to the speed for each acceleration tick
+ float acceleration; // the acceleratoin for this block
+ uint32_t initial_rate; // Initial speed in steps per second
+ uint32_t final_rate; // Final speed in steps per second
+ uint32_t accelerate_until; // Stop accelerating after this number of steps
+ uint32_t decelerate_after; // Start decelerating after this number of steps
float max_entry_speed;
- short times_taken; // A block can be "taken" by any number of modules, and the next block is not moved to until all the modules have "released" it. This value serves as a tracker.
+ int16_t times_taken; // A block can be "taken" by any number of modules, and the next block is not moved to until all the modules have "released" it. This value serves as a tracker.
std::bitset<k_max_actuators> direction_bits; // Direction for each axis in bit form, relative to the direction port's mask
struct {
// It makes sure the speed stays within the configured constraints ( acceleration, junction_deviation, etc )
// It goes over the list in both direction, every time a block is added, re-doing the math to make sure everything is optimal
-Planner::Planner(){
+Planner::Planner()
+{
clear_vector_float(this->previous_unit_vec);
config_load();
}
// Configure acceleration
-void Planner::config_load(){
+void Planner::config_load()
+{
this->acceleration = THEKERNEL->config->value(acceleration_checksum)->by_default(100.0F )->as_number(); // Acceleration is in mm/s^2
this->z_acceleration = THEKERNEL->config->value(z_acceleration_checksum)->by_default(0.0F )->as_number(); // disabled by default
// Direction bits
- for (size_t i = 0; i < THEKERNEL->robot->actuators.size(); i++)
- {
+ for (size_t i = 0; i < THEKERNEL->robot->actuators.size(); i++) {
int steps = THEKERNEL->robot->actuators[i]->steps_to_target(actuator_pos[i]);
block->direction_bits[i] = (steps < 0) ? 1 : 0;
block->steps[i] = labs(steps);
}
- acceleration= this->acceleration;
- junction_deviation= this->junction_deviation;
+ acceleration = this->acceleration;
+ junction_deviation = this->junction_deviation;
// use either regular acceleration or a z only move accleration
if(block->steps[ALPHA_STEPPER] == 0 && block->steps[BETA_STEPPER] == 0) {
// z only move
- if(this->z_acceleration > 0.0F) acceleration= this->z_acceleration;
- if(this->z_junction_deviation >= 0.0F) junction_deviation= this->z_junction_deviation;
+ if(this->z_acceleration > 0.0F) acceleration = this->z_acceleration;
+ if(this->z_junction_deviation >= 0.0F) junction_deviation = this->z_junction_deviation;
}
- block->acceleration= acceleration; // save in block
+ block->acceleration = acceleration; // save in block
// Max number of steps, for all axes
- int steps_event_count = 0;
- for (size_t s = 0; s < THEKERNEL->robot->actuators.size(); s++)
+ uint32_t steps_event_count = 0;
+ for (size_t s = 0; s < THEKERNEL->robot->actuators.size(); s++) {
steps_event_count = std::max(steps_event_count, block->steps[s]);
+ }
block->steps_event_count = steps_event_count;
block->millimeters = distance;
// Calculate speed in mm/sec 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
- if( distance > 0.0F ){
+ if( distance > 0.0F ) {
block->nominal_speed = rate_mm_s; // (mm/s) Always > 0
block->nominal_rate = ceilf(block->steps_event_count * rate_mm_s / distance); // (step/s) Always > 0
- }else{
+ } else {
block->nominal_speed = 0.0F;
block->nominal_rate = 0;
}
// and this allows one to stop with little to no decleration in many cases. This is particualrly bad on leadscrew based systems that will skip steps.
float vmax_junction = minimum_planner_speed; // Set default max junction speed
- if (!THEKERNEL->conveyor->is_queue_empty())
- {
+ if (!THEKERNEL->conveyor->is_queue_empty()) {
float previous_nominal_speed = THEKERNEL->conveyor->queue.item_ref(THEKERNEL->conveyor->queue.prev(THEKERNEL->conveyor->queue.head_i))->nominal_speed;
if (previous_nominal_speed > 0.0F && junction_deviation > 0.0F) {
// 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 = - this->previous_unit_vec[X_AXIS] * unit_vec[X_AXIS]
- - this->previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS]
- - this->previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ;
+ - this->previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS]
+ - this->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.95F) {
THEKERNEL->conveyor->queue_head_block();
}
-void Planner::recalculate() {
+void Planner::recalculate()
+{
Conveyor::Queue_t &queue = THEKERNEL->conveyor->queue;
unsigned int block_index;
block_index = queue.head_i;
current = queue.item_ref(block_index);
- if (!queue.is_empty())
- {
- while ((block_index != queue.tail_i) && current->recalculate_flag)
- {
+ if (!queue.is_empty()) {
+ while ((block_index != queue.tail_i) && current->recalculate_flag) {
entry_speed = current->reverse_pass(entry_speed);
block_index = queue.prev(block_index);
float exit_speed = current->max_exit_speed();
- while (block_index != queue.head_i)
- {
+ while (block_index != queue.head_i) {
previous = current;
block_index = queue.next(block_index);
current = queue.item_ref(block_index);
// Calculates the maximum allowable speed at this point when you must be able to reach target_velocity using the
// acceleration within the allotted distance.
-float Planner::max_allowable_speed(float acceleration, float target_velocity, float distance) {
- return(
- sqrtf(target_velocity*target_velocity-2.0F*acceleration*distance) //Was acceleration*60*60*distance, in case this breaks, but here we prefer to use seconds instead of minutes
- );
+float Planner::max_allowable_speed(float acceleration, float target_velocity, float distance)
+{
+ return(
+ sqrtf(target_velocity * target_velocity - 2.0F * acceleration * distance) //Was acceleration*60*60*distance, in case this breaks, but here we prefer to use seconds instead of minutes
+ );
}
if (segments > 1) {
// A vector to keep track of the endpoint of each segment
float segment_delta[3];
- float segment_end[3];
+ float segment_end[3]{last_milestone[X_AXIS], last_milestone[Y_AXIS], last_milestone[Z_AXIS]};
// How far do we move each segment?
for (int i = X_AXIS; i <= Z_AXIS; i++)
for (int i = 1; i < segments; i++) {
if(THEKERNEL->is_halted()) return false; // don't queue any more segments
for(int axis = X_AXIS; axis <= Z_AXIS; axis++ )
- segment_end[axis] = last_milestone[axis] + segment_delta[axis];
+ segment_end[axis] += segment_delta[axis];
// Append the end of this segment to the queue
bool b= this->append_milestone(gcode, segment_end, rate_mm_s);
this->status = BACK_OFF_HOME;
// these are handled differently
- if((is_delta || is_scara) && this->limit_enable[X_AXIS]) {
+ if(is_delta || is_scara) {
// Move off of the endstop using a regular relative move in Z only
params.push_back({'Z', this->retract_mm[Z_AXIS]*(this->home_direction[Z_AXIS]?1:-1)});
}
if(home_all) {
- // for deltas this may be important rather than setting each individually
-
// Here's where we would have been if the endstops were perfectly trimmed
float ideal_position[3] = {
this->homing_position[X_AXIS] + this->home_offset[X_AXIS],
}
}
- // on some systems where 0,0 is bed center it is noce to have home goto 0,0 after homing
+ // on some systems where 0,0 is bed center it is nice to have home goto 0,0 after homing
// default is off
- if(!is_delta && this->move_to_origin_after_home) move_to_origin(axes_to_move);
-
- // if limit switches are enabled we must back off endstop after setting home
- back_off_home(axes_to_move);
-
- // deltas are not left at 0,0 becuase of the trim settings, so move to 0,0 if requested
- if(is_delta && this->move_to_origin_after_home) move_to_origin(axes_to_move);
+ if(!is_delta) {
+ if(this->move_to_origin_after_home) move_to_origin(axes_to_move);
+ // if limit switches are enabled we must back off endstop after setting home
+ back_off_home(axes_to_move);
+
+ }else if(this->move_to_origin_after_home || this->limit_enable[X_AXIS]) {
+ // deltas are not left at 0,0 because of the trim settings, so move to 0,0 if requested, but we need to back off endstops first
+ // also need to back off endstops if limits are enabled
+ back_off_home(axes_to_move);
+ if(this->move_to_origin_after_home) move_to_origin(axes_to_move);
+ }
}
} else if (gcode->has_m) {
HCoop / clinton / Smoothieware.git / commitdiff