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
This commit is contained in:
Simen Svale Skogsrud
parent
48b596c2fe
commit
e0f3dcbe43
@ -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));
|
||||
}
|
||||
|
||||
|
||||
@ -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);
|
||||
}
|
||||
|
||||
291
stepper.c
291
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<<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 {
|
||||
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)
|
||||
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_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
|
||||
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.
|
||||
uint32_t cycles_per_step_event;
|
||||
uint32_t trapezoid_tick_cycle_counter;
|
||||
|
||||
// Variables used by the accelleration manager
|
||||
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
|
||||
// Values and variables used by the speed trapeziod generator
|
||||
// __________________________
|
||||
// /| |\ _________________ ^
|
||||
// / | | \ /| |\ |
|
||||
// / | | \ / | | \ 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);
|
||||
>>>>>>> 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<<X_DIRECTION_BIT); }
|
||||
if (steps_y < 0) { direction_bits |= (1<<Y_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
|
||||
block->direction_bits = 0;
|
||||
if (steps_x < 0) { block->direction_bits |= (1<<X_DIRECTION_BIT); }
|
||||
if (steps_y < 0) { block->direction_bits |= (1<<Y_DIRECTION_BIT); }
|
||||
if (steps_z < 0) { block->direction_bits |= (1<<Z_DIRECTION_BIT); }
|
||||
// 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;
|
||||
// Ensure that block processing is running by enabling The 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
|
||||
// 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.
|
||||
>>>>>>> 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<<OCIE1A);
|
||||
PORTD |= (1<<4);
|
||||
DISABLE_STEPPER_DRIVER_INTERRUPT();
|
||||
}
|
||||
}
|
||||
|
||||
if (current_line != NULL) {
|
||||
out_bits = current_line->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<<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) {
|
||||
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) {
|
||||
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;
|
||||
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<<COM1B0);
|
||||
|
||||
// Configure Timer 2
|
||||
TCCR2A = 0; // Normal operation
|
||||
TCCR2A = 0; // Normal operation
|
||||
TCCR2B = (1<<CS21); // Full speed, 1/8 prescaler
|
||||
TIMSK2 |= (1<<TOIE2);
|
||||
|
||||
@ -301,51 +313,60 @@ void st_init()
|
||||
// Block until all buffered steps are executed
|
||||
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
|
||||
void st_flush()
|
||||
{
|
||||
cli();
|
||||
line_buffer_tail = line_buffer_head;
|
||||
current_line = NULL;
|
||||
block_buffer_tail = block_buffer_head;
|
||||
current_block = NULL;
|
||||
sei();
|
||||
}
|
||||
|
||||
// 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 prescaler;
|
||||
if (ticks <= 0xffffL) {
|
||||
ceiling = ticks;
|
||||
uint32_t actual_cycles;
|
||||
if (cycles <= 0xffffL) {
|
||||
ceiling = cycles;
|
||||
prescaler = 0; // prescaler: 0
|
||||
} else if (ticks <= 0x7ffffL) {
|
||||
ceiling = ticks >> 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<<CS10)) | ((prescaler+1)<<CS10);
|
||||
// Set ceiling
|
||||
OCR1A = ceiling;
|
||||
return(actual_cycles);
|
||||
}
|
||||
|
||||
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()
|
||||
|
||||
Loading…
Reference in New Issue
Block a user