ManuelW/grbl-LPC-CoreXY
1
0
Fork 0
Code Issues Pull Requests Releases 1 Wiki Activity

stepper.c now has 90% of acelleration support built in except for the planner, still som known conflicts to mark rough spots that need attention later

Browse Source
This commit is contained in:
Simen Svale Skogsrud 2011-01-03 00:36:33 +01:00
parent 48b596c2fe
commit e0f3dcbe43
3 changed files with 176 additions and 155 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
acceleration.c
View File

@ -21,18 +21,18 @@
// Estimate the maximum speed at a given distance when you need to reach the given // Estimate the maximum speed at a given distance when you need to reach the given
// target_velocity with max_accelleration. // 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)) 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 // At what distance must we start accellerating/braking to reach target_speed from current_speed given the
// specified constant accelleration. // 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)); return((target_speed*target_speed-current_speed*current_speed)/(2*acceleration));
} }
// Calculate feed rate in length-units/second for a single axis // 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); } if (stepping_rate == 0) { return(0.0); }
return((TICKS_PER_MICROSECOND*1000000)*steps_per_stepping/(stepping_rate*steps_per_unit)); 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 // 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 // single axis in the corner between the outgoing and the incoming line. Accelleration control
// will regulate speed to avoid excessive swerve. // will regulate speed to avoid excessive swerve.
float calculate_swerve(struct Line* outgoing, struct Line* incoming) {
double calculate_swerve(struct Line* outgoing, struct Line* incoming) { float x_swerve = abs(
double x_swerve = abs(
axis_feed_rate( 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( - axis_feed_rate(
((double)incoming->steps_x)/incoming->maximum_steps, outgoing-> rate, settings.steps_per_mm[X_AXIS])); ((float)incoming->steps_x)/incoming->maximum_steps, outgoing-> rate, settings.steps_per_mm[X_AXIS]));
double y_swerve = abs( float y_swerve = abs(
axis_feed_rate( 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( - axis_feed_rate(
((double)incoming->steps_y)/incoming->maximum_steps, outgoing-> rate, settings.steps_per_mm[Y_AXIS])); ((float)incoming->steps_y)/incoming->maximum_steps, outgoing-> rate, settings.steps_per_mm[Y_AXIS]));
double z_swerve = abs( float z_swerve = abs(
axis_feed_rate( 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( - 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)); return max(x_swerve, max(y_swerve, z_swerve));
} }

7
motion_control.c
View File

@ -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]; steps[axis] = target[axis]-position[axis];
} }
if (invert_feed_rate) {
st_buffer_line(steps[X_AXIS], steps[Y_AXIS], steps[Z_AXIS], lround(ONE_MINUTE_OF_MICROSECONDS/feed_rate));
} else {
// Ask old Phytagoras to estimate how many mm our next move is going to take us // Ask old Phytagoras to estimate how many mm our next move is going to take us
double millimeters_of_travel = sqrt( double millimeters_of_travel = sqrt(
square(steps[X_AXIS]/settings.steps_per_mm[0]) + square(steps[X_AXIS]/settings.steps_per_mm[0]) +
square(steps[Y_AXIS]/settings.steps_per_mm[1]) + square(steps[Y_AXIS]/settings.steps_per_mm[1]) +
square(steps[Z_AXIS]/settings.steps_per_mm[2])); 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),
millimeters_of_travel);
} else {
st_buffer_line(steps[X_AXIS], steps[Y_AXIS], steps[Z_AXIS], st_buffer_line(steps[X_AXIS], steps[Y_AXIS], steps[Z_AXIS],
lround((millimeters_of_travel/feed_rate)*1000000), millimeters_of_travel); lround((millimeters_of_travel/feed_rate)*1000000), millimeters_of_travel);
} }

285
stepper.c
View File

