From 0bc0fd77577e9c8b629f3cb9dfdde8759fa52f7e Mon Sep 17 00:00:00 2001 From: Simen Svale Skogsrud Date: Mon, 24 Jan 2011 20:55:25 +0100 Subject: [PATCH] look ahead planner complete and enabled save the acceleration limiting forward scan. Not tested on real hardware, just logic analyzer --- script/console | 2 +- stepper.c | 20 ++++----- stepper_plan.c | 108 +++++++++++++++++++++++++++++++------------------ 3 files changed, 80 insertions(+), 50 deletions(-) diff --git a/script/console b/script/console index 72e102f..433b56e 100755 --- a/script/console +++ b/script/console @@ -1,4 +1,4 @@ -socat -d -d READLINE /dev/tty.usbmodem24121,clocal=1,nonblock=1,cread=1,cs8,ixon=1,ixoff=1 +socat -d -d READLINE /dev/tty.usbmodem26221,nonblock=1,clocal=1 socat -d -d READLINE /dev/tty.usbserial-A9007QcR,clocal=1,nonblock=1,cread=1,cs8,ixon=1,ixoff=1 #socat -d -d READLINE /dev/tty.FireFly-A964-SPP-1,clocal=1,nonblock=1,cread=1,cs8,ixon=1,ixoff=1 diff --git a/stepper.c b/stepper.c index e38a8af..06383c8 100644 --- a/stepper.c +++ b/stepper.c @@ -67,17 +67,15 @@ uint32_t trapezoid_rate; // The current rate of step_events accord // // time -----> // -// The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates until -// -// block->accelerate_ticks by block->rate_delta each tick, then stays up for block->plateau_ticks and -// decelerates for the rest of the block until the trapezoid generator is reset for the next block. -// The slope of acceleration is always +/- block->rate_delta. Any stage may be skipped by setting the -// duration to 0 ticks. - +// The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates by block->rate_delta +// during the first block->accelerate_until step_events then keeps going at constant speed until the step-evet count reaches +// block->decelerate_after until the trapezoid generator is reset for the next block. +// The slope of acceleration is always +/- block->rate_delta and is applied at a constant rate by trapezoid_generator_tick() +// that is called ACCELERATION_TICKS_PER_SECOND times per second. // Initializes the trapezoid generator from the current block. Called whenever a new // block begins. -inline void reset_trapezoid_generator() { +inline void trapezoid_generator_reset() { trapezoid_rate = current_block->initial_rate; set_step_events_per_minute(trapezoid_rate); } @@ -122,7 +120,9 @@ SIGNAL(TIMER1_COMPA_vect) #else SIGNAL(SIG_OUTPUT_COMPARE1A) #endif -{ +{ + // TODO: Check if the busy-flag can be eliminated by just disabeling this interrupt while we are in it + if(busy){ return; } // The busy-flag is used to avoid reentering this interrupt // Set the direction pins a cuple of nanoseconds before we step the steppers STEPPING_PORT = (STEPPING_PORT & ~DIRECTION_MASK) | (out_bits & DIRECTION_MASK); @@ -143,7 +143,7 @@ SIGNAL(SIG_OUTPUT_COMPARE1A) if (block_buffer_head != block_buffer_tail) { // Retrieve a new line and get ready to step it current_block = &block_buffer[block_buffer_tail]; - reset_trapezoid_generator(); + trapezoid_generator_reset(); counter_x = -(current_block->step_event_count >> 1); counter_y = counter_x; counter_z = counter_x; diff --git a/stepper_plan.c b/stepper_plan.c index f74b33f..fd0c59d 100644 --- a/stepper_plan.c +++ b/stepper_plan.c @@ -31,11 +31,15 @@ struct Block block_buffer[BLOCK_BUFFER_SIZE]; // A ring buffer for motion instructions volatile int block_buffer_head; // Index of the next block to be pushed volatile int block_buffer_tail; // Index of the block to process now -uint8_t acceleration_management; // Acceleration management active? +uint8_t acceleration_management; // Acceleration management active? +// NOTE: See bottom of this module for a comment outlining the reasoning behind the mathematics of the +// following functions. -// The distance it takes to accelerate from initial_rate to target_rate using the given acceleration + +// Calculates the distance (not time) it takes to accelerate from initial_rate to target_rate using the +// given acceleration: inline double estimate_acceleration_distance(double initial_rate, double target_rate, double acceleration) { return((target_rate*target_rate-initial_rate*initial_rate)/(2L*acceleration)); } @@ -45,7 +49,7 @@ inline double estimate_acceleration_distance(double initial_rate, double target_ // a total travel of distance. This can be used to compute the intersection point between acceleration and // deceleration in the cases where the trapezoid has no plateau (i.e. never reaches maximum speed) -/* + <- some rate that the client must be certain will not exceed the maximum allowable +/* + <- some maximum rate we don't care about /|\ / | \ / | + <- final_rate @@ -59,11 +63,19 @@ inline double intersection_distance(double initial_rate, double final_rate, doub return((2*acceleration*distance-initial_rate*initial_rate+final_rate*final_rate)/(4*acceleration)); } -// See bottom of this module for a comment outlining the reasoning behind the mathematics behind the -// preceding functions. // Calculates trapezoid parameters so that the entry- and exit-speed is compensated by the provided factors. -// In practice both factors must be in the range 0 ... 1.0 +// The factors represent a factor of braking and must be in the range 0.0-1.0. + +/* + +--------+ <- nominal_rate + / \ + nominal_rate*entry_factor -> + \ + | + <- nominal_rate*exit_factor + +-------------+ + time --> +*/ + void calculate_trapezoid_for_block(struct Block *block, double entry_factor, double exit_factor) { block->initial_rate = ceil(block->nominal_rate*entry_factor); int32_t final_rate = ceil(block->nominal_rate*entry_factor); @@ -71,57 +83,63 @@ void calculate_trapezoid_for_block(struct Block *block, double entry_factor, dou int32_t accelerate_steps = ceil(estimate_acceleration_distance(block->initial_rate, block->nominal_rate, acceleration_per_minute)); int32_t decelerate_steps = - estimate_acceleration_distance(block->nominal_rate, final_rate, -acceleration_per_minute); - printString("ir="); printInteger(block->initial_rate); printString("\n\r"); - printString("nr="); printInteger(block->nominal_rate); printString("\n\r"); - printString("rd="); printInteger(block->rate_delta); printString("\n\r"); - printString("aps="); printInteger(acceleration_per_minute); printString("\n\r"); - printString("acs="); printInteger(accelerate_steps); printString("\n\r"); - printString("dcs="); printInteger(decelerate_steps); printString("\n\r"); - printString("ts="); printInteger(block->step_event_count); printString("\n\r"); - // Check if the acceleration and decelleration periods overlap. In that case nominal_speed will - // never be reached but that's okay. Just truncate both periods proportionally so that they - // fit within the allotted step events. + ceil(estimate_acceleration_distance(block->nominal_rate, final_rate, -acceleration_per_minute)); + + // Calculate the size of Plateau of Nominal Rate. int32_t plateau_steps = block->step_event_count-accelerate_steps-decelerate_steps; + + // Is the Plateau of Nominal Rate smaller than nothing? That means no cruising, and we will + // have to use intersection_distance() to calculate when to abort acceleration and start braking + // in order to reach the final_rate exactly at the end of this block. if (plateau_steps < 0) { + plateau_steps = 0; accelerate_steps = ceil( intersection_distance(block->initial_rate, final_rate, acceleration_per_minute, block->step_event_count)); - plateau_steps = 0; - printString("No plateau, so: acs="); printInteger(accelerate_steps); printString("\n\r"); } + block->accelerate_until = accelerate_steps; block->decelerate_after = accelerate_steps+plateau_steps; } -inline double estimate_max_speed(double max_acceleration, double target_velocity, double distance) { - return(sqrt(-2*max_acceleration*distance+target_velocity*target_velocity)); +// Calculates the maximum allowable speed when you must be able to reach target_velocity using the +// acceleration within the allotted distance. +inline double max_allowable_speed(double acceleration, double target_velocity, double distance) { + return(sqrt(target_velocity*target_velocity-2*acceleration*distance)); } -inline double estimate_jerk(struct Block *before, struct Block *after) { - return(max(fabs(before->speed_x-after->speed_x), - max(fabs(before->speed_y-after->speed_y), - fabs(before->speed_z-after->speed_z)))); +// "Junction jerk" in this context is the immediate change in speed at the junction of two blocks. +// This method will calculate the junction jerk as the euclidean distance between the nominal +// velocities of the respective blocks. +inline double junction_jerk(struct Block *before, struct Block *after) { + return(sqrt( + pow(before->speed_x-after->speed_x, 2)+ + pow(before->speed_y-after->speed_y, 2)+ + pow(before->speed_z-after->speed_z, 2)) + ); } -// Builds plan for a single block provided. Returns TRUE if changes were made to this block -// that requires any earlier blocks to be recalculated too. -int8_t build_plan_for_single_block(struct Block *previous, struct Block *current, struct Block *next) { +// The kernel called by recalculate_plan() when scanning the plan from last to first +int8_t planner_reverse_pass_kernel(struct Block *previous, struct Block *current, struct Block *next) { if(!current){return(TRUE);} - double exit_factor; + double entry_factor = 1.0; + double exit_factor; if (next) { exit_factor = next->entry_factor; } else { exit_factor = 0.0; } + // Calculate the entry_factor for the current block. if (previous) { - double jerk = estimate_jerk(previous, current); + // Reduce speed so that junction_jerk is within the maximum allowed + double jerk = junction_jerk(previous, current); if (jerk > settings.max_jerk) { entry_factor = (settings.max_jerk/jerk); } + // If the required deceleration across the block is too rapid, reduce the entry_factor accordingly. if (exit_factornominal_speed*exit_factor, + double max_entry_speed = max_allowable_speed(-settings.acceleration,current->nominal_speed*exit_factor, current->millimeters); double max_entry_factor = max_entry_speed/current->nominal_speed; if (max_entry_factor < entry_factor) { @@ -131,6 +149,7 @@ int8_t build_plan_for_single_block(struct Block *previous, struct Block *current } else { entry_factor = 0.0; } + // Check if we made a difference for this block. If we didn't, the planner can call it quits // here. No need to process any earlier blocks. int8_t keep_going = (entry_factor > current->entry_factor ? TRUE : FALSE); @@ -140,21 +159,33 @@ int8_t build_plan_for_single_block(struct Block *previous, struct Block *current return(keep_going); } -void recalculate_plan() { +// recalculate_plan() needs to go over the current plan twice. Once in reverse and once forward. This +// implements the reverse pass. +void reverse_pass() { int8_t block_index = block_buffer_head; struct Block *block[3] = {NULL, NULL, NULL}; while(block_index != block_buffer_tail) { block[2]= block[1]; block[1]= block[0]; block[0] = &block_buffer[block_index]; - if (!build_plan_for_single_block(block[0], block[1], block[2])) {return;} + if (!planner_reverse_pass_kernel(block[0], block[1], block[2])) {return;} block_index = (block_index-1) % BLOCK_BUFFER_SIZE; } - if (block[1]) { - calculate_trapezoid_for_block(block[0], block[0]->entry_factor, block[1]->entry_factor); + planner_reverse_pass_kernel(NULL, block[0], block[1]); +} + +void forward_pass() { + int8_t block_index = block_buffer_tail; + while(block_index != block_buffer_head) { + block_index = (block_index+1) % BLOCK_BUFFER_SIZE; } } +void recalculate_plan() { + reverse_pass(); + forward_pass(); +} + void plan_enable_acceleration_management() { if (!acceleration_management) { st_synchronize(); @@ -208,11 +239,9 @@ void plan_buffer_line(int32_t steps_x, int32_t steps_y, int32_t steps_z, uint32_ // To generate trapezoids with contant acceleration between blocks the rate_delta must be computed // specifically for each line to compensate for this phenomenon: double travel_per_step = millimeters/block->step_event_count; - printString("travel_per_step*10000="); - printInteger(travel_per_step*10000);printString("\n\r"); block->rate_delta = ceil( ((settings.acceleration*60.0)/(ACCELERATION_TICKS_PER_SECOND))/ // acceleration mm/sec/sec per acceleration_tick - travel_per_step); // convert to: acceleration steps/min/acceleration_tick + travel_per_step); // convert to: acceleration steps/min/acceleration_tick if (acceleration_management) { calculate_trapezoid_for_block(block,0,0); // compute a conservative acceleration trapezoid for now } else { @@ -228,11 +257,12 @@ void plan_buffer_line(int32_t steps_x, int32_t steps_y, int32_t steps_z, uint32_ if (steps_z < 0) { block->direction_bits |= (1<