From 5880e55ce997227501f5bd90423974a7be6b6f03 Mon Sep 17 00:00:00 2001 From: Simen Svale Skogsrud Date: Sat, 15 Jan 2011 00:27:08 +0100 Subject: [PATCH] pcomplete, fully untested, support for accelleration management with look ahead optimization, finally --- Makefile | 2 +- config.c | 6 +- config.h | 4 +- main.c | 4 +- readme.txt | 1 + stepper.c | 4 +- motion_plan.c => stepper_plan.c | 112 +++++++++++++++++++++++++------- motion_plan.h => stepper_plan.h | 35 +++++----- 8 files changed, 120 insertions(+), 48 deletions(-) rename motion_plan.c => stepper_plan.c (69%) rename motion_plan.h => stepper_plan.h (73%) diff --git a/Makefile b/Makefile index abee769..5462f3b 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 motion_plan.o + eeprom.o config.o stepper_plan.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/config.c b/config.c index cffcc13..7f058ab 100644 --- a/config.c +++ b/config.c @@ -26,6 +26,8 @@ #include "wiring_serial.h" #include +struct Settings settings; + void reset_settings() { settings.steps_per_mm[0] = X_STEPS_PER_MM; settings.steps_per_mm[1] = Y_STEPS_PER_MM; @@ -36,6 +38,7 @@ void reset_settings() { settings.acceleration = DEFAULT_ACCELERATION; settings.mm_per_arc_segment = MM_PER_ARC_SEGMENT; settings.invert_mask = STEPPING_INVERT_MASK; + settings.max_jerk = DEFAULT_MAX_JERK; } void dump_settings() { @@ -50,7 +53,8 @@ void dump_settings() { 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(")\r\n$9 = ")); printFloat(settings.acceleration); - printPgmString(PSTR(" (acceleration in mm/min^2)")); + printPgmString(PSTR(" (acceleration in mm/min^2)\r\n$10 = ")); printFloat(settings.max_jerk); + printPgmString(PSTR(" (max instant cornering speed change in delta mm/min)")); printPgmString(PSTR("\r\n'$x=value' to set parameter or just '$' to dump current settings\r\n")); } diff --git a/config.h b/config.h index 86cf149..20f2e57 100644 --- a/config.h +++ b/config.h @@ -72,8 +72,9 @@ struct Settings { uint8_t invert_mask; double mm_per_arc_segment; double acceleration; + double max_jerk; }; -struct Settings settings; +extern struct Settings settings; // Initialize the configuration subsystem (load settings from EEPROM) void config_init(); @@ -97,6 +98,7 @@ void store_setting(int parameter, double value); #define RAPID_FEEDRATE 480.0 // in millimeters per minute #define DEFAULT_FEEDRATE 480.0 #define DEFAULT_ACCELERATION (DEFAULT_FEEDRATE/100.0) +#define DEFAULT_MAX_JERK 50.0 // Use this line for default operation (step-pulses high) #define STEPPING_INVERT_MASK 0 diff --git a/main.c b/main.c index 26c7ec5..31aaddc 100644 --- a/main.c +++ b/main.c @@ -21,7 +21,7 @@ #include #include #include -#include "motion_plan.h" +#include "stepper_plan.h" #include "stepper.h" #include "spindle_control.h" #include "motion_control.h" @@ -36,7 +36,7 @@ int main(void) beginSerial(BAUD_RATE); printString("A"); config_init(); - mp_init(); // initialize the motion plan subsystem + plan_init(); // initialize the stepper plan subsystem st_init(); // initialize the stepper subsystem mc_init(); // initialize motion control subsystem spindle_init(); // initialize spindle controller diff --git a/readme.txt b/readme.txt index f04bebf..f6a4613 100644 --- a/readme.txt +++ b/readme.txt @@ -20,6 +20,7 @@ Prioritized to-do: * Documentation and web-site * Support for a alphanumeric LCD readout, a joystick and a few buttons for program control * Support "headless" fabrication by buffering all code to SD-card or similar +* Backlash compensation Limitations by design: * Limited GCode-support. Focus on the kind of GCode produced by CAM tools. Leave human GCoders frustrated. diff --git a/stepper.c b/stepper.c index 4a38c03..f121a77 100644 --- a/stepper.c +++ b/stepper.c @@ -29,7 +29,7 @@ #include #include "nuts_bolts.h" #include -#include "motion_plan.h" +#include "stepper_plan.h" #include "wiring_serial.h" void set_step_events_per_minute(uint32_t steps_per_minute); @@ -120,7 +120,7 @@ inline void trapezoid_generator_tick() { // 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) { - mp_buffer_line(steps_x, steps_y, steps_z, microseconds, millimeters); + plan_buffer_line(steps_x, steps_y, steps_z, microseconds, millimeters); // Ensure that block processing is running by enabling The Stepper Driver Interrupt ENABLE_STEPPER_DRIVER_INTERRUPT(); } diff --git a/motion_plan.c b/stepper_plan.c similarity index 69% rename from motion_plan.c rename to stepper_plan.c index be9dcde..d96aed6 100644 --- a/motion_plan.c +++ b/stepper_plan.c @@ -1,5 +1,5 @@ /* - motion_plan.c - buffers movement commands and manages the acceleration profile plan + stepper_plan.c - buffers movement commands and manages the acceleration profile plan Part of Grbl Copyright (c) 2009-2011 Simen Svale Skogsrud @@ -22,7 +22,7 @@ #include #include -#include "motion_plan.h" +#include "stepper_plan.h" #include "nuts_bolts.h" #include "stepper.h" #include "config.h" @@ -43,27 +43,6 @@ inline uint32_t estimate_acceleration_ticks(int32_t start_rate, int32_t accelera acceleration_per_tick)); } -void mp_enable_acceleration_management() { - if (!acceleration_management) { - st_synchronize(); - acceleration_management = TRUE; - } -} - -void mp_disable_acceleration_management() { - if(acceleration_management) { - st_synchronize(); - acceleration_management = FALSE; - } -} - -void mp_init() { - block_buffer_head = 0; - block_buffer_tail = 0; - mp_enable_acceleration_management(); -} - - // 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) { @@ -92,10 +71,92 @@ void calculate_trapezoid_for_block(struct Block *block, double entry_factor, dou } } +inline double estimate_max_speed(double max_acceleration, double target_velocity, double distance) { + return(sqrt(-2*max_acceleration*distance+target_velocity*target_velocity)); +} + +inline double estimate_jerk(struct Block *before, struct Block *after) { + return(max(fabs(before->speed_x-after->speed_x), + max(fabs(before->speed_y-after->speed_y), + fabs(before->speed_z-after->speed_z)))); +} + +// Builds plan for a single block provided. Returns TRUE if changes were made to this block +// that requires any earlier blocks to be recalculated too. +int8_t build_plan_for_single_block(struct Block *previous, struct Block *current, struct Block *next) { + if(!current){return(TRUE);} + double exit_factor; + double entry_factor = 1.0; + if (next) { + exit_factor = next->entry_factor; + } else { + exit_factor = 0.0; + } + + if (previous) { + double jerk = estimate_jerk(previous, current); + if (jerk > settings.max_jerk) { + entry_factor = (settings.max_jerk/jerk); + } + if (exit_factornominal_speed*exit_factor, + current->millimeters); + double max_entry_factor = max_entry_speed/current->nominal_speed; + if (max_entry_factor < entry_factor) { + entry_factor = max_entry_factor; + } + } + } else { + entry_factor = 0.0; + } + // Check if we made a difference for this block. If we didn't, the planner can call it quits + // here. No need to process any earlier blocks. + int8_t keep_going = (entry_factor > current->entry_factor ? TRUE : FALSE); + // Store result and recalculate trapezoid parameters + current->entry_factor = entry_factor; + calculate_trapezoid_for_block(current, entry_factor, exit_factor); + return(keep_going); +} + +void recalculate_plan() { + int8_t block_index = block_buffer_head; + struct Block *block[3] = {NULL, NULL, NULL}; + while(block_index != block_buffer_tail) { + block[2]= block[1]; + block[1]= block[0]; + block[0] = &block_buffer[block_index]; + if (!build_plan_for_single_block(block[0], block[1], block[2])) {return;} + block_index = (block_index-1) % BLOCK_BUFFER_SIZE; + } + if (block[1]) { + calculate_trapezoid_for_block(block[0], block[0]->entry_factor, block[1]->entry_factor); + } +} + +void plan_enable_acceleration_management() { + if (!acceleration_management) { + st_synchronize(); + acceleration_management = TRUE; + } +} + +void plan_disable_acceleration_management() { + if(acceleration_management) { + st_synchronize(); + acceleration_management = FALSE; + } +} + +void plan_init() { + block_buffer_head = 0; + block_buffer_tail = 0; + plan_enable_acceleration_management(); +} + // 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. -void mp_buffer_line(int32_t steps_x, int32_t steps_y, int32_t steps_z, uint32_t microseconds, double millimeters) { +void plan_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 = (block_buffer_head + 1) % BLOCK_BUFFER_SIZE; // If the buffer is full: good! That means we are well ahead of the robot. @@ -115,7 +176,8 @@ void mp_buffer_line(int32_t steps_x, int32_t steps_y, int32_t steps_z, uint32_t 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_speed = millimeters*multiplier; + block->nominal_rate = round(block->step_event_count*multiplier); // 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 diff --git a/motion_plan.h b/stepper_plan.h similarity index 73% rename from motion_plan.h rename to stepper_plan.h index c9d3912..f9cb78c 100644 --- a/motion_plan.h +++ b/stepper_plan.h @@ -1,5 +1,5 @@ /* - motion_plan.h - buffers movement commands and manages the acceleration profile plan + stepper_plan.h - buffers movement commands and manages the acceleration profile plan Part of Grbl Copyright (c) 2009-2011 Simen Svale Skogsrud @@ -18,8 +18,8 @@ along with Grbl. If not, see . */ -#ifndef motion_plan_h -#define motion_plan_h +#ifndef stepper_plan_h +#define stepper_plan_h #include @@ -27,19 +27,23 @@ #ifdef __AVR_ATmega328P__ #define BLOCK_BUFFER_SIZE 20 // Atmega 328 has one full kilobyte of extra RAM! #else -#define BLOCK_BUFFER_SIZE 5 +#define BLOCK_BUFFER_SIZE 10 #endif -// 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. +// 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 { + // Fields used by the bresenham algorithm for tracing the line uint32_t steps_x, steps_y, steps_z; // Step count along each axis uint8_t direction_bits; // The direction bit set for this block (refers to *_DIRECTION_BIT in config.h) 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 - double speed_x, speed_y, speed_z; // Nominal mm/minute for each axis + + // Fields used by the motion planner to manage acceleration + double speed_x, speed_y, speed_z; // Nominal mm/minute for each axis + double nominal_speed; // The nominal speed for this block in mm/min + double millimeters; + double entry_factor; // The factors representing the change in speed at the start of the trapezoid 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 @@ -51,19 +55,18 @@ extern volatile int block_buffer_head; // Index of the next block to b extern volatile int block_buffer_tail; // Index of the block to process now // Initialize the motion plan subsystem -void mp_init(); +void plan_init(); +// Do not call directly unless you are writing a motor driver. In current iteration this is called by +// st_buffer_line which also wakes up the stepper subsystem. // 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. -// Do not call directly unless you are writing a motor driver. In current iteration this is called by -// st_buffer_line which also wakes up the stepper subsystem. -void mp_buffer_line(int32_t steps_x, int32_t steps_y, int32_t steps_z, uint32_t microseconds, double millimeters); +void plan_buffer_line(int32_t steps_x, int32_t steps_y, int32_t steps_z, uint32_t microseconds, double millimeters); // Enables acceleration-management for upcoming blocks -void mp_enable_acceleration_management(); +void plan_enable_acceleration_management(); // Disables acceleration-management for upcoming blocks -void mp_disable_acceleration_management(); - +void plan_disable_acceleration_management(); #endif \ No newline at end of file