diff --git a/Makefile b/Makefile index ab41f34..90742cc 100644 --- a/Makefile +++ b/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 diff --git a/accelleration.h b/accelleration.h index ab73d6d..6b4a89d 100644 --- a/accelleration.h +++ b/accelleration.h @@ -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 { diff --git a/config.c b/config.c index 13be1de..2512981 100644 --- a/config.c +++ b/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,22 +77,14 @@ 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; + printPgmString(PSTR("Unknown parameter\r\n")); + return; } write_settings(); printPgmString(PSTR("Stored new setting\r\n")); diff --git a/config.h b/config.h index 1e3d79b..f12c2a0 100644 --- a/config.h +++ b/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 diff --git a/gcode.c b/gcode.c index 1931a58..64ea2a4 100644 --- a/gcode.c +++ b/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; diff --git a/motion_control.c b/motion_control.c index 587c615..ce67a27 100644 --- a/motion_control.c +++ b/motion_control.c @@ -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); } } diff --git a/motion_control.h b/motion_control.h index dcee5eb..122b0ea 100644 --- a/motion_control.h +++ b/motion_control.h @@ -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 diff --git a/acceleration.c b/notes/acceleration.c similarity index 50% rename from acceleration.c rename to notes/acceleration.c index c334b5c..a8fb6ed 100644 --- a/acceleration.c +++ b/notes/acceleration.c @@ -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)); } diff --git a/acceleration.h b/notes/acceleration.h similarity index 100% rename from acceleration.h rename to notes/acceleration.h diff --git a/nuts_bolts.h b/nuts_bolts.h index cc6664d..515f988 100644 --- a/nuts_bolts.h +++ b/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 diff --git a/script/console b/script/console index 28fea7e..076d429 100755 --- a/script/console +++ b/script/console @@ -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 diff --git a/script/stream.rb b/script/stream.rb index 2f56959..cd2a4cc 100644 --- a/script/stream.rb +++ b/script/stream.rb @@ -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 diff --git a/stepper.c b/stepper.c index 774d134..a72b18c 100644 --- a/stepper.c +++ b/stepper.c @@ -28,7 +28,6 @@ #include #include #include "nuts_bolts.h" -#include "acceleration.h" #include #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< // // 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() { - // 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); + if (trapezoid_delta) { + trapezoid_rate += trapezoid_delta; + set_step_events_per_minute(trapezoid_rate); + } } else { - // Stage complete, move on + // Is there a block currently in execution? + if(!current_block) {return;} + // 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<