From 49a16cb777e8ca4e5c62665436ac29b7021c0bf9 Mon Sep 17 00:00:00 2001 From: Simen Svale Skogsrud Date: Fri, 14 Jan 2011 16:45:18 +0100 Subject: [PATCH] refactored block buffer into separate module motion_plan pending the addition of the actual look ahead planner --- Makefile | 4 +-- accelleration.h | 51 ----------------------------- config.c | 2 +- config.h | 6 ++-- gcode.c | 2 +- gcode.h | 2 +- main.c | 3 +- motion_control.c | 4 +-- motion_control.h | 4 +-- motion_plan.c | 69 +++++++++++++++++++++++++++++++++++++++ motion_plan.h | 57 ++++++++++++++++++++++++++++++++ notes/acceleration.c | 2 +- nuts_bolts.h | 2 +- script/console | 1 + serial_protocol.c | 2 +- serial_protocol.h | 2 +- spindle_control.c | 2 +- spindle_control.h | 2 +- stepper.c | 78 ++++---------------------------------------- stepper.h | 7 ++-- todo.txt | 7 ++-- 21 files changed, 163 insertions(+), 146 deletions(-) delete mode 100644 accelleration.h create mode 100644 motion_plan.c create mode 100644 motion_plan.h diff --git a/Makefile b/Makefile index 90742cc..4f824e0 100644 --- a/Makefile +++ b/Makefile @@ -1,6 +1,6 @@ # Part of Grbl # -# Copyright (c) 2009 Simen Svale Skogsrud +# Copyright (c) 2009-2011 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 @@ -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 + eeprom.o config.o motion_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/accelleration.h b/accelleration.h deleted file mode 100644 index 6b4a89d..0000000 --- a/accelleration.h +++ /dev/null @@ -1,51 +0,0 @@ -/* - accelleration.h - accelleration management support - 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 . -*/ - -#ifndef accelleration_h -#define accelleration_h - -// Unless someone else defined AC_TICKS_PER_SECOND, we define a sensible default -#ifndef AC_TICKS_PER_SECOND -#define AC_TICKS_PER_SECOND 10 -#endif - -struct AccellerationProfile { - double initial_scaler; - double final_scaler; - double accelleration_delta; - double decelleration_delta; - uint32_t accellerate_ticks; - uint32_t plateau_ticks; -}; - -struct AccellerationProfileSegment { - double v_entry[3]; - double v_ideal[3]; - double v_exit[3]; - double distance; - double f_entry, f_exit; -}; - -struct AccellerationProfileBuilder { - AccellerationProfileSegment segment[3]; - uint8_t current; -}; - -#endif \ No newline at end of file diff --git a/config.c b/config.c index 2512981..cffcc13 100644 --- a/config.c +++ b/config.c @@ -2,7 +2,7 @@ config.c - eeprom and compile time configuration handling Part of Grbl - Copyright (c) 2009 Simen Svale Skogsrud + Copyright (c) 2009-2011 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 diff --git a/config.h b/config.h index f12c2a0..9aa5f51 100644 --- a/config.h +++ b/config.h @@ -2,7 +2,7 @@ config.h - eeprom and compile time configuration handling Part of Grbl - Copyright (c) 2009 Simen Svale Skogsrud + Copyright (c) 2009-2011 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 @@ -27,8 +27,8 @@ // Settings that can only be set at compile-time: -// #define BAUD_RATE 9600 -#define BAUD_RATE 115200 +#define BAUD_RATE 9600 +//#define BAUD_RATE 115200 #define STEPPERS_ENABLE_DDR DDRD #define STEPPERS_ENABLE_PORT PORTD diff --git a/gcode.c b/gcode.c index 64ea2a4..436543e 100644 --- a/gcode.c +++ b/gcode.c @@ -2,7 +2,7 @@ gcode.c - rs274/ngc parser. Part of Grbl - Copyright (c) 2009 Simen Svale Skogsrud + Copyright (c) 2009-2011 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 diff --git a/gcode.h b/gcode.h index f3a1d32..d00fcbb 100644 --- a/gcode.h +++ b/gcode.h @@ -2,7 +2,7 @@ gcode.c - rs274/ngc parser. Part of Grbl - Copyright (c) 2009 Simen Svale Skogsrud + Copyright (c) 2009-2011 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 diff --git a/main.c b/main.c index 36fd6fe..39f6526 100644 --- a/main.c +++ b/main.c @@ -2,7 +2,7 @@ main.c - An embedded CNC Controller with rs274/ngc (g-code) support Part of Grbl - Copyright (c) 2009 Simen Svale Skogsrud + Copyright (c) 2009-2011 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 @@ -33,6 +33,7 @@ int main(void) { beginSerial(BAUD_RATE); + printString("A"); config_init(); st_init(); // initialize the stepper subsystem mc_init(); // initialize motion control subsystem diff --git a/motion_control.c b/motion_control.c index ce67a27..3529c3c 100644 --- a/motion_control.c +++ b/motion_control.c @@ -1,8 +1,8 @@ /* - motion_control.c - cartesian robot controller. + motion_control.c - high level interface for issuing motion commands Part of Grbl - Copyright (c) 2009 Simen Svale Skogsrud + Copyright (c) 2009-2011 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 diff --git a/motion_control.h b/motion_control.h index 122b0ea..b9104c5 100644 --- a/motion_control.h +++ b/motion_control.h @@ -1,8 +1,8 @@ /* - motion_control.h - cartesian robot controller. + motion_control.h - high level interface for issuing motion commands Part of Grbl - Copyright (c) 2009 Simen Svale Skogsrud + Copyright (c) 2009-2011 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 diff --git a/motion_plan.c b/motion_plan.c new file mode 100644 index 0000000..be51a66 --- /dev/null +++ b/motion_plan.c @@ -0,0 +1,69 @@ +/* + motion_plan.c - buffers movement commands and manages the acceleration profile plan + Part of Grbl + + Copyright (c) 2009-2011 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 . +*/ + +#include +#include + +#include "motion_plan.h" +#include "nuts_bolts.h" +#include "stepper.h" + +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_tail = 0; // Index of the block to process now + +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 = fabs(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; + } +} diff --git a/motion_plan.h b/motion_plan.h new file mode 100644 index 0000000..e28e4a7 --- /dev/null +++ b/motion_plan.h @@ -0,0 +1,57 @@ +/* + motion_plan.h - buffers movement commands and manages the acceleration profile plan + Part of Grbl + + Copyright (c) 2009-2011 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 . +*/ + +#ifndef motion_plan_h +#define motion_plan_h + +#include + +// Pick a suitable block-buffer size +#ifdef __AVR_ATmega328P__ +#define BLOCK_BUFFER_SIZE 20 // Atmega 328 has one full kilobyte of extra RAM! +#else +#define BLOCK_BUFFER_SIZE 5 +#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. +struct Block { + 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 + 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 +}; + +extern struct Block block_buffer[BLOCK_BUFFER_SIZE]; // A ring buffer for motion instructions +extern volatile int block_buffer_head; // Index of the next block to be pushed +extern volatile int block_buffer_tail; // Index of the block to process now + +// 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); + +#endif \ No newline at end of file diff --git a/notes/acceleration.c b/notes/acceleration.c index a8fb6ed..4694966 100644 --- a/notes/acceleration.c +++ b/notes/acceleration.c @@ -2,7 +2,7 @@ acceleration.c - support methods for acceleration-related calcul Part of Grbl - Copyright (c) 2009 Simen Svale Skogsrud + Copyright (c) 2009-2011 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 diff --git a/nuts_bolts.h b/nuts_bolts.h index 515f988..cca0009 100644 --- a/nuts_bolts.h +++ b/nuts_bolts.h @@ -2,7 +2,7 @@ motion_control.h - cartesian robot controller. Part of Grbl - Copyright (c) 2009 Simen Svale Skogsrud + Copyright (c) 2009-2011 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 diff --git a/script/console b/script/console index 076d429..d5c27ac 100755 --- a/script/console +++ b/script/console @@ -1,4 +1,5 @@ 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.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/serial_protocol.c b/serial_protocol.c index 3e246f1..2d58352 100644 --- a/serial_protocol.c +++ b/serial_protocol.c @@ -2,7 +2,7 @@ serial_protocol.c - the serial protocol master control unit Part of Grbl - Copyright (c) 2009 Simen Svale Skogsrud + Copyright (c) 2009-2011 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 diff --git a/serial_protocol.h b/serial_protocol.h index 16bb448..d03819f 100644 --- a/serial_protocol.h +++ b/serial_protocol.h @@ -2,7 +2,7 @@ serial_protocol.h - the serial protocol master control unit Part of Grbl - Copyright (c) 2009 Simen Svale Skogsrud + Copyright (c) 2009-2011 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 diff --git a/spindle_control.c b/spindle_control.c index 4dbf9fd..4d58b34 100644 --- a/spindle_control.c +++ b/spindle_control.c @@ -2,7 +2,7 @@ spindle_control.c - spindle control methods Part of Grbl - Copyright (c) 2009 Simen Svale Skogsrud + Copyright (c) 2009-2011 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 diff --git a/spindle_control.h b/spindle_control.h index 5a64418..49eda23 100644 --- a/spindle_control.h +++ b/spindle_control.h @@ -2,7 +2,7 @@ spindle_control.h - spindle control methods Part of Grbl - Copyright (c) 2009 Simen Svale Skogsrud + Copyright (c) 2009-2011 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 diff --git a/stepper.c b/stepper.c index a72b18c..f3445ca 100644 --- a/stepper.c +++ b/stepper.c @@ -1,8 +1,8 @@ /* - stepper.c - stepper motor interface + stepper.c - stepper motor driver: executes motion plans using stepper motors Part of Grbl - Copyright (c) 2009 Simen Svale Skogsrud + Copyright (c) 2009-2011 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 @@ -29,49 +29,20 @@ #include #include "nuts_bolts.h" #include - +#include "motion_plan.h" #include "wiring_serial.h" -// Pick a suitable block-buffer size -#ifdef __AVR_ATmega328P__ -#define BLOCK_BUFFER_SIZE 40 // Atmega 328 has one full kilobyte of extra RAM! -#else -#define BLOCK_BUFFER_SIZE 10 -#endif - - void set_step_events_per_minute(uint32_t steps_per_minute); #define ENABLE_STEPPER_DRIVER_INTERRUPT() TIMSK1 |= (1<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 +// Initializes the trapezoid generator from the current block. Called whenever a new +// block begins. inline void reset_trapezoid_generator() { trapezoid_stage = TRAPEZOID_STAGE_ACCELERATING; trapezoid_stage_ticks = current_block->accelerate_ticks; diff --git a/stepper.h b/stepper.h index b926809..e4201b8 100644 --- a/stepper.h +++ b/stepper.h @@ -1,8 +1,8 @@ /* - stepper.h - stepper motor interface + stepper.h - stepper motor driver: executes motion plans using stepper motors Part of Grbl - Copyright (c) 2009 Simen Svale Skogsrud + Copyright (c) 2009-2011 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 @@ -24,6 +24,9 @@ #include #include +#define ACCELERATION_TICKS_PER_SECOND 10 +#define MINIMAL_STEP_RATE (ACCELERATION_TICKS_PER_SECOND*5) + // Initialize and start the stepper motor subsystem void st_init(); diff --git a/todo.txt b/todo.txt index 029be3a..6d802a8 100644 --- a/todo.txt +++ b/todo.txt @@ -1,5 +1,8 @@ -* Complete support for using and setting separate seek-rate for G0-commnads +Todo +* Refactor stepper.c extracting motion blocks so that they can be processed by other code +* non blocking dwells +* Path Control Modes * Implement limit switch support in stepper.c (use port-triggered interrupts?) * Implement homing cycle in stepper.c -* Path Control Modes * Spindle speed support +