diff --git a/acceleration.c b/acceleration.c index f057623..c334b5c 100644 --- a/acceleration.c +++ b/acceleration.c @@ -21,18 +21,18 @@ // Estimate the maximum speed at a given distance when you need to reach the given // target_velocity with max_accelleration. -double estimate_max_speed(double max_accelleration, double target_velocity, double distance) { +float estimate_max_speed(float max_accelleration, float target_velocity, float distance) { return(sqrt(-2*max_accelleration*distance+target_velocity*target_velocity)) } // At what distance must we start accellerating/braking to reach target_speed from current_speed given the // specified constant accelleration. -double estimate_brake_distance(double current_speed, double target_speed, double acceleration) { +float estimate_brake_distance(float current_speed, float target_speed, float acceleration) { return((target_speed*target_speed-current_speed*current_speed)/(2*acceleration)); } // Calculate feed rate in length-units/second for a single axis -double axis_feed_rate(double steps_per_stepping, uint32_t stepping_rate, double steps_per_unit) { +float axis_feed_rate(float steps_per_stepping, uint32_t stepping_rate, float steps_per_unit) { if (stepping_rate == 0) { return(0.0); } return((TICKS_PER_MICROSECOND*1000000)*steps_per_stepping/(stepping_rate*steps_per_unit)); } @@ -40,23 +40,22 @@ double axis_feed_rate(double steps_per_stepping, uint32_t stepping_rate, double // The 'swerve' of a joint is equal to the maximum accelleration of any single // single axis in the corner between the outgoing and the incoming line. Accelleration control // will regulate speed to avoid excessive swerve. - -double calculate_swerve(struct Line* outgoing, struct Line* incoming) { - double x_swerve = abs( +float calculate_swerve(struct Line* outgoing, struct Line* incoming) { + float x_swerve = abs( axis_feed_rate( - ((double)incoming->steps_x)/incoming->maximum_steps, incoming->rate, settings.steps_per_mm[X_AXIS]) + ((float)incoming->steps_x)/incoming->maximum_steps, incoming->rate, settings.steps_per_mm[X_AXIS]) - axis_feed_rate( - ((double)incoming->steps_x)/incoming->maximum_steps, outgoing-> rate, settings.steps_per_mm[X_AXIS])); - double y_swerve = abs( + ((float)incoming->steps_x)/incoming->maximum_steps, outgoing-> rate, settings.steps_per_mm[X_AXIS])); + float y_swerve = abs( axis_feed_rate( - ((double)incoming->steps_y)/incoming->maximum_steps, incoming->rate, settings.steps_per_mm[Y_AXIS]) + ((float)incoming->steps_y)/incoming->maximum_steps, incoming->rate, settings.steps_per_mm[Y_AXIS]) - axis_feed_rate( - ((double)incoming->steps_y)/incoming->maximum_steps, outgoing-> rate, settings.steps_per_mm[Y_AXIS])); - double z_swerve = abs( + ((float)incoming->steps_y)/incoming->maximum_steps, outgoing-> rate, settings.steps_per_mm[Y_AXIS])); + float z_swerve = abs( axis_feed_rate( - ((double)incoming->steps_z)/incoming->maximum_steps, incoming->rate, settings.steps_per_mm[Z_AXIS]) + ((float)incoming->steps_z)/incoming->maximum_steps, incoming->rate, settings.steps_per_mm[Z_AXIS]) - axis_feed_rate( - ((double)incoming->steps_z)/incoming->maximum_steps, outgoing-> rate, settings.steps_per_mm[Z_AXIS])); + ((float)incoming->steps_z)/incoming->maximum_steps, outgoing-> rate, settings.steps_per_mm[Z_AXIS])); return max(x_swerve, max(y_swerve, z_swerve)); } diff --git a/motion_control.c b/motion_control.c index 6b2c391..587c615 100644 --- a/motion_control.c +++ b/motion_control.c @@ -59,14 +59,15 @@ void mc_line(double x, double y, double z, float feed_rate, int invert_feed_rate steps[axis] = target[axis]-position[axis]; } + // Ask old Phytagoras to estimate how many mm our next move is going to take us + double millimeters_of_travel = sqrt( + square(steps[X_AXIS]/settings.steps_per_mm[0]) + + square(steps[Y_AXIS]/settings.steps_per_mm[1]) + + square(steps[Z_AXIS]/settings.steps_per_mm[2])); if (invert_feed_rate) { - st_buffer_line(steps[X_AXIS], steps[Y_AXIS], steps[Z_AXIS], lround(ONE_MINUTE_OF_MICROSECONDS/feed_rate)); + st_buffer_line(steps[X_AXIS], steps[Y_AXIS], steps[Z_AXIS], lround(ONE_MINUTE_OF_MICROSECONDS/feed_rate), + millimeters_of_travel); } else { - // Ask old Phytagoras to estimate how many mm our next move is going to take us - double millimeters_of_travel = sqrt( - square(steps[X_AXIS]/settings.steps_per_mm[0]) + - square(steps[Y_AXIS]/settings.steps_per_mm[1]) + - square(steps[Z_AXIS]/settings.steps_per_mm[2])); st_buffer_line(steps[X_AXIS], steps[Y_AXIS], steps[Z_AXIS], lround((millimeters_of_travel/feed_rate)*1000000), millimeters_of_travel); } diff --git a/stepper.c b/stepper.c index 603ab89..774d134 100644 --- a/stepper.c +++ b/stepper.c @@ -33,65 +33,45 @@ #include "wiring_serial.h" -// Pick a suitable line-buffer size +// Pick a suitable block-buffer size #ifdef __AVR_ATmega328P__ -#define LINE_BUFFER_SIZE 40 // Atmega 328 has one full kilobyte of extra RAM! +#define BLOCK_BUFFER_SIZE 40 // Atmega 328 has one full kilobyte of extra RAM! #else -#define LINE_BUFFER_SIZE 10 +#define BLOCK_BUFFER_SIZE 10 #endif -<<<<<<< Updated upstream -struct Line { - uint32_t steps_x, steps_y, steps_z; - int32_t maximum_steps; - uint8_t direction_bits; - double average_millimeters_per_step_event; - uin32_t ideal_rate; // in step-events/minute - uin32_t exit_rate; - uin32_t brake_point; // the point where braking starts measured in step-events from end point - uint32_t rate; // in cpu-ticks pr. step -}; - -struct Line line_buffer[LINE_BUFFER_SIZE]; // A buffer for step instructions -volatile int line_buffer_head = 0; -volatile int line_buffer_tail = 0; -volatile int moving = FALSE; - -// Variables used by SIG_OUTPUT_COMPARE1A -uint8_t out_bits; // The next stepping-bits to be output -struct Line *current_line; // A pointer to the line currently being traced -volatile int32_t counter_x, - counter_y, counter_z; // counter variables for the bresenham line tracer -uint32_t iterations; // The number of iterations left to complete the current_line -volatile int busy; // TRUE when SIG_OUTPUT_COMPARE1A is being serviced. Used to avoid retriggering that handler. void set_step_events_per_minute(uint32_t steps_per_minute); -uint32_t mm_per_minute_to_step_events_pr_minute(struct Line* line, double mm_per_minute) { - return(mm_per_minute/line->average_millimeters_per_step_event); -} - -void update_accelleration_plan() { +void update_acceleration_plan() { // Store the current - int initial_buffer_tail = line_buffer_tail; + int initial_buffer_tail = block_buffer_tail; } -======= + #define ENABLE_STEPPER_DRIVER_INTERRUPT() TIMSK1 |= (1< +// +// The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates for +// block->accelerate_ticks 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. + +#define TRAPEZOID_STAGE_ACCELERATING 0 +#define TRAPEZOID_STAGE_PLATEAU 1 +#define TRAPEZOID_STAGE_DECELERATING 2 +uint8_t trapezoid_stage = TRAPEZOID_STAGE_IDLE; +uint16_t trapezoid_stage_ticks; +uint32_t trapezoid_rate; +int16_t trapezoid_delta; + +// Call this when a new block is started +inline void reset_trapezoid_generator() { + trapezoid_stage = TRAPEZOID_STAGE_ACCELERATING; + trapezoid_stage_ticks = current_block->accelerate_ticks; + trapezoid_delta = current_block->rate_delta; + trapezoid_rate = current_block->initial_rate; + set_step_events_per_minute(trapezoid_rate); +} + +// This is called ACCELERATION_TICKS_PER_SECOND times per second by the step_event +// interrupt. It can be assumed that the trapezoid-generator-parameters and the +// current_block stays untouched by outside handlers for the duration of this function call. +inline void trapezoid_generator_tick() { + // Is there a block currently in execution? + if(!current_block) {return;} + + if (trapezoid_stage_ticks) { + trapezoid_rate += trapezoid_delta; + trapezoid_stage_ticks--; + set_step_events_per_minute(trapezoid_rate); + } else { + // Stage complete, move on + if(trapezoid_stage == TRAPEZOID_STAGE_ACCELERATING) { + // Progress to plateau stage + trapezoid_delta = 0; + trapezoid_stage_ticks = current_block->plateau_ticks; + trapezoid_stage = TRAPEZOID_STAGE_PLATEAU + } elsif (trapezoid_stage == TRAPEZOID_STAGE_PLATEAU) { + // Progress to deceleration stage + trapezoid_delta = -current_block->rate_delta; + trapezoid_stage_ticks = 0xffff; // "forever" until the block is complete + trapezoid_stage = TRAPEZOID_STAGE_DECELERATING; + } + } +} void config_step_timer(uint32_t microseconds); ->>>>>>> Stashed changes // Add a new linear movement to the buffer. steps_x, _y and _z is the signed, relative motion in -// steps. Microseconds specify how many microseconds the move should take to perform. To aid accelleration +// steps. 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 st_buffer_line(int32_t steps_x, int32_t steps_y, int32_t steps_z, uint32_t microseconds, double millimeters) { // Calculate the buffer head after we push this byte - int next_buffer_head = (line_buffer_head + 1) % LINE_BUFFER_SIZE; + int next_buffer_head = (block_buffer_head + 1) % BLOCK_BUFFER_SIZE; // If the buffer is full: good! That means we are well ahead of the robot. -<<<<<<< Updated upstream - // Nap until there is room in the buffer. - while(line_buffer_tail == next_buffer_head) { sleep_mode(); } - // Setup line record - struct Line *line = &line_buffer[line_buffer_head]; - line->steps_x = labs(steps_x); - line->steps_y = labs(steps_y); - line->steps_z = labs(steps_z); - line->maximum_steps = max(line->steps_x, max(line->steps_y, line->steps_z)); - // Bail if this is a zero-length line - if (line->maximum_steps == 0) { return; }; - line->rate = (TICKS_PER_MICROSECOND*microseconds)/line->maximum_steps; -======= // Rest here until there is room in the buffer. while(block_buffer_tail == next_buffer_head) { sleep_mode(); } // Prepare to set up new block @@ -139,36 +163,23 @@ void st_buffer_line(int32_t steps_x, int32_t steps_y, int32_t steps_z, uint32_t // Number of steps for each axis block->steps_x = labs(steps_x); block->steps_y = labs(steps_y); - block->steps_z = labs(steps_z); - block->maximum_steps = max(block->steps_x, max(block->steps_y, block->steps_z)); + block->steps_z = labs(steps_z); + block->step_event_count = max(block->steps_x, max(block->steps_y, block->steps_z)); +// block->travel_per_step = (1.0*millimeters)/block->step_event_count; // Bail if this is a zero-length block - if (block->maximum_steps == 0) { return; }; - // Rate in steps/second for each axis - double rate_multiplier = 60.0*1000000.0/microseconds; - block->rate_x = round(block->steps_x*rate_multiplier); - block->rate_y = round(block->steps_y*rate_multiplier); - block->rate_z = round(block->steps_z*rate_multiplier); - block->rate = microseconds/block->maximum_steps; + if (block->step_event_count == 0) { return; }; + // Calculate speed in steps/second for each axis + float multiplier = 60.0*1000000.0/microseconds; + block->speed_x = block->steps_x*multiplier/settings.steps_per_mm[0]; + block->speed_y = block->steps_y*multiplier/settings.steps_per_mm[1]; + block->speed_z = block->steps_z*multiplier/settings.steps_per_mm[2]; + block->nominal_rate = round(block->step_event_count*multiplier); // Compute direction bits for this block ->>>>>>> Stashed changes - uint8_t direction_bits = 0; - if (steps_x < 0) { direction_bits |= (1<direction_bits = direction_bits; - line->average_millimeters_per_step_event = millimeters/line->maximum_steps + block->direction_bits = 0; + if (steps_x < 0) { block->direction_bits |= (1<direction_bits |= (1<direction_bits |= (1<>>>>>> Stashed changes #ifdef TIMER1_COMPA_vect SIGNAL(TIMER1_COMPA_vect) #else @@ -186,7 +196,6 @@ SIGNAL(SIG_OUTPUT_COMPARE1A) { if(busy){ return; } // The busy-flag is used to avoid reentering this interrupt - PORTD |= (1<<3); // Set the direction pins a cuple of nanoseconds before we step the steppers STEPPING_PORT = (STEPPING_PORT & ~DIRECTION_MASK) | (out_bits & DIRECTION_MASK); // Then pulse the stepping pins @@ -197,61 +206,64 @@ SIGNAL(SIG_OUTPUT_COMPARE1A) busy = TRUE; sei(); // Re enable interrupts (normally disabled while inside an interrupt handler) - // We re-enable interrupts in order for SIG_OVERFLOW2 to be able to be triggered - // at exactly the right time even if we occasionally spend a lot of time inside this handler. + // ((We re-enable interrupts in order for SIG_OVERFLOW2 to be able to be triggered + // at exactly the right time even if we occasionally spend a lot of time inside this handler.)) - // If there is no current line, attempt to pop one from the buffer - if (current_line == NULL) { - PORTD &= ~(1<<4); + // If there is no current block, attempt to pop one from the buffer + if (current_block == NULL) { // Anything in the buffer? - if (line_buffer_head != line_buffer_tail) { - PORTD ^= (1<<5); + if (block_buffer_head != block_buffer_tail) { // Retrieve a new line and get ready to step it - current_line = &line_buffer[line_buffer_tail]; - config_step_timer(current_line->rate); - counter_x = -(current_line->maximum_steps >> 1); + current_block = &block_buffer[block_buffer_tail]; + reset_trapezoid_generator(); + counter_x = -(current_block->step_event_count >> 1); counter_y = counter_x; counter_z = counter_x; - iterations = current_line->maximum_steps; - moving = TRUE; + iterations = current_block->step_event_count; } else { - // disable this interrupt until there is something to handle - moving = FALSE; - TIMSK1 &= ~(1<direction_bits; - counter_x += current_line->steps_x; + if (current_block != NULL) { + out_bits = current_block->direction_bits; + counter_x += current_block->steps_x; if (counter_x > 0) { out_bits |= (1<maximum_steps; + counter_x -= current_block->step_event_count; } - counter_y += current_line->steps_y; + counter_y += current_block->steps_y; if (counter_y > 0) { out_bits |= (1<maximum_steps; + counter_y -= current_block->step_event_count; } - counter_z += current_line->steps_z; + counter_z += current_block->steps_z; if (counter_z > 0) { out_bits |= (1<maximum_steps; + counter_z -= current_block->step_event_count; } - // If current line is finished, reset pointer + // If current block is finished, reset pointer iterations -= 1; if (iterations <= 0) { - current_line = NULL; - // move the line buffer tail to the next instruction - line_buffer_tail = (line_buffer_tail + 1) % LINE_BUFFER_SIZE; + current_block = NULL; + // move the block buffer tail to the next instruction + block_buffer_tail = (block_buffer_tail + 1) % BLOCK_BUFFER_SIZE; } } else { out_bits = 0; - } + } out_bits ^= settings.invert_mask; + + // In average this generates a trapezoid_generator_tick every CYCLES_PER_ACCELERATION_TICK by keeping track + // of the number of elapsed cycles. The code assumes that step_events occur significantly more often than + // trapezoid_generator_ticks as they well should. + trapezoid_tick_cycle_counter += cycles_per_step_event; + if(trapezoid_tick_cycle_counter > CYCLES_PER_ACCELERATION_TICK) { + trapezoid_tick_cycle_counter -= CYCLES_PER_ACCELERATION_TICK; + trapezoid_generator_tick(); + } + busy=FALSE; - PORTD &= ~(1<<3); } // This interrupt is set up by SIG_OUTPUT_COMPARE1A when it sets the motor port bits. It resets @@ -286,7 +298,7 @@ void st_init() TCCR1A &= ~(3<> 3; + actual_cycles = ceiling; + } else if (cycles <= 0x7ffffL) { + ceiling = cycles >> 3; prescaler = 1; // prescaler: 8 - } else if (ticks <= 0x3fffffL) { - ceiling = ticks >> 6; + actual_cycles = ceiling * 8; + } else if (cycles <= 0x3fffffL) { + ceiling = cycles >> 6; prescaler = 2; // prescaler: 64 - } else if (ticks <= 0xffffffL) { - ceiling = (ticks >> 8); + actual_cycles = ceiling * 64; + } else if (cycles <= 0xffffffL) { + ceiling = (cycles >> 8); prescaler = 3; // prescaler: 256 - } else if (ticks <= 0x3ffffffL) { - ceiling = (ticks >> 10); + actual_cycles = ceiling * 256; + } else if (cycles <= 0x3ffffffL) { + ceiling = (cycles >> 10); prescaler = 4; // prescaler: 1024 + actual_cycles = ceiling * 1024; } else { // Okay, that was slower than we actually go. Just set the slowest speed ceiling = 0xffff; prescaler = 4; + actual_cycles = 0xffff * 1024; } // Set prescaler TCCR1B = (TCCR1B & ~(0x07<