CMS/Marlin
3
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Cleanup of planner code

Browse Source 
- Use named axis indexes, `X_AXIS` etc.
- Replace `block.steps_A` with block.steps[A]`
- Replace `A_segment_time` with `segment_time[A]`
- Add `A_AXIS`, `B_AXIS` for `COREXY` axes
- Conditional compile based on `EXTRUDERS`
- Add BLOCK_MOD macro for planner block indexes
- Apply coding standards to `planner.h` and `planner.cpp`
- Small optimizations of planner code
- Update `stepper.cpp` for new `block` struct
- Replace `memcpy` with loops, let the compiler unroll them
- Make `movesplanned` into an inline function
This commit is contained in:
Scott Lahteine 2015-03-20 20:42:49 -07:00
parent 0d869703ca
commit 13fbf42d95
4 changed files with 616 additions and 754 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
Marlin/Marlin.h
View File

@ -183,7 +183,7 @@ void manage_inactivity(bool ignore_stepper_queue=false);
#define disable_e3() /* nothing */ #define disable_e3() /* nothing */
#endif #endif
enum AxisEnum {X_AXIS=0, Y_AXIS=1, Z_AXIS=2, E_AXIS=3, X_HEAD=4, Y_HEAD=5}; enum AxisEnum {X_AXIS=0, Y_AXIS=1, A_AXIS=0, B_AXIS=1, Z_AXIS=2, E_AXIS=3, X_HEAD=4, Y_HEAD=5};
//X_HEAD and Y_HEAD is used for systems that don't have a 1:1 relationship between X_AXIS and X Head movement, like CoreXY bots. //X_HEAD and Y_HEAD is used for systems that don't have a 1:1 relationship between X_AXIS and X Head movement, like CoreXY bots.
void FlushSerialRequestResend(); void FlushSerialRequestResend();
@ -270,7 +270,7 @@ extern unsigned char fanSpeedSoftPwm;
extern bool filament_sensor; //indicates that filament sensor readings should control extrusion extern bool filament_sensor; //indicates that filament sensor readings should control extrusion
extern float filament_width_meas; //holds the filament diameter as accurately measured extern float filament_width_meas; //holds the filament diameter as accurately measured
extern signed char measurement_delay[]; //ring buffer to delay measurement extern signed char measurement_delay[]; //ring buffer to delay measurement
extern int delay_index1, delay_index2; //index into ring buffer extern int delay_index1, delay_index2; //ring buffer index. used by planner, temperature, and main code
extern float delay_dist; //delay distance counter extern float delay_dist; //delay distance counter
extern int meas_delay_cm; //delay distance extern int meas_delay_cm; //delay distance
#endif #endif

1142
Marlin/planner.cpp
View File

File diff suppressed because it is too large Load Diff

117
Marlin/planner.h
View File

@ -21,20 +21,16 @@
// This module is to be considered a sub-module of stepper.c. Please don't include // This module is to be considered a sub-module of stepper.c. Please don't include
// this file from any other module. // this file from any other module.
#ifndef planner_h #ifndef PLANNER_H
#define planner_h #define PLANNER_H
#include "Marlin.h" #include "Marlin.h"
#ifdef ENABLE_AUTO_BED_LEVELING
#include "vector_3.h"
#endif // ENABLE_AUTO_BED_LEVELING
// This struct is used when buffering the setup for each linear movement "nominal" values are as specified in // This struct is used when buffering the setup for each linear movement "nominal" values are as specified in
// the source g-code and may never actually be reached if acceleration management is active. // the source g-code and may never actually be reached if acceleration management is active.
typedef struct { typedef struct {
// Fields used by the bresenham algorithm for tracing the line // Fields used by the bresenham algorithm for tracing the line
long steps_x, steps_y, steps_z, steps_e; // Step count along each axis long steps[NUM_AXIS]; // Step count along each axis
unsigned long step_event_count; // The number of step events required to complete this block unsigned long step_event_count; // The number of step events required to complete this block
long accelerate_until; // The index of the step event on which to stop acceleration long accelerate_until; // The index of the step event on which to stop acceleration
long decelerate_after; // The index of the step event on which to start decelerating long decelerate_after; // The index of the step event on which to start decelerating
@ -49,7 +45,7 @@ typedef struct {
#endif #endif
// Fields used by the motion planner to manage acceleration // Fields used by the motion planner to manage acceleration
// float speed_x, speed_y, speed_z, speed_e; // Nominal mm/sec for each axis // float speed_x, speed_y, speed_z, speed_e; // Nominal mm/sec for each axis
float nominal_speed; // The nominal speed for this block in mm/sec float nominal_speed; // The nominal speed for this block in mm/sec
float entry_speed; // Entry speed at previous-current junction in mm/sec float entry_speed; // Entry speed at previous-current junction in mm/sec
float max_entry_speed; // Maximum allowable junction entry speed in mm/sec float max_entry_speed; // Maximum allowable junction entry speed in mm/sec
@ -65,48 +61,44 @@ typedef struct {
unsigned long acceleration_st; // acceleration steps/sec^2 unsigned long acceleration_st; // acceleration steps/sec^2
unsigned long fan_speed; unsigned long fan_speed;
#ifdef BARICUDA #ifdef BARICUDA
unsigned long valve_pressure; unsigned long valve_pressure;
unsigned long e_to_p_pressure; unsigned long e_to_p_pressure;
#endif #endif
volatile char busy; volatile char busy;
} block_t; } block_t;
#ifdef ENABLE_AUTO_BED_LEVELING #define BLOCK_MOD(n) ((n)&(BLOCK_BUFFER_SIZE-1))
// this holds the required transform to compensate for bed level
extern matrix_3x3 plan_bed_level_matrix;
#endif // #ifdef ENABLE_AUTO_BED_LEVELING
// Initialize the motion plan subsystem // Initialize the motion plan subsystem
void plan_init(); void plan_init();
// Add a new linear movement to the buffer. x, y and z is the signed, absolute target position in void check_axes_activity();
// millimaters. Feed rate specifies the speed of the motion.
// Get the number of buffered moves
extern volatile unsigned char block_buffer_head;
extern volatile unsigned char block_buffer_tail;
FORCE_INLINE uint8_t movesplanned() { return BLOCK_MOD(block_buffer_head - block_buffer_tail + BLOCK_BUFFER_SIZE); }
#ifdef ENABLE_AUTO_BED_LEVELING #ifdef ENABLE_AUTO_BED_LEVELING
void plan_buffer_line(float x, float y, float z, const float &e, float feed_rate, const uint8_t &extruder); #include "vector_3.h"
// this holds the required transform to compensate for bed level
extern matrix_3x3 plan_bed_level_matrix;
// 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.
void plan_buffer_line(float x, float y, float z, const float &e, float feed_rate, const uint8_t &extruder);
// Set position. Used for G92 instructions.
void plan_set_position(float x, float y, float z, const float &e);
#ifndef DELTA #ifndef DELTA
// Get the position applying the bed level matrix if enabled // Get the position applying the bed level matrix if enabled
vector_3 plan_get_position(); vector_3 plan_get_position();
#endif #endif
#else #else //!ENABLE_AUTO_BED_LEVELING
void plan_buffer_line(const float &x, const float &y, const float &z, const float &e, float feed_rate, const uint8_t &extruder); void plan_buffer_line(const float &x, const float &y, const float &z, const float &e, float feed_rate, const uint8_t &extruder);
#endif // ENABLE_AUTO_BED_LEVELING void plan_set_position(const float &x, const float &y, const float &z, const float &e);
#endif //!ENABLE_AUTO_BED_LEVELING
// Set position. Used for G92 instructions.
#ifdef ENABLE_AUTO_BED_LEVELING
void plan_set_position(float x, float y, float z, const float &e);
#else
void plan_set_position(const float &x, const float &y, const float &z, const float &e);
#endif // ENABLE_AUTO_BED_LEVELING
void plan_set_e_position(const float &e); void plan_set_e_position(const float &e);
void check_axes_activity();
uint8_t movesplanned(); //return the nr of buffered moves
extern unsigned long minsegmenttime; extern unsigned long minsegmenttime;
extern float max_feedrate[NUM_AXIS]; // set the max speeds extern float max_feedrate[NUM_AXIS]; // set the max speeds
extern float axis_steps_per_unit[NUM_AXIS]; extern float axis_steps_per_unit[NUM_AXIS];
@ -122,44 +114,41 @@ extern float mintravelfeedrate;
extern unsigned long axis_steps_per_sqr_second[NUM_AXIS]; extern unsigned long axis_steps_per_sqr_second[NUM_AXIS];
#ifdef AUTOTEMP #ifdef AUTOTEMP
extern bool autotemp_enabled; extern bool autotemp_enabled;
extern float autotemp_max; extern float autotemp_max;
extern float autotemp_min; extern float autotemp_min;
extern float autotemp_factor; extern float autotemp_factor;
#endif #endif
extern block_t block_buffer[BLOCK_BUFFER_SIZE]; // A ring buffer for motion instructions
extern block_t block_buffer[BLOCK_BUFFER_SIZE]; // A ring buffer for motion instfructions
extern volatile unsigned char block_buffer_head; // Index of the next block to be pushed extern volatile unsigned char block_buffer_head; // Index of the next block to be pushed
extern volatile unsigned char block_buffer_tail; extern volatile unsigned char block_buffer_tail;
// Called when the current block is no longer needed. Discards the block and makes the memory
// availible for new blocks.
FORCE_INLINE void plan_discard_current_block()
{
if (block_buffer_head != block_buffer_tail) {
block_buffer_tail = (block_buffer_tail + 1) & (BLOCK_BUFFER_SIZE - 1);
}
}
// Gets the current block. Returns NULL if buffer empty
FORCE_INLINE block_t *plan_get_current_block()
{
if (block_buffer_head == block_buffer_tail) {
return(NULL);
}
block_t *block = &block_buffer[block_buffer_tail];
block->busy = true;
return(block);
}
// Returns true if the buffer has a queued block, false otherwise // Returns true if the buffer has a queued block, false otherwise
FORCE_INLINE bool blocks_queued() { return (block_buffer_head != block_buffer_tail); } FORCE_INLINE bool blocks_queued() { return (block_buffer_head != block_buffer_tail); }
// Called when the current block is no longer needed. Discards
// the block and makes the memory available for new blocks.
FORCE_INLINE void plan_discard_current_block() {
if (blocks_queued())
block_buffer_tail = BLOCK_MOD(block_buffer_tail + 1);
}
// Gets the current block. Returns NULL if buffer empty
FORCE_INLINE block_t *plan_get_current_block() {
if (blocks_queued()) {
block_t *block = &block_buffer[block_buffer_tail];
block->busy = true;
return block;
}
else
return NULL;
}
#ifdef PREVENT_DANGEROUS_EXTRUDE #ifdef PREVENT_DANGEROUS_EXTRUDE
void set_extrude_min_temp(float temp); void set_extrude_min_temp(float temp);
#endif #endif
void reset_acceleration_rates(); void reset_acceleration_rates();
#endif
#endif //PLANNER_H

107
Marlin/stepper.cpp
View File

@ -370,7 +370,7 @@ ISR(TIMER1_COMPA_vect) {
step_events_completed = 0; step_events_completed = 0;
#ifdef Z_LATE_ENABLE #ifdef Z_LATE_ENABLE
if (current_block->steps_z > 0) { if (current_block->steps[Z_AXIS] > 0) {
enable_z(); enable_z();
OCR1A = 2000; //1ms wait OCR1A = 2000; //1ms wait
return; return;
@ -411,7 +411,7 @@ ISR(TIMER1_COMPA_vect) {
#define UPDATE_ENDSTOP(axis,AXIS,minmax,MINMAX) \ #define UPDATE_ENDSTOP(axis,AXIS,minmax,MINMAX) \
bool axis ##_## minmax ##_endstop = (READ(AXIS ##_## MINMAX ##_PIN) != AXIS ##_## MINMAX ##_ENDSTOP_INVERTING); \ bool axis ##_## minmax ##_endstop = (READ(AXIS ##_## MINMAX ##_PIN) != AXIS ##_## MINMAX ##_ENDSTOP_INVERTING); \
if (axis ##_## minmax ##_endstop && old_## axis ##_## minmax ##_endstop && (current_block->steps_## axis > 0)) { \ if (axis ##_## minmax ##_endstop && old_## axis ##_## minmax ##_endstop && (current_block->steps[AXIS ##_AXIS] > 0)) { \
endstops_trigsteps[AXIS ##_AXIS] = count_position[AXIS ##_AXIS]; \ endstops_trigsteps[AXIS ##_AXIS] = count_position[AXIS ##_AXIS]; \
endstop_## axis ##_hit = true; \ endstop_## axis ##_hit = true; \
step_events_completed = current_block->step_event_count; \ step_events_completed = current_block->step_event_count; \
@ -420,54 +420,54 @@ ISR(TIMER1_COMPA_vect) {
// Check X and Y endstops // Check X and Y endstops
if (check_endstops) { if (check_endstops) {
#ifndef COREXY #ifdef COREXY
if (TEST(out_bits, X_AXIS)) // stepping along -X axis (regular cartesians bot)
#else
// Head direction in -X axis for CoreXY bots. // Head direction in -X axis for CoreXY bots.
// If DeltaX == -DeltaY, the movement is only in Y axis // If DeltaX == -DeltaY, the movement is only in Y axis
if (current_block->steps_x != current_block->steps_y || (TEST(out_bits, X_AXIS) == TEST(out_bits, Y_AXIS))) if (current_block->steps[A_AXIS] != current_block->steps[B_AXIS] || (TEST(out_bits, A_AXIS) == TEST(out_bits, B_AXIS)))
if (TEST(out_bits, X_HEAD)) if (TEST(out_bits, X_HEAD))
#endif
{ // -direction
#ifdef DUAL_X_CARRIAGE
// with 2 x-carriages, endstops are only checked in the homing direction for the active extruder
if ((current_block->active_extruder == 0 && X_HOME_DIR == -1) || (current_block->active_extruder != 0 && X2_HOME_DIR == -1))
#endif
{
#if defined(X_MIN_PIN) && X_MIN_PIN >= 0
UPDATE_ENDSTOP(x, X, min, MIN);
#endif
}
}
else { // +direction
#ifdef DUAL_X_CARRIAGE
// with 2 x-carriages, endstops are only checked in the homing direction for the active extruder
if ((current_block->active_extruder == 0 && X_HOME_DIR == 1) || (current_block->active_extruder != 0 && X2_HOME_DIR == 1))
#endif
{
#if defined(X_MAX_PIN) && X_MAX_PIN >= 0
UPDATE_ENDSTOP(x, X, max, MAX);
#endif
}
}
#ifndef COREXY
if (TEST(out_bits, Y_AXIS)) // -direction
#else #else
if (TEST(out_bits, X_AXIS)) // stepping along -X axis (regular cartesians bot)
#endif
{ // -direction
#ifdef DUAL_X_CARRIAGE
// with 2 x-carriages, endstops are only checked in the homing direction for the active extruder
if ((current_block->active_extruder == 0 && X_HOME_DIR == -1) || (current_block->active_extruder != 0 && X2_HOME_DIR == -1))
#endif
{
#if defined(X_MIN_PIN) && X_MIN_PIN >= 0
UPDATE_ENDSTOP(x, X, min, MIN);
#endif
}
}
else { // +direction
#ifdef DUAL_X_CARRIAGE
// with 2 x-carriages, endstops are only checked in the homing direction for the active extruder
if ((current_block->active_extruder == 0 && X_HOME_DIR == 1) || (current_block->active_extruder != 0 && X2_HOME_DIR == 1))
#endif
{
#if defined(X_MAX_PIN) && X_MAX_PIN >= 0
UPDATE_ENDSTOP(x, X, max, MAX);
#endif
}
}
#ifdef COREXY
// Head direction in -Y axis for CoreXY bots. // Head direction in -Y axis for CoreXY bots.
// If DeltaX == DeltaY, the movement is only in X axis // If DeltaX == DeltaY, the movement is only in X axis
if (current_block->steps_x != current_block->steps_y || (TEST(out_bits, X_AXIS) != TEST(out_bits, Y_AXIS))) if (current_block->steps[A_AXIS] != current_block->steps[B_AXIS] || (TEST(out_bits, A_AXIS) != TEST(out_bits, B_AXIS)))
if (TEST(out_bits, Y_HEAD)) if (TEST(out_bits, Y_HEAD))
#else
if (TEST(out_bits, Y_AXIS)) // -direction
#endif #endif
{ // -direction { // -direction
#if defined(Y_MIN_PIN) && Y_MIN_PIN >= 0 #if defined(Y_MIN_PIN) && Y_MIN_PIN >= 0
UPDATE_ENDSTOP(y, Y, min, MIN); UPDATE_ENDSTOP(y, Y, min, MIN);
#endif #endif
} }
else { // +direction else { // +direction
#if defined(Y_MAX_PIN) && Y_MAX_PIN >= 0 #if defined(Y_MAX_PIN) && Y_MAX_PIN >= 0
UPDATE_ENDSTOP(y, Y, max, MAX); UPDATE_ENDSTOP(y, Y, max, MAX);
#endif #endif
} }
} }
if (TEST(out_bits, Z_AXIS)) { // -direction if (TEST(out_bits, Z_AXIS)) { // -direction
@ -515,7 +515,7 @@ ISR(TIMER1_COMPA_vect) {
#endif #endif
#ifdef ADVANCE #ifdef ADVANCE
counter_e += current_block->steps_e; counter_e += current_block->steps[E_AXIS];
if (counter_e > 0) { if (counter_e > 0) {
counter_e -= current_block->step_event_count; counter_e -= current_block->step_event_count;
e_steps[current_block->active_extruder] += TEST(out_bits, E_AXIS) ? -1 : 1; e_steps[current_block->active_extruder] += TEST(out_bits, E_AXIS) ? -1 : 1;
@ -529,15 +529,14 @@ ISR(TIMER1_COMPA_vect) {
* instead of doing each in turn. The extra tests add enough * instead of doing each in turn. The extra tests add enough
* lag to allow it work with without needing NOPs * lag to allow it work with without needing NOPs
*/ */
counter_x += current_block->steps_x; #define STEP_ADD(axis, AXIS) \
if (counter_x > 0) X_STEP_WRITE(HIGH); counter_## axis += current_block->steps[AXIS ##_AXIS]; \
counter_y += current_block->steps_y; if (counter_## axis > 0) { AXIS ##_STEP_WRITE(HIGH); }
if (counter_y > 0) Y_STEP_WRITE(HIGH); STEP_ADD(x,X);
counter_z += current_block->steps_z; STEP_ADD(y,Y);
if (counter_z > 0) Z_STEP_WRITE(HIGH); STEP_ADD(z,Z);
#ifndef ADVANCE #ifndef ADVANCE
counter_e += current_block->steps_e; STEP_ADD(e,E);
if (counter_e > 0) E_STEP_WRITE(HIGH);
#endif #endif
#define STEP_IF_COUNTER(axis, AXIS) \ #define STEP_IF_COUNTER(axis, AXIS) \
@ -557,7 +556,7 @@ ISR(TIMER1_COMPA_vect) {
#else // !CONFIG_STEPPERS_TOSHIBA #else // !CONFIG_STEPPERS_TOSHIBA
#define APPLY_MOVEMENT(axis, AXIS) \ #define APPLY_MOVEMENT(axis, AXIS) \
counter_## axis += current_block->steps_## axis; \ counter_## axis += current_block->steps[AXIS ##_AXIS]; \
if (counter_## axis > 0) { \ if (counter_## axis > 0) { \
AXIS ##_APPLY_STEP(!INVERT_## AXIS ##_STEP_PIN,0); \ AXIS ##_APPLY_STEP(!INVERT_## AXIS ##_STEP_PIN,0); \
counter_## axis -= current_block->step_event_count; \ counter_## axis -= current_block->step_event_count; \