diff --git a/Firmware/planner.cpp b/Firmware/planner.cpp index eb41e2fea..20bf76522 100644 --- a/Firmware/planner.cpp +++ b/Firmware/planner.cpp @@ -483,10 +483,10 @@ void getHighESpeed() uint8_t block_index = block_buffer_tail; while(block_index != block_buffer_head) { - if((block_buffer[block_index].steps_x.wide != 0) || - (block_buffer[block_index].steps_y.wide != 0) || - (block_buffer[block_index].steps_z.wide != 0)) { - float se=(float(block_buffer[block_index].steps_e.wide)/float(block_buffer[block_index].step_event_count.wide))*block_buffer[block_index].nominal_speed; + if((block_buffer[block_index].steps[X_AXIS].wide != 0) || + (block_buffer[block_index].steps[Y_AXIS].wide != 0) || + (block_buffer[block_index].steps[Z_AXIS].wide != 0)) { + float se=(float(block_buffer[block_index].steps[E_AXIS].wide)/float(block_buffer[block_index].step_event_count.wide))*block_buffer[block_index].nominal_speed; //se; mm/sec; if(se>high) { @@ -521,7 +521,7 @@ bool e_active() while(block_index != block_buffer_head) { block = &block_buffer[block_index]; - if(block->steps_e.wide != 0) e_active++; + if(block->steps[E_AXIS].wide != 0) e_active++; block_index = (block_index+1) & (BLOCK_BUFFER_SIZE - 1); } } @@ -544,10 +544,10 @@ void check_axes_activity() while(block_index != block_buffer_head) { block = &block_buffer[block_index]; - if(block->steps_x.wide != 0) x_active++; - if(block->steps_y.wide != 0) y_active++; - if(block->steps_z.wide != 0) z_active++; - if(block->steps_e.wide != 0) e_active++; + if(block->steps[X_AXIS].wide != 0) x_active++; + if(block->steps[Y_AXIS].wide != 0) y_active++; + if(block->steps[Z_AXIS].wide != 0) z_active++; + if(block->steps[E_AXIS].wide != 0) e_active++; block_index = (block_index+1) & (BLOCK_BUFFER_SIZE - 1); } } @@ -862,17 +862,17 @@ void plan_buffer_line(float x, float y, float z, const float &e, float feed_rate // Number of steps for each axis #ifndef COREXY // default non-h-bot planning -block->steps_x.wide = labs(dx); -block->steps_y.wide = labs(dy); +block->steps[X_AXIS].wide = labs(dx); +block->steps[Y_AXIS].wide = labs(dy); #else // corexy planning // these equations follow the form of the dA and dB equations on http://www.corexy.com/theory.html -block->steps_x.wide = labs(dx + dy); -block->steps_y.wide = labs(dx - dy); +block->steps[X_AXIS].wide = labs(dx + dy); +block->steps[Y_AXIS].wide = labs(dx - dy); #endif - block->steps_z.wide = labs(dz); - block->steps_e.wide = labs(de); - block->step_event_count.wide = max(block->steps_x.wide, max(block->steps_y.wide, max(block->steps_z.wide, block->steps_e.wide))); + block->steps[Z_AXIS].wide = labs(dz); + block->steps[E_AXIS].wide = labs(de); + block->step_event_count.wide = max(block->steps[X_AXIS].wide, max(block->steps[Y_AXIS].wide, max(block->steps[Z_AXIS].wide, block->steps[E_AXIS].wide))); // Bail if this is a zero-length block if (block->step_event_count.wide <= dropsegments) @@ -899,19 +899,19 @@ block->steps_y.wide = labs(dx - dy); //enable active axes #ifdef COREXY - if((block->steps_x.wide != 0) || (block->steps_y.wide != 0)) + if((block->steps[X_AXIS].wide != 0) || (block->steps[Y_AXIS].wide != 0)) { enable_x(); enable_y(); } #else - if(block->steps_x.wide != 0) enable_x(); - if(block->steps_y.wide != 0) enable_y(); + if(block->steps[X_AXIS].wide != 0) enable_x(); + if(block->steps[Y_AXIS].wide != 0) enable_y(); #endif - if(block->steps_z.wide != 0) enable_z(); - if(block->steps_e.wide != 0) enable_e0(); + if(block->steps[Z_AXIS].wide != 0) enable_z(); + if(block->steps[E_AXIS].wide != 0) enable_e0(); - if (block->steps_e.wide == 0) + if (block->steps[E_AXIS].wide == 0) { if(feed_ratesteps_x.wide <=dropsegments && block->steps_y.wide <=dropsegments && block->steps_z.wide <=dropsegments ) + if ( block->steps[X_AXIS].wide <=dropsegments && block->steps[Y_AXIS].wide <=dropsegments && block->steps[Z_AXIS].wide <=dropsegments ) { block->millimeters = fabs(delta_mm[E_AXIS]); } @@ -1006,7 +1006,7 @@ Having the real displacement of the head, we can calculate the total movement le // block->millimeters ... Euclidian length of the XYZ movement or the E length, if no XYZ movement. float steps_per_mm = block->step_event_count.wide/block->millimeters; uint32_t accel; - if(block->steps_x.wide == 0 && block->steps_y.wide == 0 && block->steps_z.wide == 0) + if(block->steps[X_AXIS].wide == 0 && block->steps[Y_AXIS].wide == 0 && block->steps[Z_AXIS].wide == 0) { accel = ceil(cs.retract_acceleration * steps_per_mm); // convert to: acceleration steps/sec^2 #ifdef LIN_ADVANCE @@ -1015,7 +1015,7 @@ Having the real displacement of the head, we can calculate the total movement le } else { - accel = ceil((block->steps_e.wide ? cs.acceleration : cs.travel_acceleration) * steps_per_mm); // convert to: acceleration steps/sec^2 + accel = ceil((block->steps[E_AXIS].wide ? cs.acceleration : cs.travel_acceleration) * steps_per_mm); // convert to: acceleration steps/sec^2 #ifdef LIN_ADVANCE /** @@ -1063,15 +1063,14 @@ Having the real displacement of the head, we can calculate the total movement le #endif // Limit acceleration per axis - //FIXME Vojtech: One shall rather limit a projection of the acceleration vector instead of using the limit. - if(((float)accel * (float)block->steps_x.wide / (float)block->step_event_count.wide) > max_acceleration_steps_per_s2[X_AXIS]) - { accel = max_acceleration_steps_per_s2[X_AXIS]; } - if(((float)accel * (float)block->steps_y.wide / (float)block->step_event_count.wide) > max_acceleration_steps_per_s2[Y_AXIS]) - { accel = max_acceleration_steps_per_s2[Y_AXIS]; } - if(((float)accel * (float)block->steps_e.wide / (float)block->step_event_count.wide) > max_acceleration_steps_per_s2[E_AXIS]) - { accel = max_acceleration_steps_per_s2[E_AXIS]; } - if(((float)accel * (float)block->steps_z.wide / (float)block->step_event_count.wide ) > max_acceleration_steps_per_s2[Z_AXIS]) - { accel = max_acceleration_steps_per_s2[Z_AXIS]; } + for (uint8_t axis = 0; axis < NUM_AXIS; axis++) + { + if(block->steps[axis].wide && max_acceleration_steps_per_s2[axis] < accel) + { + const float max_possible = float(max_acceleration_steps_per_s2[axis]) * float(block->step_event_count.wide) / float(block->steps[axis].wide); + if (max_possible < accel) accel = max_possible; + } + } } // Acceleration of the segment, in mm/sec^2 block->acceleration_steps_per_s2 = accel; diff --git a/Firmware/planner.h b/Firmware/planner.h index 1e0aeb94a..ae90428e9 100644 --- a/Firmware/planner.h +++ b/Firmware/planner.h @@ -71,7 +71,7 @@ union dda_usteps_t typedef struct { // Fields used by the bresenham algorithm for tracing the line // steps_x.y,z, step_event_count, acceleration_rate, direction_bits and active_extruder are set by plan_buffer_line(). - dda_isteps_t steps_x, steps_y, steps_z, steps_e; // Step count along each axis + dda_isteps_t steps[NUM_AXIS]; // Step count along each axis dda_usteps_t step_event_count; // The number of step events required to complete this block uint32_t acceleration_rate; // The acceleration rate used for acceleration calculation unsigned char direction_bits; // The direction bit set for this block (refers to *_DIRECTION_BIT in config.h) diff --git a/Firmware/stepper.cpp b/Firmware/stepper.cpp index 7e511c7a3..1d1658d0e 100644 --- a/Firmware/stepper.cpp +++ b/Firmware/stepper.cpp @@ -324,7 +324,7 @@ FORCE_INLINE void stepper_next_block() current_block = plan_get_current_block(); if (current_block != NULL) { #ifdef BACKLASH_X - if (current_block->steps_x.wide) + if (current_block->steps[X_AXIS].wide) { //X-axis movement if ((current_block->direction_bits ^ last_dir_bits) & 1) { @@ -347,7 +347,7 @@ FORCE_INLINE void stepper_next_block() } #endif #ifdef BACKLASH_Y - if (current_block->steps_y.wide) + if (current_block->steps[Y_AXIS].wide) { //Y-axis movement if ((current_block->direction_bits ^ last_dir_bits) & 2) { @@ -401,7 +401,7 @@ FORCE_INLINE void stepper_next_block() counter_z.lo = counter_x.lo; counter_e.lo = counter_x.lo; #ifdef LIN_ADVANCE - e_extruding = current_block->steps_e.lo != 0; + e_extruding = current_block->steps[E_AXIS].lo != 0; #endif } else { counter_x.wide = -(current_block->step_event_count.wide >> 1); @@ -409,7 +409,7 @@ FORCE_INLINE void stepper_next_block() counter_z.wide = counter_x.wide; counter_e.wide = counter_x.wide; #ifdef LIN_ADVANCE - e_extruding = current_block->steps_e.wide != 0; + e_extruding = current_block->steps[E_AXIS].wide != 0; #endif } step_events_completed.wide = 0; @@ -488,7 +488,7 @@ FORCE_INLINE void stepper_check_endstops() // Normal homing SET_BIT_TO(_endstop, X_AXIS, (READ(X_MIN_PIN) != X_MIN_ENDSTOP_INVERTING)); #endif - if((_endstop & _old_endstop & _BV(X_AXIS)) && (current_block->steps_x.wide > 0)) { + if((_endstop & _old_endstop & _BV(X_AXIS)) && (current_block->steps[X_AXIS].wide > 0)) { #ifdef VERBOSE_CHECK_HIT_ENDSTOPS endstops_trigsteps[X_AXIS] = count_position[X_AXIS]; #endif //VERBOSE_CHECK_HIT_ENDSTOPS @@ -505,7 +505,7 @@ FORCE_INLINE void stepper_check_endstops() // Normal homing SET_BIT_TO(_endstop, X_AXIS + 4, (READ(X_MAX_PIN) != X_MAX_ENDSTOP_INVERTING)); #endif - if((_endstop & _old_endstop & _BV(X_AXIS + 4)) && (current_block->steps_x.wide > 0)){ + if((_endstop & _old_endstop & _BV(X_AXIS + 4)) && (current_block->steps[X_AXIS].wide > 0)){ #ifdef VERBOSE_CHECK_HIT_ENDSTOPS endstops_trigsteps[X_AXIS] = count_position[X_AXIS]; #endif //VERBOSE_CHECK_HIT_ENDSTOPS @@ -529,7 +529,7 @@ FORCE_INLINE void stepper_check_endstops() // Normal homing SET_BIT_TO(_endstop, Y_AXIS, (READ(Y_MIN_PIN) != Y_MIN_ENDSTOP_INVERTING)); #endif - if((_endstop & _old_endstop & _BV(Y_AXIS)) && (current_block->steps_y.wide > 0)) { + if((_endstop & _old_endstop & _BV(Y_AXIS)) && (current_block->steps[Y_AXIS].wide > 0)) { #ifdef VERBOSE_CHECK_HIT_ENDSTOPS endstops_trigsteps[Y_AXIS] = count_position[Y_AXIS]; #endif //VERBOSE_CHECK_HIT_ENDSTOPS @@ -546,7 +546,7 @@ FORCE_INLINE void stepper_check_endstops() // Normal homing SET_BIT_TO(_endstop, Y_AXIS + 4, (READ(Y_MAX_PIN) != Y_MAX_ENDSTOP_INVERTING)); #endif - if((_endstop & _old_endstop & _BV(Y_AXIS + 4)) && (current_block->steps_y.wide > 0)){ + if((_endstop & _old_endstop & _BV(Y_AXIS + 4)) && (current_block->steps[Y_AXIS].wide > 0)){ #ifdef VERBOSE_CHECK_HIT_ENDSTOPS endstops_trigsteps[Y_AXIS] = count_position[Y_AXIS]; #endif //VERBOSE_CHECK_HIT_ENDSTOPS @@ -571,7 +571,7 @@ FORCE_INLINE void stepper_check_endstops() #else SET_BIT_TO(_endstop, Z_AXIS, (READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING)); #endif //TMC2130_SG_HOMING - if((_endstop & _old_endstop & _BV(Z_AXIS)) && (current_block->steps_z.wide > 0)) { + if((_endstop & _old_endstop & _BV(Z_AXIS)) && (current_block->steps[Z_AXIS].wide > 0)) { #ifdef VERBOSE_CHECK_HIT_ENDSTOPS endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS]; #endif //VERBOSE_CHECK_HIT_ENDSTOPS @@ -593,7 +593,7 @@ FORCE_INLINE void stepper_check_endstops() #else SET_BIT_TO(_endstop, Z_AXIS + 4, (READ(Z_MAX_PIN) != Z_MAX_ENDSTOP_INVERTING)); #endif //TMC2130_SG_HOMING - if((_endstop & _old_endstop & _BV(Z_AXIS + 4)) && (current_block->steps_z.wide > 0)) { + if((_endstop & _old_endstop & _BV(Z_AXIS + 4)) && (current_block->steps[Z_AXIS].wide > 0)) { #ifdef VERBOSE_CHECK_HIT_ENDSTOPS endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS]; #endif //VERBOSE_CHECK_HIT_ENDSTOPS @@ -646,7 +646,7 @@ FORCE_INLINE void stepper_tick_lowres() for (uint8_t i=0; i < step_loops; ++ i) { // Take multiple steps per interrupt (For high speed moves) MSerial.checkRx(); // Check for serial chars. // Step in X axis - counter_x.lo += current_block->steps_x.lo; + counter_x.lo += current_block->steps[X_AXIS].lo; if (counter_x.lo > 0) { STEP_NC_HI(X_AXIS); #ifdef DEBUG_XSTEP_DUP_PIN @@ -660,7 +660,7 @@ FORCE_INLINE void stepper_tick_lowres() #endif //DEBUG_XSTEP_DUP_PIN } // Step in Y axis - counter_y.lo += current_block->steps_y.lo; + counter_y.lo += current_block->steps[Y_AXIS].lo; if (counter_y.lo > 0) { STEP_NC_HI(Y_AXIS); #ifdef DEBUG_YSTEP_DUP_PIN @@ -674,7 +674,7 @@ FORCE_INLINE void stepper_tick_lowres() #endif //DEBUG_YSTEP_DUP_PIN } // Step in Z axis - counter_z.lo += current_block->steps_z.lo; + counter_z.lo += current_block->steps[Z_AXIS].lo; if (counter_z.lo > 0) { STEP_NC_HI(Z_AXIS); counter_z.lo -= current_block->step_event_count.lo; @@ -682,7 +682,7 @@ FORCE_INLINE void stepper_tick_lowres() STEP_NC_LO(Z_AXIS); } // Step in E axis - counter_e.lo += current_block->steps_e.lo; + counter_e.lo += current_block->steps[E_AXIS].lo; if (counter_e.lo > 0) { #ifndef LIN_ADVANCE STEP_NC_HI(E_AXIS); @@ -708,7 +708,7 @@ FORCE_INLINE void stepper_tick_highres() for (uint8_t i=0; i < step_loops; ++ i) { // Take multiple steps per interrupt (For high speed moves) MSerial.checkRx(); // Check for serial chars. // Step in X axis - counter_x.wide += current_block->steps_x.wide; + counter_x.wide += current_block->steps[X_AXIS].wide; if (counter_x.wide > 0) { STEP_NC_HI(X_AXIS); #ifdef DEBUG_XSTEP_DUP_PIN @@ -722,7 +722,7 @@ FORCE_INLINE void stepper_tick_highres() #endif //DEBUG_XSTEP_DUP_PIN } // Step in Y axis - counter_y.wide += current_block->steps_y.wide; + counter_y.wide += current_block->steps[Y_AXIS].wide; if (counter_y.wide > 0) { STEP_NC_HI(Y_AXIS); #ifdef DEBUG_YSTEP_DUP_PIN @@ -736,7 +736,7 @@ FORCE_INLINE void stepper_tick_highres() #endif //DEBUG_YSTEP_DUP_PIN } // Step in Z axis - counter_z.wide += current_block->steps_z.wide; + counter_z.wide += current_block->steps[Z_AXIS].wide; if (counter_z.wide > 0) { STEP_NC_HI(Z_AXIS); counter_z.wide -= current_block->step_event_count.wide; @@ -744,7 +744,7 @@ FORCE_INLINE void stepper_tick_highres() STEP_NC_LO(Z_AXIS); } // Step in E axis - counter_e.wide += current_block->steps_e.wide; + counter_e.wide += current_block->steps[E_AXIS].wide; if (counter_e.wide > 0) { #ifndef LIN_ADVANCE STEP_NC_HI(E_AXIS);