From 703d812b85f6c4065e9b366ee8300e34c4697348 Mon Sep 17 00:00:00 2001 From: Simen Svale Skogsrud Date: Mon, 28 Jun 2010 23:29:58 +0200 Subject: [PATCH] rough accelleration stuff --- acceleration.c | 62 ++++++++++++++++++++++++++++++++++++++++++++++++ acceleration.h | 12 ++++++++++ config.c | 14 +++++++---- config.h | 1 + motion_control.c | 7 +++--- script/console | 4 ++-- script/stream.rb | 6 ++++- stepper.c | 46 ++++++++++++++++++++++++++--------- 8 files changed, 131 insertions(+), 21 deletions(-) create mode 100644 acceleration.c create mode 100644 acceleration.h diff --git a/acceleration.c b/acceleration.c new file mode 100644 index 0000000..f057623 --- /dev/null +++ b/acceleration.c @@ -0,0 +1,62 @@ +/* + acceleration.c - support methods for acceleration-related calcul + Part of Grbl + + Copyright (c) 2009 Simen Svale Skogsrud + + Grbl is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + Grbl is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with Grbl. If not, see . +*/ + + +// 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) { + 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) { + 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) { + 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 +// 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( + axis_feed_rate( + ((double)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( + axis_feed_rate( + ((double)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( + axis_feed_rate( + ((double)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])); + return max(x_swerve, max(y_swerve, z_swerve)); +} + diff --git a/acceleration.h b/acceleration.h new file mode 100644 index 0000000..988c4ee --- /dev/null +++ b/acceleration.h @@ -0,0 +1,12 @@ +#ifndef acceleration_h +#define acceleration_h + +// 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); + +// 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); + +#endif \ No newline at end of file diff --git a/config.c b/config.c index d36043c..e8af99d 100644 --- a/config.c +++ b/config.c @@ -33,6 +33,8 @@ void reset_settings() { settings.pulse_microseconds = STEP_PULSE_MICROSECONDS; settings.default_feed_rate = DEFAULT_FEEDRATE; settings.default_seek_rate = RAPID_FEEDRATE; + settings.dead_feed_rate = DEFAULT_FEEDRATE/5; + settings.acceleration = DEFAULT_FEEDRATE/100; settings.mm_per_arc_segment = MM_PER_ARC_SEGMENT; settings.invert_mask = STEPPING_INVERT_MASK; } @@ -44,8 +46,10 @@ void dump_settings() { 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$6 = ")); printFloat(settings.mm_per_arc_segment); - printPgmString(PSTR(" (mm/arc segment)\r\n$7 = ")); printInteger(settings.invert_mask); + 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(" (step port invert mask. binary = ")); printIntegerInBase(settings.invert_mask, 2); printPgmString(PSTR(")\r\n\r\n'$x=value' to set parameter or just '$' to dump current settings\r\n")); } @@ -74,8 +78,10 @@ void store_setting(int parameter, double value) { case 3: settings.pulse_microseconds = round(value); break; case 4: settings.default_feed_rate = value; break; case 5: settings.default_seek_rate = value; break; - case 6: settings.mm_per_arc_segment = value; break; - case 7: settings.invert_mask = trunc(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; default: printPgmString(PSTR("Unknown parameter\r\n")); return; diff --git a/config.h b/config.h index 0c5c64f..68ac922 100644 --- a/config.h +++ b/config.h @@ -69,6 +69,7 @@ struct Settings { double default_seek_rate; uint8_t invert_mask; double mm_per_arc_segment; + double accelleration; }; struct Settings settings; diff --git a/motion_control.c b/motion_control.c index 5b1dd34..6b2c391 100644 --- a/motion_control.c +++ b/motion_control.c @@ -68,7 +68,7 @@ void mc_line(double x, double y, double z, float feed_rate, int invert_feed_rate 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)); + lround((millimeters_of_travel/feed_rate)*1000000), millimeters_of_travel); } memcpy(position, target, sizeof(target)); // position[] = target[] } @@ -79,7 +79,7 @@ void mc_line(double x, double y, double z, float feed_rate, int invert_feed_rate // axis in axis_l which will be the axis for linear travel if you are tracing a helical motion. // The arc is approximated by generating a huge number of tiny, linear segments. The length of each -// segment is configured in config.h by setting MM_PER_ARC_SEGMENT. +// segment is configured in settings.mm_per_arc_segment. void mc_arc(double theta, double angular_travel, double radius, double linear_travel, int axis_1, int axis_2, int axis_linear, double feed_rate, int invert_feed_rate) { @@ -107,7 +107,8 @@ 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); + mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], feed_rate, invert_feed_rate, + settings.mm_per_arc_segment); } } diff --git a/script/console b/script/console index c68c823..28fea7e 100755 --- a/script/console +++ b/script/console @@ -1,3 +1,3 @@ -# 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.FireFly-A964-SPP-1,clocal=1,nonblock=1,cread=1,cs8,ixon=1,ixoff=1 + 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.FireFly-A964-SPP-1,clocal=1,nonblock=1,cread=1,cs8,ixon=1,ixoff=1 diff --git a/script/stream.rb b/script/stream.rb index 778ac03..2f56959 100644 --- a/script/stream.rb +++ b/script/stream.rb @@ -24,7 +24,9 @@ if ARGV.empty? exit end -SerialPort.open('/dev/tty.FireFly-A964-SPP-1', 115200) do |sp| +# SerialPort.open('/dev/tty.FireFly-A964-SPP-1', 115200) do |sp| +SerialPort.open('/dev/tty.usbserial-A9007QcR', 9600) do |sp| + sp.write("\r\n\r\n"); sleep 1 ARGV.each do |file| @@ -44,4 +46,6 @@ SerialPort.open('/dev/tty.FireFly-A964-SPP-1', 115200) do |sp| end end end + puts "Done." + sleep 500 end \ No newline at end of file diff --git a/stepper.c b/stepper.c index d939c73..e6a1f99 100644 --- a/stepper.c +++ b/stepper.c @@ -28,6 +28,7 @@ #include #include #include "nuts_bolts.h" +#include "acceleration.h" #include #include "wiring_serial.h" @@ -43,25 +44,42 @@ struct Line { uint32_t steps_x, steps_y, steps_z; int32_t maximum_steps; uint8_t direction_bits; - uint32_t rate; + 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 +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. +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 config_step_timer(uint32_t microseconds); +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() { + // Store the current + int initial_buffer_tail = line_buffer_tail; + +} // 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 microseconds) { +// steps. Microseconds specify how many microseconds the move should take to perform. To aid accelleration +// 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; // If the buffer is full: good! That means we are well ahead of the robot. @@ -75,12 +93,13 @@ void st_buffer_line(int32_t steps_x, int32_t steps_y, int32_t steps_z, uint32_t 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 = microseconds/line->maximum_steps; + line->rate = (TICKS_PER_MICROSECOND*microseconds)/line->maximum_steps; uint8_t direction_bits = 0; if (steps_x < 0) { direction_bits |= (1<direction_bits = direction_bits; + line->average_millimeters_per_step_event = millimeters/line->maximum_steps // Move buffer head line_buffer_head = next_buffer_head; // enable stepper interrupt @@ -126,8 +145,10 @@ SIGNAL(SIG_OUTPUT_COMPARE1A) counter_y = counter_x; counter_z = counter_x; iterations = current_line->maximum_steps; + moving = TRUE; } else { // disable this interrupt until there is something to handle + moving = FALSE; TIMSK1 &= ~(1<