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:
parent
48b596c2fe
commit
e0f3dcbe43
@ -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));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -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];
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// 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) {
|
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 {
|
} 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],
|
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);
|
||||||
}
|
}
|
||||||
|
287
stepper.c
287
stepper.c
@ -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
|
||||||
@ -286,7 +298,7 @@ void st_init()
|
|||||||
TCCR1A &= ~(3<<COM1B0);
|
TCCR1A &= ~(3<<COM1B0);
|
||||||
|
|
||||||
// Configure Timer 2
|
// Configure Timer 2
|
||||||
TCCR2A = 0; // Normal operation
|
TCCR2A = 0; // Normal operation
|
||||||
TCCR2B = (1<<CS21); // Full speed, 1/8 prescaler
|
TCCR2B = (1<<CS21); // Full speed, 1/8 prescaler
|
||||||
TIMSK2 |= (1<<TOIE2);
|
TIMSK2 |= (1<<TOIE2);
|
||||||
|
|
||||||
@ -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()
|
||||||
|
Loading…
Reference in New Issue
Block a user