@ -33,65 +33,45 @@
#include "wiring_serial.h" #include "wiring_serial.h"
// Pick a suitable line-buffer size // Pick a suitable block-buffer size
#ifdef __AVR_ATmega328P__ #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 #else
#define LINE_BUFFER_SIZE 10 #define BLOCK_BUFFER_SIZE 10
#endif #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); 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) { void update_acceleration_plan() {
return(mm_per_minute/line->average_millimeters_per_step_event);
}
void update_accelleration_plan() {
// Store the current // Store the current
int initial_buffer_tail = line_buffer_tail; int initial_buffer_tail = block_buffer_tail;
} }
=======
#define ENABLE_STEPPER_DRIVER_INTERRUPT() TIMSK1 |= (1<<OCIE1A) #define ENABLE_STEPPER_DRIVER_INTERRUPT() TIMSK1 |= (1<<OCIE1A)
#define DISABLE_STEPPER_DRIVER_INTERRUPT() TIMSK1 &= ~(1<<OCIE1A) #define DISABLE_STEPPER_DRIVER_INTERRUPT() TIMSK1 &= ~(1<<OCIE1A)
#define CYCLES_PER_ACCELLERATION_TICK (F_CPU) #define ACCELERATION_TICKS_PER_SECOND 10
#define CYCLES_PER_ACCELERATION_TICK ((TICKS_PER_MICROSECOND*1000000)/ACCELERATION_TICKS_PER_SECOND)
// This record is used to buffer the setup for each linear movement // 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.
struct Block { struct Block {
uint32_t steps_x, steps_y, steps_z; // Step count along each axis uint32_t steps_x, steps_y, steps_z; // Step count along each axis
double rate_x, rate_y, rate_z; // Nominal steps/minute for each axis
int32_t maximum_steps; // The largest stepcount of any axis for this block
uint8_t direction_bits; // The direction bit set for this block (refers to *_DIRECTION_BIT in config.h) uint8_t direction_bits; // The direction bit set for this block (refers to *_DIRECTION_BIT in config.h)
uint32_t rate; // The nominal step rate for this block in microseconds/step int32_t step_event_count; // The number of step events required to complete this block
uint32_t nominal_rate; // The nominal step rate for this block in step_events/minute
// Values used for acceleration management
float speed_x, speed_y, speed_z; // Nominal mm/minute for each axis
uint32_t initial_rate; // The jerk-adjusted step rate at start of block
int16_t rate_delta; // The steps/minute to add or subtract when changing speed (must be positive)
uint16_t accelerate_ticks; // The number of acceleration-ticks to accelerate
uint16_t plateau_ticks; // The number of acceleration-ticks to maintain top speed
}; };
struct Block block_buffer[BLOCK_BUFFER_SIZE]; // A buffer for motion instructions struct Block block_buffer[BLOCK_BUFFER_SIZE]; // A ring buffer for motion instructions
volatile int block_buffer_head = 0; // Index of the next block to be pushed volatile int block_buffer_head = 0; // Index of the next block to be pushed
volatile int block_buffer_tail = 0; // Index of the block to process now volatile int block_buffer_tail = 0; // Index of the block to process now
@ -103,35 +83,79 @@ int32_t counter_x,
counter_z; // counter variables for the bresenham line tracer counter_z; // counter variables for the bresenham line tracer
uint32_t iterations; // The number of iterations left to complete the current_block uint32_t iterations; // The number of iterations left to complete the current_block
volatile int busy; // TRUE when SIG_OUTPUT_COMPARE1A is being serviced. Used to avoid retriggering that handler. volatile int busy; // TRUE when SIG_OUTPUT_COMPARE1A is being serviced. Used to avoid retriggering that handler.
uint32_t cycles_per_step_event;
uint32_t trapezoid_tick_cycle_counter;
// Variables used by the accelleration manager // Values and variables used by the speed trapeziod generator
float rate_multiplier; // The current rate multiplier. at 1.0 nominal rates equals actual rates // __________________________
float rate_change_rate; // The amount the rate_multiplier changes each // /| |\ _________________ ^
uint32_t rate_ramp_iterations; // The accelleration iterations for which the current rate ramp is valid // / | | \ /| |\ |
// / | | \ / | | \ s
// / | | | | | \ p
// / | | | | | \ e
// +-----+------------------------+---+--+---------------+----+ e
// | BLOCK 1 | BLOCK 2 | d
//
// time ----->
//
// 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); 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 // 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. // 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) { 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 // 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. // 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. // Rest here until there is room in the buffer.
while(block_buffer_tail == next_buffer_head) { sleep_mode(); } while(block_buffer_tail == next_buffer_head) { sleep_mode(); }
// Prepare to set up new block // Prepare to set up new block
@ -140,35 +164,22 @@ void st_buffer_line(int32_t steps_x, int32_t steps_y, int32_t steps_z, uint32_t
block->steps_x = labs(steps_x); block->steps_x = labs(steps_x);
block->steps_y = labs(steps_y); block->steps_y = labs(steps_y);
block->steps_z = labs(steps_z); block->steps_z = labs(steps_z);
block->maximum_steps = max(block->steps_x, max(block->steps_y, block->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 // Bail if this is a zero-length block
if (block->maximum_steps == 0) { return; }; if (block->step_event_count == 0) { return; };
// Rate in steps/second for each axis // Calculate speed in steps/second for each axis
double rate_multiplier = 60.0*1000000.0/microseconds; float multiplier = 60.0*1000000.0/microseconds;
block->rate_x = round(block->steps_x*rate_multiplier); block->speed_x = block->steps_x*multiplier/settings.steps_per_mm[0];
block->rate_y = round(block->steps_y*rate_multiplier); block->speed_y = block->steps_y*multiplier/settings.steps_per_mm[1];
block->rate_z = round(block->steps_z*rate_multiplier); block->speed_z = block->steps_z*multiplier/settings.steps_per_mm[2];
block->rate = microseconds/block->maximum_steps; block->nominal_rate = round(block->step_event_count*multiplier);
// Compute direction bits for this block // Compute direction bits for this block
>>>>>>> Stashed changes block->direction_bits = 0;
uint8_t direction_bits = 0; if (steps_x < 0) { block->direction_bits |= (1<<X_DIRECTION_BIT); }
if (steps_x < 0) { direction_bits |= (1<<X_DIRECTION_BIT); } if (steps_y < 0) { block->direction_bits |= (1<<Y_DIRECTION_BIT); }
if (steps_y < 0) { direction_bits |= (1<<Y_DIRECTION_BIT); } if (steps_z < 0) { block->direction_bits |= (1<<Z_DIRECTION_BIT); }
if (steps_z < 0) { direction_bits |= (1<<Z_DIRECTION_BIT); }
line->direction_bits = direction_bits;
line->average_millimeters_per_step_event = millimeters/line->maximum_steps
// Move buffer head // Move buffer head
<<<<<<< Updated upstream
line_buffer_head = next_buffer_head;
// enable stepper interrupt
TIMSK1 |= (1<<OCIE1A);
}
// This timer interrupt is executed at the rate set with config_step_timer. It pops one instruction from
// the line_buffer, executes it. Then it starts timer2 in order to reset the motor port after
// five microseconds.
=======
block_buffer_head = next_buffer_head; block_buffer_head = next_buffer_head;
// Ensure that block processing is running by enabling The Stepper Driver Interrupt // Ensure that block processing is running by enabling The Stepper Driver Interrupt
ENABLE_STEPPER_DRIVER_INTERRUPT(); ENABLE_STEPPER_DRIVER_INTERRUPT();
@ -177,7 +188,6 @@ void st_buffer_line(int32_t steps_x, int32_t steps_y, int32_t steps_z, uint32_t
// "The Stepper Driver Interrupt" - This timer interrupt is the workhorse of Grbl. It is executed at the rate set with // "The Stepper Driver Interrupt" - This timer interrupt is the workhorse of Grbl. It is executed at the rate set with
// config_step_timer. It pops blocks from the block_buffer and executes them by pulsing the stepper pins appropriately. // config_step_timer. It pops blocks from the block_buffer and executes them by pulsing the stepper pins appropriately.
// It is supported by The Stepper Port Reset Interrupt which it uses to reset the stepper port after each pulse. // It is supported by The Stepper Port Reset Interrupt which it uses to reset the stepper port after each pulse.
>>>>>>> Stashed changes
#ifdef TIMER1_COMPA_vect #ifdef TIMER1_COMPA_vect
SIGNAL(TIMER1_COMPA_vect) SIGNAL(TIMER1_COMPA_vect)
#else #else
@ -186,7 +196,6 @@ SIGNAL(SIG_OUTPUT_COMPARE1A)
{ {
if(busy){ return; } // The busy-flag is used to avoid reentering this interrupt 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 // Set the direction pins a cuple of nanoseconds before we step the steppers
STEPPING_PORT = (STEPPING_PORT & ~DIRECTION_MASK) | (out_bits & DIRECTION_MASK); STEPPING_PORT = (STEPPING_PORT & ~DIRECTION_MASK) | (out_bits & DIRECTION_MASK);
// Then pulse the stepping pins // Then pulse the stepping pins
@ -197,61 +206,64 @@ SIGNAL(SIG_OUTPUT_COMPARE1A)
busy = TRUE; busy = TRUE;
sei(); // Re enable interrupts (normally disabled while inside an interrupt handler) 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 // ((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. // 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 there is no current block, attempt to pop one from the buffer
if (current_line == NULL) { if (current_block == NULL) {
PORTD &= ~(1<<4);
// Anything in the buffer? // Anything in the buffer?
if (line_buffer_head != line_buffer_tail) { if (block_buffer_head != block_buffer_tail) {
PORTD ^= (1<<5);
// Retrieve a new line and get ready to step it // Retrieve a new line and get ready to step it
current_line = &line_buffer[line_buffer_tail]; current_block = &block_buffer[block_buffer_tail];
config_step_timer(current_line->rate); reset_trapezoid_generator();
counter_x = -(current_line->maximum_steps >> 1); counter_x = -(current_block->step_event_count >> 1);
counter_y = counter_x; counter_y = counter_x;
counter_z = counter_x; counter_z = counter_x;
iterations = current_line->maximum_steps; iterations = current_block->step_event_count;
moving = TRUE;
} else { } else {
// disable this interrupt until there is something to handle DISABLE_STEPPER_DRIVER_INTERRUPT();
moving = FALSE;
TIMSK1 &= ~(1<<OCIE1A);
PORTD |= (1<<4);
} }
} }
if (current_line != NULL) { if (current_block != NULL) {
out_bits = current_line->direction_bits; out_bits = current_block->direction_bits;
counter_x += current_line->steps_x; counter_x += current_block->steps_x;
if (counter_x > 0) { if (counter_x > 0) {
out_bits |= (1<<X_STEP_BIT); out_bits |= (1<<X_STEP_BIT);
counter_x -= current_line->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) { if (counter_y > 0) {
out_bits |= (1<<Y_STEP_BIT); out_bits |= (1<<Y_STEP_BIT);
counter_y -= current_line->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) { if (counter_z > 0) {
out_bits |= (1<<Z_STEP_BIT); out_bits |= (1<<Z_STEP_BIT);
counter_z -= current_line->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; iterations -= 1;
if (iterations <= 0) { if (iterations <= 0) {
current_line = NULL; current_block = NULL;
// move the line buffer tail to the next instruction // move the block buffer tail to the next instruction
line_buffer_tail = (line_buffer_tail + 1) % LINE_BUFFER_SIZE; block_buffer_tail = (block_buffer_tail + 1) % BLOCK_BUFFER_SIZE;
} }
} else { } else {
out_bits = 0; out_bits = 0;
} }
out_bits ^= settings.invert_mask; 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; busy=FALSE;
PORTD &= ~(1<<3);
} }
// This interrupt is set up by SIG_OUTPUT_COMPARE1A when it sets the motor port bits. It resets // This interrupt is set up by SIG_OUTPUT_COMPARE1A when it sets the motor port bits. It resets
@ -301,51 +313,60 @@ void st_init()
// Block until all buffered steps are executed // Block until all buffered steps are executed
void st_synchronize() void st_synchronize()
{ {
while(line_buffer_tail != line_buffer_head) { sleep_mode(); } while(block_buffer_tail != block_buffer_head) { sleep_mode(); }
} }
// Cancel all buffered steps // Cancel all buffered steps
void st_flush() void st_flush()
{ {
cli(); cli();
line_buffer_tail = line_buffer_head; block_buffer_tail = block_buffer_head;
current_line = NULL; current_block = NULL;
sei(); sei();
} }
// Configures the prescaler and ceiling of timer 1 to produce the given rate as accurately as possible. // Configures the prescaler and ceiling of timer 1 to produce the given rate as accurately as possible.
void config_step_timer(uint32_t ticks) // Returns the actual number of cycles per interrupt
uint32_t config_step_timer(uint32_t cycles)
{ {
uint16_t ceiling; uint16_t ceiling;
uint16_t prescaler; uint16_t prescaler;
if (ticks <= 0xffffL) { uint32_t actual_cycles;
ceiling = ticks; if (cycles <= 0xffffL) {
ceiling = cycles;
prescaler = 0; // prescaler: 0 prescaler = 0; // prescaler: 0
} else if (ticks <= 0x7ffffL) { actual_cycles = ceiling;
ceiling = ticks >> 3; } else if (cycles <= 0x7ffffL) {
ceiling = cycles >> 3;
prescaler = 1; // prescaler: 8 prescaler = 1; // prescaler: 8
} else if (ticks <= 0x3fffffL) { actual_cycles = ceiling * 8;
ceiling = ticks >> 6; } else if (cycles <= 0x3fffffL) {
ceiling = cycles >> 6;
prescaler = 2; // prescaler: 64 prescaler = 2; // prescaler: 64
} else if (ticks <= 0xffffffL) { actual_cycles = ceiling * 64;
ceiling = (ticks >> 8); } else if (cycles <= 0xffffffL) {
ceiling = (cycles >> 8);
prescaler = 3; // prescaler: 256 prescaler = 3; // prescaler: 256
} else if (ticks <= 0x3ffffffL) { actual_cycles = ceiling * 256;
ceiling = (ticks >> 10); } else if (cycles <= 0x3ffffffL) {
ceiling = (cycles >> 10);
prescaler = 4; // prescaler: 1024 prescaler = 4; // prescaler: 1024
actual_cycles = ceiling * 1024;
} else { } else {
// Okay, that was slower than we actually go. Just set the slowest speed // Okay, that was slower than we actually go. Just set the slowest speed
ceiling = 0xffff; ceiling = 0xffff;
prescaler = 4; prescaler = 4;
actual_cycles = 0xffff * 1024;
} }
// Set prescaler // Set prescaler
TCCR1B = (TCCR1B & ~(0x07<<CS10)) | ((prescaler+1)<<CS10); TCCR1B = (TCCR1B & ~(0x07<<CS10)) | ((prescaler+1)<<CS10);
// Set ceiling // Set ceiling
OCR1A = ceiling; OCR1A = ceiling;
return(actual_cycles);
} }
void set_step_events_per_minute(uint32_t steps_per_minute) { void set_step_events_per_minute(uint32_t steps_per_minute) {
config_step_timer((TICKS_PER_MICROSECOND*1000000*60)/steps_per_minute); cycles_per_step_event = config_step_timer((TICKS_PER_MICROSECOND*1000000*60)/steps_per_minute);
} }
void st_go_home() void st_go_home()