added basic accelleration management with trapezoid accelleration profiles but no look ahead optimization (coming next patch)
This commit is contained in:
Simen Svale Skogsrud
parent
e0f3dcbe43
commit
b628a4aabf
2
Makefile
2
Makefile
@ -31,7 +31,7 @@ DEVICE = atmega328p
|
||||
CLOCK = 16000000
|
||||
PROGRAMMER = -c avrisp2 -P usb
|
||||
OBJECTS = main.o motion_control.o gcode.o spindle_control.o wiring_serial.o serial_protocol.o stepper.o \
|
||||
eeprom.o config.o accelleration.o
|
||||
eeprom.o config.o
|
||||
# FUSES = -U hfuse:w:0xd9:m -U lfuse:w:0x24:m
|
||||
FUSES = -U hfuse:w:0xd2:m -U lfuse:w:0xff:m
|
||||
|
||||
|
||||
@ -27,20 +27,20 @@
|
||||
#endif
|
||||
|
||||
struct AccellerationProfile {
|
||||
float initial_scaler;
|
||||
float final_scaler;
|
||||
float accelleration_delta;
|
||||
float decelleration_delta;
|
||||
double initial_scaler;
|
||||
double final_scaler;
|
||||
double accelleration_delta;
|
||||
double decelleration_delta;
|
||||
uint32_t accellerate_ticks;
|
||||
uint32_t plateau_ticks;
|
||||
};
|
||||
|
||||
struct AccellerationProfileSegment {
|
||||
float v_entry[3];
|
||||
float v_ideal[3];
|
||||
float v_exit[3];
|
||||
float distance;
|
||||
float f_entry, f_exit;
|
||||
double v_entry[3];
|
||||
double v_ideal[3];
|
||||
double v_exit[3];
|
||||
double distance;
|
||||
double f_entry, f_exit;
|
||||
};
|
||||
|
||||
struct AccellerationProfileBuilder {
|
||||
|
||||
39
config.c
39
config.c
@ -32,19 +32,10 @@ void reset_settings() {
|
||||
settings.steps_per_mm[2] = Z_STEPS_PER_MM;
|
||||
settings.pulse_microseconds = STEP_PULSE_MICROSECONDS;
|
||||
settings.default_feed_rate = DEFAULT_FEEDRATE;
|
||||
<<<<<<< Updated upstream
|
||||
settings.default_seek_rate = RAPID_FEEDRATE;
|
||||
settings.dead_feed_rate = DEFAULT_FEEDRATE/5;
|
||||
settings.acceleration = DEFAULT_FEEDRATE/100;
|
||||
settings.acceleration = DEFAULT_ACCELERATION;
|
||||
settings.mm_per_arc_segment = MM_PER_ARC_SEGMENT;
|
||||
settings.invert_mask = STEPPING_INVERT_MASK;
|
||||
=======
|
||||
settings.seek_rate = DEFAULT_SEEKRATE;
|
||||
settings.mm_per_arc_segment = DEFAULT_MM_PER_ARC_SEGMENT;
|
||||
settings.invert_mask = 0;
|
||||
settings.max_jerk = DEFAULT_MAX_JERK;
|
||||
settings.accelleration = DEFAULT_ACCELLERATION;
|
||||
>>>>>>> Stashed changes
|
||||
}
|
||||
|
||||
void dump_settings() {
|
||||
@ -53,15 +44,13 @@ void dump_settings() {
|
||||
printPgmString(PSTR(" (steps/mm y)\r\n$2 = ")); printFloat(settings.steps_per_mm[2]);
|
||||
printPgmString(PSTR(" (steps/mm z)\r\n$3 = ")); printInteger(settings.pulse_microseconds);
|
||||
printPgmString(PSTR(" (microseconds step pulse)\r\n$4 = ")); printFloat(settings.default_feed_rate);
|
||||
printPgmString(PSTR(" (mm/sec default feed rate)\r\n$5 = ")); printFloat(settings.default_seek_rate);
|
||||
printPgmString(PSTR(" (mm/sec default seek rate)\r\n$7 = ")); printFloat(settings.dead_feed_rate);
|
||||
printPgmString(PSTR(" (mm/sec max start and stop feed rate)\r\n$8 = ")); printFloat(settings.mm_per_arc_segment);
|
||||
printPgmString(PSTR(" (mm/sec^2 max acceleration)\r\n$9 = ")); printFloat(settings.acceleration);
|
||||
printPgmString(PSTR(" (mm/arc segment)\r\n$10 = ")); printInteger(settings.invert_mask);
|
||||
printPgmString(PSTR(" (mm/min default feed rate)\r\n$5 = ")); printFloat(settings.default_seek_rate);
|
||||
printPgmString(PSTR(" (mm/min default seek rate)\r\n$6 = ")); printFloat(settings.mm_per_arc_segment);
|
||||
printPgmString(PSTR(" (mm/min^2 max acceleration)\r\n$7 = ")); printFloat(settings.acceleration);
|
||||
printPgmString(PSTR(" (mm/arc segment)\r\n$8 = ")); printInteger(settings.invert_mask);
|
||||
printPgmString(PSTR(" (step port invert mask. binary = ")); printIntegerInBase(settings.invert_mask, 2);
|
||||
printPgmString(PSTR(")\n\r$8 = ")); printFloat(settings.max_jerk);
|
||||
printPgmString(PSTR(" (max jerk in delta mm/second)\r\n$9 = ")); printFloat(settings.accelleration);
|
||||
printPgmString(PSTR(" (accelleration in mm/second^2)"));
|
||||
printPgmString(PSTR(")\r\n$9 = ")); printFloat(settings.acceleration);
|
||||
printPgmString(PSTR(" (acceleration in mm/min^2)"));
|
||||
printPgmString(PSTR("\r\n'$x=value' to set parameter or just '$' to dump current settings\r\n"));
|
||||
}
|
||||
|
||||
@ -88,19 +77,11 @@ void store_setting(int parameter, double value) {
|
||||
settings.steps_per_mm[parameter] = value; break;
|
||||
case 3: settings.pulse_microseconds = round(value); break;
|
||||
case 4: settings.default_feed_rate = value; break;
|
||||
<<<<<<< Updated upstream
|
||||
case 5: settings.default_seek_rate = value; break;
|
||||
case 6: settings.dead_feed_rate = value; break;
|
||||
case 8: settings.mm_per_arc_segment = value; break;
|
||||
case 9: settings.acceleration = value; break;
|
||||
case 10: settings.invert_mask = trunc(value); break;
|
||||
=======
|
||||
case 5: settings.seek_rate = value; break;
|
||||
case 6: settings.mm_per_arc_segment = value; break;
|
||||
case 7: settings.invert_mask = trunc(value); break;
|
||||
case 8: settings.max_jerk = value; break;
|
||||
case 9: settings.accelleration = fabs(value); break;
|
||||
>>>>>>> Stashed changes
|
||||
case 7: settings.acceleration = value; break;
|
||||
case 8: settings.invert_mask = trunc(value); break;
|
||||
case 9: settings.acceleration = fabs(value); break;
|
||||
default:
|
||||
printPgmString(PSTR("Unknown parameter\r\n"));
|
||||
return;
|
||||
|
||||
9
config.h
9
config.h
@ -71,11 +71,7 @@ struct Settings {
|
||||
double default_seek_rate;
|
||||
uint8_t invert_mask;
|
||||
double mm_per_arc_segment;
|
||||
<<<<<<< Updated upstream
|
||||
=======
|
||||
double max_jerk;
|
||||
>>>>>>> Stashed changes
|
||||
double accelleration;
|
||||
double acceleration;
|
||||
};
|
||||
struct Settings settings;
|
||||
|
||||
@ -100,8 +96,7 @@ void store_setting(int parameter, double value);
|
||||
|
||||
#define RAPID_FEEDRATE 480.0 // in millimeters per minute
|
||||
#define DEFAULT_FEEDRATE 480.0
|
||||
#define DEFAULT_MAX_JERK 10.0
|
||||
#define DEFAULT_ACCELLERATION 0.1
|
||||
#define DEFAULT_ACCELERATION (DEFAULT_FEEDRATE/100.0)
|
||||
|
||||
// Use this line for default operation (step-pulses high)
|
||||
#define STEPPING_INVERT_MASK 0
|
||||
|
||||
4
gcode.c
4
gcode.c
@ -116,7 +116,7 @@ void gc_init() {
|
||||
gc.absolute_mode = TRUE;
|
||||
}
|
||||
|
||||
inline float to_millimeters(double value) {
|
||||
inline double to_millimeters(double value) {
|
||||
return(gc.inches_mode ? (value * INCHES_PER_MM) : value);
|
||||
}
|
||||
|
||||
@ -138,7 +138,7 @@ double theta(double x, double y)
|
||||
}
|
||||
|
||||
// Executes one line of 0-terminated G-Code. The line is assumed to contain only uppercase
|
||||
// characters and signed floats (no whitespace).
|
||||
// characters and signed floating point values (no whitespace).
|
||||
uint8_t gc_execute_line(char *line) {
|
||||
int counter = 0;
|
||||
char letter;
|
||||
|
||||
@ -45,7 +45,7 @@ void mc_dwell(uint32_t milliseconds)
|
||||
// Execute linear motion in absolute millimeter coordinates. Feed rate given in millimeters/second
|
||||
// unless invert_feed_rate is true. Then the feed_rate means that the motion should be completed in
|
||||
// 1/feed_rate minutes.
|
||||
void mc_line(double x, double y, double z, float feed_rate, int invert_feed_rate)
|
||||
void mc_line(double x, double y, double z, double feed_rate, int invert_feed_rate)
|
||||
{
|
||||
uint8_t axis; // loop variable
|
||||
int32_t target[3]; // The target position in absolute steps
|
||||
@ -108,8 +108,7 @@ void mc_arc(double theta, double angular_travel, double radius, double linear_tr
|
||||
theta += theta_per_segment;
|
||||
target[axis_1] = center_x+sin(theta)*radius;
|
||||
target[axis_2] = center_y+cos(theta)*radius;
|
||||
mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], feed_rate, invert_feed_rate,
|
||||
settings.mm_per_arc_segment);
|
||||
mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], feed_rate, invert_feed_rate);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -29,7 +29,7 @@ void mc_init();
|
||||
// Execute linear motion in absolute millimeter coordinates. Feed rate given in millimeters/second
|
||||
// unless invert_feed_rate is true. Then the feed_rate means that the motion should be completed in
|
||||
// (1 minute)/feed_rate time.
|
||||
void mc_line(double x, double y, double z, float feed_rate, int invert_feed_rate);
|
||||
void mc_line(double x, double y, double z, double feed_rate, int invert_feed_rate);
|
||||
|
||||
// Execute an arc. theta == start angle, angular_travel == number of radians to go along the arc,
|
||||
// positive angular_travel means clockwise, negative means counterclockwise. Radius == the radius of the
|
||||
|
||||
@ -20,42 +20,42 @@
|
||||
|
||||
|
||||
// Estimate the maximum speed at a given distance when you need to reach the given
|
||||
// target_velocity with max_accelleration.
|
||||
float estimate_max_speed(float max_accelleration, float target_velocity, float distance) {
|
||||
return(sqrt(-2*max_accelleration*distance+target_velocity*target_velocity))
|
||||
// target_velocity with max_acceleration.
|
||||
double estimate_max_speed(double max_acceleration, double target_velocity, double distance) {
|
||||
return(sqrt(-2*max_acceleration*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.
|
||||
float estimate_brake_distance(float current_speed, float target_speed, float acceleration) {
|
||||
// At what distance must we start accelerating/braking to reach target_speed from current_speed given the
|
||||
// specified constant acceleration.
|
||||
double estimate_acceleration_distance(double current_speed, double target_speed, double acceleration) {
|
||||
return((target_speed*target_speed-current_speed*current_speed)/(2*acceleration));
|
||||
}
|
||||
|
||||
// Calculate feed rate in length-units/second for a single axis
|
||||
float axis_feed_rate(float steps_per_stepping, uint32_t stepping_rate, float steps_per_unit) {
|
||||
double axis_feed_rate(double steps_per_stepping, uint32_t stepping_rate, double steps_per_unit) {
|
||||
if (stepping_rate == 0) { return(0.0); }
|
||||
return((TICKS_PER_MICROSECOND*1000000)*steps_per_stepping/(stepping_rate*steps_per_unit));
|
||||
}
|
||||
|
||||
// The 'swerve' of a joint is equal to the maximum accelleration of any single
|
||||
// The 'swerve' of a joint is equal to the maximum acceleration of any single
|
||||
// single axis in the corner between the outgoing and the incoming line. Accelleration control
|
||||
// will regulate speed to avoid excessive swerve.
|
||||
float calculate_swerve(struct Line* outgoing, struct Line* incoming) {
|
||||
float x_swerve = abs(
|
||||
double calculate_swerve(struct Line* outgoing, struct Line* incoming) {
|
||||
double x_swerve = abs(
|
||||
axis_feed_rate(
|
||||
((float)incoming->steps_x)/incoming->maximum_steps, incoming->rate, settings.steps_per_mm[X_AXIS])
|
||||
((double)incoming->steps_x)/incoming->maximum_steps, incoming->rate, settings.steps_per_mm[X_AXIS])
|
||||
- axis_feed_rate(
|
||||
((float)incoming->steps_x)/incoming->maximum_steps, outgoing-> rate, settings.steps_per_mm[X_AXIS]));
|
||||
float y_swerve = abs(
|
||||
((double)incoming->steps_x)/incoming->maximum_steps, outgoing-> rate, settings.steps_per_mm[X_AXIS]));
|
||||
double y_swerve = abs(
|
||||
axis_feed_rate(
|
||||
((float)incoming->steps_y)/incoming->maximum_steps, incoming->rate, settings.steps_per_mm[Y_AXIS])
|
||||
((double)incoming->steps_y)/incoming->maximum_steps, incoming->rate, settings.steps_per_mm[Y_AXIS])
|
||||
- axis_feed_rate(
|
||||
((float)incoming->steps_y)/incoming->maximum_steps, outgoing-> rate, settings.steps_per_mm[Y_AXIS]));
|
||||
float z_swerve = abs(
|
||||
((double)incoming->steps_y)/incoming->maximum_steps, outgoing-> rate, settings.steps_per_mm[Y_AXIS]));
|
||||
double z_swerve = abs(
|
||||
axis_feed_rate(
|
||||
((float)incoming->steps_z)/incoming->maximum_steps, incoming->rate, settings.steps_per_mm[Z_AXIS])
|
||||
((double)incoming->steps_z)/incoming->maximum_steps, incoming->rate, settings.steps_per_mm[Z_AXIS])
|
||||
- axis_feed_rate(
|
||||
((float)incoming->steps_z)/incoming->maximum_steps, outgoing-> rate, settings.steps_per_mm[Z_AXIS]));
|
||||
((double)incoming->steps_z)/incoming->maximum_steps, outgoing-> rate, settings.steps_per_mm[Z_AXIS]));
|
||||
return max(x_swerve, max(y_swerve, z_swerve));
|
||||
}
|
||||
|
||||
10
nuts_bolts.h
10
nuts_bolts.h
@ -41,10 +41,10 @@
|
||||
#define Y_AXIS 1
|
||||
#define Z_AXIS 2
|
||||
|
||||
void scale_vector(float *target, float *source, float multiplier) {
|
||||
target[0] = source[0]*multiplier;
|
||||
target[1] = source[1]*multiplier;
|
||||
target[2] = source[2]*multiplier;
|
||||
}
|
||||
// void scale_vector(double *target, double *source, double multiplier) {
|
||||
// target[0] = source[0]*multiplier;
|
||||
// target[1] = source[1]*multiplier;
|
||||
// target[2] = source[2]*multiplier;
|
||||
// }
|
||||
|
||||
#endif
|
||||
|
||||
@ -1,3 +1,4 @@
|
||||
socat -d -d READLINE /dev/tty.usbserial-A9007QcR,clocal=1,nonblock=1,cread=1,cs8,ixon=1,ixoff=1
|
||||
socat -d -d READLINE /dev/tty.usbserial-A700e0GO,clocal=1,nonblock=1,cread=1,cs8,ixon=1,ixoff=1
|
||||
|
||||
#socat -d -d READLINE /dev/tty.FireFly-A964-SPP-1,clocal=1,nonblock=1,cread=1,cs8,ixon=1,ixoff=1
|
||||
|
||||
|
||||
@ -25,7 +25,7 @@ if ARGV.empty?
|
||||
end
|
||||
|
||||
# SerialPort.open('/dev/tty.FireFly-A964-SPP-1', 115200) do |sp|
|
||||
SerialPort.open('/dev/tty.usbserial-A9007QcR', 9600) do |sp|
|
||||
SerialPort.open('/dev/tty.usbserial-A700e0GO', 9600) do |sp|
|
||||
|
||||
sp.write("\r\n\r\n");
|
||||
sleep 1
|
||||
|
||||
103
stepper.c
103
stepper.c
@ -28,7 +28,6 @@
|
||||
#include <stdlib.h>
|
||||
#include <util/delay.h>
|
||||
#include "nuts_bolts.h"
|
||||
#include "acceleration.h"
|
||||
#include <avr/interrupt.h>
|
||||
|
||||
#include "wiring_serial.h"
|
||||
@ -43,16 +42,11 @@
|
||||
|
||||
void set_step_events_per_minute(uint32_t steps_per_minute);
|
||||
|
||||
void update_acceleration_plan() {
|
||||
// Store the current
|
||||
int initial_buffer_tail = block_buffer_tail;
|
||||
|
||||
}
|
||||
|
||||
#define ENABLE_STEPPER_DRIVER_INTERRUPT() TIMSK1 |= (1<<OCIE1A)
|
||||
#define DISABLE_STEPPER_DRIVER_INTERRUPT() TIMSK1 &= ~(1<<OCIE1A)
|
||||
|
||||
#define ACCELERATION_TICKS_PER_SECOND 10
|
||||
#define MINIMAL_STEP_RATE (ACCELERATION_TICKS_PER_SECOND*5)
|
||||
#define CYCLES_PER_ACCELERATION_TICK ((TICKS_PER_MICROSECOND*1000000)/ACCELERATION_TICKS_PER_SECOND)
|
||||
|
||||
// This struct is used when buffering the setup for each linear movement
|
||||
@ -64,7 +58,7 @@ struct Block {
|
||||
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
|
||||
double 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
|
||||
@ -99,18 +93,58 @@ uint32_t trapezoid_tick_cycle_counter;
|
||||
// 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.
|
||||
// block->accelerate_ticks by block->rate_delta each tick, then stays up for block->plateau_ticks and
|
||||
// decelerates for the rest of the block until the trapezoid generator is reset for the next block.
|
||||
// The slope of acceleration is always +/- block->rate_delta. Any stage may be skipped by setting the
|
||||
// duration to 0 ticks.
|
||||
|
||||
#define TRAPEZOID_STAGE_ACCELERATING 0
|
||||
#define TRAPEZOID_STAGE_PLATEAU 1
|
||||
#define TRAPEZOID_STAGE_DECELERATING 2
|
||||
uint8_t trapezoid_stage = TRAPEZOID_STAGE_IDLE;
|
||||
uint8_t trapezoid_stage;
|
||||
uint16_t trapezoid_stage_ticks;
|
||||
uint32_t trapezoid_rate;
|
||||
int16_t trapezoid_delta;
|
||||
|
||||
inline uint32_t estimate_acceleration_distance(int32_t current_rate, int32_t target_rate, int32_t acceleration) {
|
||||
return((target_rate*target_rate-current_rate*current_rate)/(2*acceleration));
|
||||
}
|
||||
|
||||
inline uint32_t estimate_acceleration_ticks(int32_t start_rate, int32_t acceleration_per_tick, int32_t step_events) {
|
||||
return(
|
||||
round(
|
||||
(sqrt(2*acceleration_per_tick*step_events+(start_rate*start_rate))-start_rate)/
|
||||
acceleration_per_tick));
|
||||
}
|
||||
|
||||
// Calculates trapezoid parameters so that the entry- and exit-speed is compensated by the provided factors.
|
||||
// In practice both factors must be in the range 0 ... 1.0
|
||||
void calculate_trapezoid_for_block(struct Block *block, double entry_factor, double exit_factor) {
|
||||
block->initial_rate = max(round(block->nominal_rate*entry_factor),MINIMAL_STEP_RATE);
|
||||
int32_t final_rate = max(round(block->nominal_rate*entry_factor),MINIMAL_STEP_RATE);
|
||||
int32_t acceleration_per_second = block->rate_delta*ACCELERATION_TICKS_PER_SECOND;
|
||||
int32_t acceleration_steps =
|
||||
estimate_acceleration_distance(block->initial_rate, block->nominal_rate, acceleration_per_second);
|
||||
int32_t decelleration_steps =
|
||||
estimate_acceleration_distance(block->nominal_rate, final_rate, -acceleration_per_second);
|
||||
// Check if the acceleration and decelleration periods overlap. In that case nominal_speed will
|
||||
// never be reached but that's okay. Just truncate both periods proportionally so that they
|
||||
// fit within the allotted step events.
|
||||
int32_t plateau_steps = block->step_event_count-acceleration_steps-decelleration_steps;
|
||||
if (plateau_steps < 0) {
|
||||
int32_t half_overlap_region = abs(plateau_steps)/2;
|
||||
plateau_steps = 0;
|
||||
acceleration_steps = max(acceleration_steps-half_overlap_region,0);
|
||||
decelleration_steps = max(decelleration_steps-half_overlap_region,0);
|
||||
}
|
||||
block->accelerate_ticks = estimate_acceleration_ticks(block->initial_rate, block->rate_delta, acceleration_steps);
|
||||
if (plateau_steps) {
|
||||
block->plateau_ticks = round(1.0*plateau_steps/(block->nominal_rate*ACCELERATION_TICKS_PER_SECOND));
|
||||
} else {
|
||||
block->plateau_ticks = 0;
|
||||
}
|
||||
}
|
||||
|
||||
// Call this when a new block is started
|
||||
inline void reset_trapezoid_generator() {
|
||||
trapezoid_stage = TRAPEZOID_STAGE_ACCELERATING;
|
||||
@ -124,21 +158,22 @@ inline void reset_trapezoid_generator() {
|
||||
// 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() {
|
||||
if (trapezoid_stage_ticks) {
|
||||
trapezoid_stage_ticks--;
|
||||
if (trapezoid_delta) {
|
||||
trapezoid_rate += trapezoid_delta;
|
||||
set_step_events_per_minute(trapezoid_rate);
|
||||
}
|
||||
} else {
|
||||
// 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
|
||||
// Trapezoid 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) {
|
||||
trapezoid_stage = TRAPEZOID_STAGE_PLATEAU;
|
||||
} else if (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
|
||||
@ -147,8 +182,6 @@ inline void trapezoid_generator_tick() {
|
||||
}
|
||||
}
|
||||
|
||||
void config_step_timer(uint32_t microseconds);
|
||||
|
||||
// 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 acceleration
|
||||
// calculation the caller must also provide the physical length of the line in millimeters.
|
||||
@ -165,15 +198,27 @@ void st_buffer_line(int32_t steps_x, int32_t steps_y, int32_t steps_z, uint32_t
|
||||
block->steps_y = labs(steps_y);
|
||||
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->step_event_count == 0) { return; };
|
||||
// Calculate speed in steps/second for each axis
|
||||
float multiplier = 60.0*1000000.0/microseconds;
|
||||
// Calculate speed in mm/minute for each axis
|
||||
double 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);
|
||||
block->nominal_rate = max(round(block->step_event_count*multiplier), MINIMAL_STEP_RATE);
|
||||
|
||||
// Compute the acceleration rate for the trapezoid generator. Depending on the slope of the line
|
||||
// average travel per step event changes. For a line along one axis the travel per step event
|
||||
// is equal to the travel/step in the particular axis. For a 45 degree line the steppers of both
|
||||
// axes might step for every step event. Travel per step event is then sqrt(travel_x^2+travel_y^2).
|
||||
// To generate trapezoids with contant acceleration between blocks the rate_delta must be computed
|
||||
// specifically for each line to compensate for this phenomenon:
|
||||
double travel_per_step = (1.0*millimeters)/block->step_event_count;
|
||||
block->rate_delta = round(
|
||||
(settings.acceleration/(60.0*ACCELERATION_TICKS_PER_SECOND))/ // acceleration mm/min per acceleration_tick
|
||||
travel_per_step); // convert to: acceleration steps/min/acceleration_tick
|
||||
calculate_trapezoid_for_block(block,0,0); // compute a default trapezoid
|
||||
|
||||
// Compute direction bits for this block
|
||||
block->direction_bits = 0;
|
||||
if (steps_x < 0) { block->direction_bits |= (1<<X_DIRECTION_BIT); }
|
||||
@ -302,8 +347,8 @@ void st_init()
|
||||
TCCR2B = (1<<CS21); // Full speed, 1/8 prescaler
|
||||
TIMSK2 |= (1<<TOIE2);
|
||||
|
||||
// Just set the step_timer to something serviceably lazy
|
||||
config_step_timer(20000);
|
||||
DISABLE_STEPPER_DRIVER_INTERRUPT();
|
||||
|
||||
// set enable pin
|
||||
STEPPERS_ENABLE_PORT |= 1<<STEPPERS_ENABLE_BIT;
|
||||
|
||||
|
||||
@ -29,7 +29,7 @@ void st_init();
|
||||
|
||||
// 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.
|
||||
void st_buffer_line(int32_t steps_x, int32_t steps_y, int32_t steps_z, uint32_t rate);
|
||||
void st_buffer_line(int32_t steps_x, int32_t steps_y, int32_t steps_z, uint32_t rate, double millimeters);
|
||||
|
||||
// Block until all buffered steps are executed
|
||||
void st_synchronize();
|
||||
|
||||
Loading…
Reference in New Issue
Block a user