ManuelW/grbl-LPC-CoreXY
1
0
Fork 0
Code Issues Pull Requests Releases 1 Wiki Activity

Protected buffer works! Vast improvements to planner efficiency. Many things still broken with overhaul.

Browse Source 
Development push. Lots still broken.

- Protected planner concept works! This is a critical precursor to
enabling feedrate overrides in allowing the planner buffer and the
stepper execution operate atomically. This is done through a
intermediary segment buffer.

- Still lots of work to be done, as this was a complete overhaul of the
planner and stepper subsystems. The code can be cleaned up quite a bit,
re-enabling some of the broken features like feed holds, and finishing
up some of the concepts

- Pushed some of the fixes from the master and edge branch to here, as
this will likely replace the edge branch when done.
This commit is contained in:
Sonny Jeon 2013-10-09 09:33:22 -06:00
parent 7a175bd2db
commit 805f0f219c
10 changed files with 793 additions and 301 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Makefile
View File

@ -84,7 +84,7 @@ main.elf: $(OBJECTS)
grbl.hex: main.elf
rm -f grbl.hex
avr-objcopy -j .text -j .data -O ihex main.elf grbl.hex
avr-size -C --mcu=$(DEVICE) main.elf
avr-size --format=berkeley main.elf
# If you have an EEPROM section, you must also create a hex file for the
# EEPROM and add it to the "flash" target.

93
config.h
View File

@ -2,8 +2,8 @@
config.h - compile time configuration
Part of Grbl
Copyright (c) 2009-2011 Simen Svale Skogsrud
Copyright (c) 2011-2013 Sungeun K. Jeon
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
@ -19,80 +19,24 @@
along with Grbl. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef config_h
#define config_h
// This file contains compile-time configurations for Grbl's internal system. For the most part,
// users will not need to directly modify these, but they are here for specific needs, i.e.
// performance tuning or adjusting to non-typical machines.
// IMPORTANT: Any changes here requires a full re-compiling of the source code to propagate them.
#ifndef config_h
#define config_h
// Default settings. Used when resetting EEPROM. Change to desired name in defaults.h
#define DEFAULTS_ZEN_TOOLWORKS_7x7
// Serial baud rate
#define BAUD_RATE 9600
#define BAUD_RATE 115200
// Define pin-assignments
// NOTE: All step bit and direction pins must be on the same port.
#define STEPPING_DDR DDRD
#define STEPPING_PORT PORTD
#define X_STEP_BIT 2 // Uno Digital Pin 2
#define Y_STEP_BIT 3 // Uno Digital Pin 3
#define Z_STEP_BIT 4 // Uno Digital Pin 4
#define X_DIRECTION_BIT 5 // Uno Digital Pin 5
#define Y_DIRECTION_BIT 6 // Uno Digital Pin 6
#define Z_DIRECTION_BIT 7 // Uno Digital Pin 7
#define STEP_MASK ((1<<X_STEP_BIT)|(1<<Y_STEP_BIT)|(1<<Z_STEP_BIT)) // All step bits
#define DIRECTION_MASK ((1<<X_DIRECTION_BIT)|(1<<Y_DIRECTION_BIT)|(1<<Z_DIRECTION_BIT)) // All direction bits
#define STEPPING_MASK (STEP_MASK | DIRECTION_MASK) // All stepping-related bits (step/direction)
#define STEPPERS_DISABLE_DDR DDRB
#define STEPPERS_DISABLE_PORT PORTB
#define STEPPERS_DISABLE_BIT 0 // Uno Digital Pin 8
#define STEPPERS_DISABLE_MASK (1<<STEPPERS_DISABLE_BIT)
// NOTE: All limit bit pins must be on the same port
#define LIMIT_DDR DDRB
#define LIMIT_PIN PINB
#define LIMIT_PORT PORTB
#define X_LIMIT_BIT 1 // Uno Digital Pin 9
#define Y_LIMIT_BIT 2 // Uno Digital Pin 10
#define Z_LIMIT_BIT 3 // Uno Digital Pin 11
#define LIMIT_INT PCIE0 // Pin change interrupt enable pin
#define LIMIT_INT_vect PCINT0_vect
#define LIMIT_PCMSK PCMSK0 // Pin change interrupt register
#define LIMIT_MASK ((1<<X_LIMIT_BIT)|(1<<Y_LIMIT_BIT)|(1<<Z_LIMIT_BIT)) // All limit bits
#define SPINDLE_ENABLE_DDR DDRB
#define SPINDLE_ENABLE_PORT PORTB
#define SPINDLE_ENABLE_BIT 4 // Uno Digital Pin 12
#define SPINDLE_DIRECTION_DDR DDRB
#define SPINDLE_DIRECTION_PORT PORTB
#define SPINDLE_DIRECTION_BIT 5 // Uno Digital Pin 13 (NOTE: D13 can't be pulled-high input due to LED.)
#define COOLANT_FLOOD_DDR DDRC
#define COOLANT_FLOOD_PORT PORTC
#define COOLANT_FLOOD_BIT 3 // Uno Analog Pin 3
// NOTE: Uno analog pins 4 and 5 are reserved for an i2c interface, and may be installed at
// a later date if flash and memory space allows.
// #define ENABLE_M7 // Mist coolant disabled by default. Uncomment to enable.
#ifdef ENABLE_M7
#define COOLANT_MIST_DDR DDRC
#define COOLANT_MIST_PORT PORTC
#define COOLANT_MIST_BIT 4 // Uno Analog Pin 4
#endif
// NOTE: All pinouts pins must be on the same port
#define PINOUT_DDR DDRC
#define PINOUT_PIN PINC
#define PINOUT_PORT PORTC
#define PIN_RESET 0 // Uno Analog Pin 0
#define PIN_FEED_HOLD 1 // Uno Analog Pin 1
#define PIN_CYCLE_START 2 // Uno Analog Pin 2
#define PINOUT_INT PCIE1 // Pin change interrupt enable pin
#define PINOUT_INT_vect PCINT1_vect
#define PINOUT_PCMSK PCMSK1 // Pin change interrupt register
#define PINOUT_MASK ((1<<PIN_RESET)|(1<<PIN_FEED_HOLD)|(1<<PIN_CYCLE_START))
// Default pin mappings. Grbl officially supports the Arduino Uno only. Other processor types
// may exist from user-supplied templates or directly user-defined in pin_map.h
#define PIN_MAP_ARDUINO_UNO
// Define runtime command special characters. These characters are 'picked-off' directly from the
// serial read data stream and are not passed to the grbl line execution parser. Select characters
@ -137,11 +81,6 @@
// zero and report it to the Grbl administrators.
#define INV_TIME_MULTIPLIER 10000000.0
// Minimum planner junction speed. Sets the default minimum speed the planner plans for at the end
// of the buffer and all stops. This should not be much greater than zero and should only be changed
// if unwanted behavior is observed on a user's machine when running at very slow speeds.
#define MINIMUM_PLANNER_SPEED 0.0 // (mm/min)
// Minimum stepper rate for the "Stepper Driver Interrupt". Sets the absolute minimum stepper rate
// in the stepper program and never runs slower than this value. If the RANADE_MULTIPLIER value
// changes, it will affect how this value works. So, if a zero is add/subtracted from the
@ -152,6 +91,14 @@
// Minimum stepper rate. Only used by homing at this point. May be removed in later releases.
#define MINIMUM_STEPS_PER_MINUTE 800 // (steps/min) - Integer value only
// Minimum planner junction speed. Sets the default minimum junction speed the planner plans to at
// every buffer block junction, except for starting from rest and end of the buffer, which are always
// zero. This value controls how fast the machine moves through junctions with no regard for acceleration
// limits or angle between neighboring block line move directions. This is useful for machines that can't
// tolerate the tool dwelling for a split second, i.e. 3d printers or laser cutters. If used, this value
// should not be much greater than zero or to the minimum value necessary for the machine to work.
#define MINIMUM_JUNCTION_SPEED 0.0 // (mm/min)
// Time delay increments performed during a dwell. The default value is set at 50ms, which provides
// a maximum time delay of roughly 55 minutes, more than enough for most any application. Increasing
// this delay will increase the maximum dwell time linearly, but also reduces the responsiveness of
@ -217,7 +164,7 @@
// each of the startup blocks, as they are each stored as a string of this size. Make sure
// to account for the available EEPROM at the defined memory address in settings.h and for
// the number of desired startup blocks.
// NOTE: 50 characters is not a problem except for extreme cases, but the line buffer size
// NOTE: 70 characters is not a problem except for extreme cases, but the line buffer size
// can be too small and g-code blocks can get truncated. Officially, the g-code standards
// support up to 256 characters. In future versions, this default will be increased, when
// we know how much extra memory space we can re-invest into this.

97
doc/pinmapping.txt Normal file
View File

@ -0,0 +1,97 @@
Mega328P Arduino Pin Mapping
============================
Digital 0 PD0 (RX)
Digital 1 PD1 (TX)
Digital 2 PD2
Digital 3 PD3
Digital 4 PD4
Digital 5 PD5
Digital 6 PD6
Digital 7 PD7
Digital 8 PB0
Digital 9 PB1
Digital 10 PB2
Digital 11 PB3 (MOSI)
Digital 12 PB4 (MISO)
Digital 13 PB5 (SCK)
Analog 0 PC0
Analog 1 PC1
Analog 2 PC2
Analog 3 PC3
Analog 4 PC4
Mega2560 Arduino Pin Mapping
============================
Digital pin 22 PA0 ( AD0 )
Digital pin 23 PA1 ( AD1 )
Digital pin 24 PA2 ( AD2 )
Digital pin 25 PA3 ( AD3 )
Digital pin 26 PA4 ( AD4 )
Digital pin 27 PA5 ( AD5 )
Digital pin 28 PA6 ( AD6 )
Digital pin 29 PA7 ( AD7 )
Digital pin 53 (PWM)(RX1) PB0 ( SS/PCINT0 )
Digital pin 52 (PWM)(SDA) PB1 ( SCK/PCINT1 )
Digital pin 51 (PWM)(SCL) PB2 ( MOSI/PCINT2 )
Digital pin 50 PB3 ( MISO/PCINT3 )
Digital pin 10 (PWM) PB4 ( OC2A/PCINT4 )
Digital pin 11 (PWM) PB5 ( OC1A/PCINT5 )
Digital pin 12 (PWM) PB6 ( OC1B/PCINT6 )
Digital pin 13 (PWM) PB7 ( OC0A/OC1C/PCINT7 )
Digital pin 37 PC0 ( A8 )
Digital pin 36 PC1 ( A9 )
Digital pin 35 PC2 ( A10 )
Digital pin 34 PC3 ( A11 )
Digital pin 33 PC4 ( A12 )
Digital pin 32 PC5 ( A13 )
Digital pin 31 PC6 ( A14 )
Digital pin 30 PC7 ( A15 )
Digital pin 21 (SCL) PD0 ( SCL/INT0 )
Digital pin 20 (SDA) PD1 ( SDA/INT1 )
Digital pin 19 PD2 ( RXDI/INT2 )
Digital pin 18 PD3 ( TXD1/INT3 )
Digital pin 38 PD7 ( T0 )
Digital pin 0 (PWM) (RX0) PE0 ( RXD0/PCINT8 )
Digital pin 1 (PWM) (TX0) PE1 ( TXD0 )
Digital pin 5 (PWM) PE3 ( OC3A/AIN1 )
Digital pin 2 (PWM) PE4 ( OC3B/INT4 )
Digital pin 3 (PWM) PE5 ( OC3C/INT5 )
Analog pin 0 PF0 ( ADC0 )
Analog pin 1 PF1 ( ADC1 )
Analog pin 2 PF2 ( ADC2 )
Analog pin 3 PF3 ( ADC3 )
Analog pin 4 PF4 ( ADC4/TMK )
Analog pin 5 PF5 ( ADC5/TMS )
Analog pin 6 PF6 ( ADC6/PCINT14 )
Analog pin 7 PF7 ( ADC7/PCINT15 )
Digital pin 41 PG0 ( WR )
Digital pin 40 PG1 ( RD )
Digital pin 39 PG2 ( ALE )
Digital pin 4 (PWM) PG5 ( OC0B )
Digital pin 17 (PWM) PH0 ( RXD2 )
Digital pin 16 (PWM) PH1 ( TXD2 )
Digital pin 6 (PWM)(RX3 ) PH3 ( OC4A )
Digital pin 7 (PWM)(TX2) PH4 ( OC4B )
Digital pin 8 (PWM)(RX2 ) PH5 ( OC4C )
Digital pin 9 (PWM)(TX1) PH6 ( OC2B )
Digital pin 15 PJ0 ( RXD3/PCINT9 )
Digital pin 14 PJ1 ( TXD3/PCINT10 )
Analog pin 8 PK0 ( ADC8/PCINT16 )
Analog pin 9 PK1 ( ADC9/PCINT17 )
Analog pin 10 PK2 ( ADC10/PCINT18 )
Analog pin 11 PK3 ( ADC11/PCINT19 )
Analog pin 12 PK4 ( ADC12/PCINT20 )
Analog pin 13 PK5 ( ADC13/PCINT21 )
Analog pin 14 PK6 ( ADC14/PCINT22 )
Analog pin 15 PK7 ( ADC15/PCINT23 )
Digital pin 49 PL0 ( ICP4 )
Digital pin 48 PL1 ( ICP5 )
Digital pin 47 PL2 ( T5 )
Digital pin 46 (PWM) PL3 ( OC5A )
Digital pin 45 (PWM) PL4 ( OC5B )
Digital pin 44 (PWM) PL5 ( OC5C )
Digital pin 43 PL6
Digital pin 42 PL7

1
nuts_bolts.h
View File

@ -27,6 +27,7 @@
#include <stdbool.h>
#include "config.h"
#include "defaults.h"
#include "pin_map.h"
#define false 0
#define true 1

181
pin_map.h Normal file
View File

@ -0,0 +1,181 @@
/*
pin_map.h - Pin mapping configuration file
Part of Grbl
Copyright (c) 2013 Sungeun K. Jeon
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 <http://www.gnu.org/licenses/>.
*/
/* The pin_map.h file serves as a central pin mapping settings file for different processor
types, i.e. AVR 328p or AVR Mega 2560. Grbl officially supports the Arduino Uno, but the
other supplied pin mappings are supplied by users, so your results may vary. */
#ifndef pin_map_h
#define pin_map_h
#ifdef PIN_MAP_ARDUINO_UNO // AVR 328p, Officially supported by Grbl.
// Serial port pins
#define SERIAL_RX USART_RX_vect
#define SERIAL_UDRE USART_UDRE_vect
// NOTE: All step bit and direction pins must be on the same port.
#define STEPPING_DDR DDRD
#define STEPPING_PORT PORTD
#define X_STEP_BIT 2 // Uno Digital Pin 2
#define Y_STEP_BIT 3 // Uno Digital Pin 3
#define Z_STEP_BIT 4 // Uno Digital Pin 4
#define X_DIRECTION_BIT 5 // Uno Digital Pin 5
#define Y_DIRECTION_BIT 6 // Uno Digital Pin 6
#define Z_DIRECTION_BIT 7 // Uno Digital Pin 7
#define STEP_MASK ((1<<X_STEP_BIT)|(1<<Y_STEP_BIT)|(1<<Z_STEP_BIT)) // All step bits
#define DIRECTION_MASK ((1<<X_DIRECTION_BIT)|(1<<Y_DIRECTION_BIT)|(1<<Z_DIRECTION_BIT)) // All direction bits
#define STEPPING_MASK (STEP_MASK | DIRECTION_MASK) // All stepping-related bits (step/direction)
#define STEPPERS_DISABLE_DDR DDRB
#define STEPPERS_DISABLE_PORT PORTB
#define STEPPERS_DISABLE_BIT 0 // Uno Digital Pin 8
#define STEPPERS_DISABLE_MASK (1<<STEPPERS_DISABLE_BIT)
// NOTE: All limit bit pins must be on the same port
#define LIMIT_DDR DDRB
#define LIMIT_PIN PINB
#define LIMIT_PORT PORTB
#define X_LIMIT_BIT 1 // Uno Digital Pin 9
#define Y_LIMIT_BIT 2 // Uno Digital Pin 10
#define Z_LIMIT_BIT 3 // Uno Digital Pin 11
#define LIMIT_INT PCIE0 // Pin change interrupt enable pin
#define LIMIT_INT_vect PCINT0_vect
#define LIMIT_PCMSK PCMSK0 // Pin change interrupt register
#define LIMIT_MASK ((1<<X_LIMIT_BIT)|(1<<Y_LIMIT_BIT)|(1<<Z_LIMIT_BIT)) // All limit bits
#define SPINDLE_ENABLE_DDR DDRB
#define SPINDLE_ENABLE_PORT PORTB
#define SPINDLE_ENABLE_BIT 4 // Uno Digital Pin 12
#define SPINDLE_DIRECTION_DDR DDRB
#define SPINDLE_DIRECTION_PORT PORTB
#define SPINDLE_DIRECTION_BIT 5 // Uno Digital Pin 13 (NOTE: D13 can't be pulled-high input due to LED.)
#define COOLANT_FLOOD_DDR DDRC
#define COOLANT_FLOOD_PORT PORTC
#define COOLANT_FLOOD_BIT 3 // Uno Analog Pin 3
// NOTE: Uno analog pins 4 and 5 are reserved for an i2c interface, and may be installed at
// a later date if flash and memory space allows.
// #define ENABLE_M7 // Mist coolant disabled by default. Uncomment to enable.
#ifdef ENABLE_M7
#define COOLANT_MIST_DDR DDRC
#define COOLANT_MIST_PORT PORTC
#define COOLANT_MIST_BIT 4 // Uno Analog Pin 4
#endif
// NOTE: All pinouts pins must be on the same port
#define PINOUT_DDR DDRC
#define PINOUT_PIN PINC
#define PINOUT_PORT PORTC
#define PIN_RESET 0 // Uno Analog Pin 0
#define PIN_FEED_HOLD 1 // Uno Analog Pin 1
#define PIN_CYCLE_START 2 // Uno Analog Pin 2
#define PINOUT_INT PCIE1 // Pin change interrupt enable pin
#define PINOUT_INT_vect PCINT1_vect
#define PINOUT_PCMSK PCMSK1 // Pin change interrupt register
#define PINOUT_MASK ((1<<PIN_RESET)|(1<<PIN_FEED_HOLD)|(1<<PIN_CYCLE_START))
#endif
#ifdef PIN_MAP_ARDUINO_MEGA_2560 // Unsupported. Doesn't work. Supplied by @elmom.
// Serial port pins
#define SERIAL_RX USART0_RX_vect
#define SERIAL_UDRE USART0_UDRE_vect
// NOTE: All step bit and direction pins must be on the same port.
#define STEPPING_DDR DDRA
#define STEPPING_PORT PORTA
#define STEPPING_PIN PINA
#define X_STEP_BIT 0 // MEGA2560 Digital Pin 22
#define Y_STEP_BIT 1 // MEGA2560 Digital Pin 23
#define Z_STEP_BIT 2 // MEGA2560 Digital Pin 24
// #define C_STEP_BIT 3 // MEGA2560 Digital Pin 25
#define X_DIRECTION_BIT 4 // MEGA2560 Digital Pin 26
#define Y_DIRECTION_BIT 5 // MEGA2560 Digital Pin 27
#define Z_DIRECTION_BIT 6 // MEGA2560 Digital Pin 28
// #define C_DIRECTION_BIT 7 // MEGA2560 Digital Pin 29
#define STEP_MASK ((1<<X_STEP_BIT)|(1<<Y_STEP_BIT)|(1<<Z_STEP_BIT)) // All step bits
#define DIRECTION_MASK ((1<<X_DIRECTION_BIT)|(1<<Y_DIRECTION_BIT)|(1<<Z_DIRECTION_BIT)) // All direction bits
#define STEPPING_MASK (STEP_MASK | DIRECTION_MASK) // All stepping-related bits (step/direction)
#define STEPPERS_DISABLE_DDR DDRC
#define STEPPERS_DISABLE_PORT PORTC
#define STEPPERS_DISABLE_BIT 7 // MEGA2560 Digital Pin 30
#define STEPPERS_DISABLE_MASK (1<<STEPPERS_DISABLE_BIT)
// NOTE: All limit bit pins must be on the same port
#define LIMIT_DDR DDRC
#define LIMIT_PORT PORTC
#define LIMIT_PIN PINC
#define X_LIMIT_BIT 6 // MEGA2560 Digital Pin 31
#define Y_LIMIT_BIT 5 // MEGA2560 Digital Pin 32
#define Z_LIMIT_BIT 4 // MEGA2560 Digital Pin 33
// #define C_LIMIT_BIT 3 // MEGA2560 Digital Pin 34
#define LIMIT_INT PCIE0 // Pin change interrupt enable pin
#define LIMIT_INT_vect PCINT0_vect
#define LIMIT_PCMSK PCMSK0 // Pin change interrupt register
#define LIMIT_MASK ((1<<X_LIMIT_BIT)|(1<<Y_LIMIT_BIT)|(1<<Z_LIMIT_BIT)) // All limit bits
#define SPINDLE_ENABLE_DDR DDRC
#define SPINDLE_ENABLE_PORT PORTC
#define SPINDLE_ENABLE_BIT 2 // MEGA2560 Digital Pin 35
#define SPINDLE_DIRECTION_DDR DDRC
#define SPINDLE_DIRECTION_PORT PORTC
#define SPINDLE_DIRECTION_BIT 1 // MEGA2560 Digital Pin 36
#define COOLANT_FLOOD_DDR DDRC
#define COOLANT_FLOOD_PORT PORTC
#define COOLANT_FLOOD_BIT 0 // MEGA2560 Digital Pin 37
// #define ENABLE_M7 // Mist coolant disabled by default. Uncomment to enable.
#ifdef ENABLE_M7
#define COOLANT_MIST_DDR DDRC
#define COOLANT_MIST_PORT PORTC
#define COOLANT_MIST_BIT 0 // MEGA2560 Digital Pin 37
#endif
// NOTE: All pinouts pins must be on the same port
#define PINOUT_DDR DDRC
#define PINOUT_PIN PINC
#define PINOUT_PORT PORTC
#define PIN_RESET 0 // Uno Analog Pin 0
#define PIN_FEED_HOLD 1 // Uno Analog Pin 1
#define PIN_CYCLE_START 2 // Uno Analog Pin 2
#define PINOUT_INT PCIE1 // Pin change interrupt enable pin
#define PINOUT_INT_vect PCINT1_vect
#define PINOUT_PCMSK PCMSK1 // Pin change interrupt register
#define PINOUT_MASK ((1<<PIN_RESET)|(1<<PIN_FEED_HOLD)|(1<<PIN_CYCLE_START))
#endif
/*
#ifdef PIN_MAP_CUSTOM_PROC
// For a custom pin map or different processor, copy and paste one of the default pin map
// settings above and modify it to your needs. Then, make sure the defined name is also
// changed in the config.h file.
#endif
*/
#endif

355
planner.c
View File

@ -70,11 +70,52 @@ static uint8_t prev_block_index(uint8_t block_index)
}
// Update the entry speed and millimeters remaining to execute for a partially completed block. Called only
// when the planner knows it will be changing the conditions of this block.
// TODO: Set up to be called from planner calculations. Need supporting code framework still, i.e. checking
// and executing this only when necessary, combine with the block_buffer_safe pointer.
// TODO: This is very similar to the planner reinitialize after a feed hold. Could make this do double duty.
void plan_update_partial_block(uint8_t block_index, float exit_speed_sqr)
{
// TODO: Need to make a condition to check if we need make these calculations. We don't if nothing has
// been executed or placed into segment buffer. This happens with the first block upon startup or if
// the segment buffer is exactly in between two blocks. Just check if the step_events_remaining is equal
// the total step_event_count in the block. If so, we don't have to do anything.
// !!! block index is the same as block_buffer_safe.
// See if we can reduce this down to just requesting the millimeters remaining..
uint8_t is_decelerating;
float millimeters_remaining = 0.0;
st_fetch_partial_block_parameters(block_index, &millimeters_remaining, &is_decelerating);
if (millimeters_remaining != 0.0) {
// Point to current block partially executed by stepper algorithm
plan_block_t *partial_block = plan_get_block_by_index(block_index);
// Compute the midway speed of the partially completely block at the end of the segment buffer.
if (is_decelerating) { // Block is decelerating
partial_block->entry_speed_sqr = exit_speed_sqr - 2*partial_block->acceleration*millimeters_remaining;
} else { // Block is accelerating or cruising
partial_block->entry_speed_sqr += 2*partial_block->acceleration*(partial_block->millimeters-millimeters_remaining);
partial_block->entry_speed_sqr = min(partial_block->entry_speed_sqr, partial_block->nominal_speed_sqr);
}
// Update only the relevant planner block information so the planner can plan correctly.
partial_block->millimeters = millimeters_remaining;
partial_block->max_entry_speed_sqr = partial_block->entry_speed_sqr; // Not sure if this needs to be updated.
}
}
/* PLANNER SPEED DEFINITION
+--------+ <- current->nominal_speed
/ \
current->entry_speed -> + \
| + <- next->entry_speed
| + <- next->entry_speed (aka exit speed)
+-------------+
time -->
@ -112,7 +153,7 @@ static uint8_t prev_block_index(uint8_t block_index)
in the entire buffer to accelerate up to the nominal speed and then decelerate to a stop at the end of the
buffer. There are a few simple solutions to this: (1) Maximize the machine acceleration. The planner will be
able to compute higher speed profiles within the same combined distance. (2) Increase line segment(s) distance.
The more combined distance the planner has to use, the faster it can go. (3) Increase the MINIMUM_PLANNER_SPEED.
The more combined distance the planner has to use, the faster it can go. (3) Increase the MINIMUM_JUNCTION_SPEED.
Not recommended. This will change what speed the planner plans to at the end of the buffer. Can lead to lost
steps when coming to a stop. (4) [BEST] Increase the planner buffer size. The more combined distance, the
bigger the balloon, or faster it can go. But this is not possible for 328p Arduinos because its limited memory
@ -123,54 +164,162 @@ static uint8_t prev_block_index(uint8_t block_index)
as possible. For example, in situations like arc generation or complex curves, the short, rapid line segments
can execute faster than new blocks can be added, and the planner buffer will then starve and empty, leading
to weird hiccup-like jerky motions.
Index mapping:
- block_buffer_head: Points to the newest incoming buffer block just added by plan_buffer_line(). The planner
never touches the exit speed of this block, which always defaults to MINIMUM_JUNCTION_SPEED.
- block_buffer_tail: Points to the beginning of the planner buffer. First to be executed or being executed.
Can dynamically change with the old stepper algorithm, but with the new algorithm, this should be impossible
as long as the segment buffer is not empty.
- next_buffer_head: Points to next planner buffer block after the last block. Should always be empty.
- block_buffer_safe: Points to the first planner block in the buffer for which it is safe to change. Since
the stepper can be executing the first block and if the planner changes its conditions, this will cause
a discontinuity and error in the stepper profile with lost steps likely. With the new stepper algorithm,
the block_buffer_safe is always where the stepper segment buffer ends and can never be overwritten, but
this can change the state of the block profile from a pure trapezoid assumption. Meaning, if that block
is decelerating, the planner conditions can change such that the block can new accelerate mid-block.
!!! I need to make sure that the stepper algorithm can modify the acceleration mid-block. Needed for feedrate overrides too.
!!! planner_recalculate() may not work correctly with re-planning.... may need to artificially set both the
block_buffer_head and next_buffer_head back one index so that this works correctly, or allow the operation
of this function to accept two different conditions to operate on.
- block_buffer_planned: Points to the first buffer block after the last optimally fixed block, which can no longer be
improved. This block and the trailing buffer blocks that can still be altered when new blocks are added. This planned
block points to the transition point between the fixed and non-fixed states and is handled slightly different. The entry
speed is fixed, indicating the reverse pass cannot maximize the speed further, but the velocity profile within it
can still be changed, meaning the forward pass calculations must start from here and influence the following block
entry speed.
!!! Need to check if this is the start of the non-optimal or the end of the optimal block.
*/
static void planner_recalculate()
{
// Last/newest block in buffer. Exit speed is set with MINIMUM_PLANNER_SPEED. Always recalculated.
uint8_t block_index = block_buffer_head;
plan_block_t *current = &block_buffer[block_index]; // Set as last/newest block in buffer
// Query stepper module for safe planner block index to recalculate to, which corresponds to the end
// of the step segment buffer.
uint8_t block_buffer_safe = st_get_prep_block_index();
// TODO: Make sure that we don't have to check for the block_buffer_tail condition, if the stepper module
// returns a NULL pointer or something. This could happen when the segment buffer is empty. Although,
// this call won't return a NULL, only an index.. I have to make sure that this index is synced with the
// planner at all times.
// Ping the stepper algorithm to check if we can alter the parameters of the currently executing
// block. If not, skip it and work on the next block.
// TODO: Need to look into if there are conditions where this fails.
uint8_t block_buffer_safe = next_block_index( block_buffer_tail );
/* - In theory, the state of the segment buffer can exist anywhere within the planner buffer tail and head-1
or is empty, when there is nothing in the segment queue. The safe pointer can be the buffer head only
when the planner queue has been entirely queued into the segment buffer and there are no more blocks
in the planner buffer. The segment buffer will to continue to execute the remainder of it, but the
planner should be able to treat a newly added block during this time as an empty planner buffer since
we can't touch the segment buffer.
// TODO: Need to recompute buffer tail millimeters based on how much is completed.
- The segment buffer is atomic to the planner buffer, because the main program computes these seperately.
Even if we move the planner head pointer early at the end of plan_buffer_line(), this shouldn't
effect the safe pointer.
if (block_buffer_safe == next_buffer_head) { // Only one safe block in buffer to operate on
- If the safe pointer is at head-1, this means that the stepper algorithm has segments queued and may
be executing. This is the last block in the planner queue, so it has been planned to decelerate to
zero at its end. When adding a new block, there will be at least two blocks to work with. When resuming,
from a feed hold, we only have this block and will be computing nothing. The planner doesn't have to
do anything, since the trapezoid calculations called by the stepper module should complete the block plan.
block_buffer_planned = block_buffer_safe;
// calculate_trapezoid_for_block(current, 0.0, MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED);
- In most cases, the safe pointer is at the plan tail or the block after, and rarely on the block two
beyond the tail. Since the safe pointer points to the block used at the end of the segment buffer, it
can be in any one of these states. As the stepper module executes the planner block, the buffer tail,
and hence the safe pointer, can push forward through the planner blocks and overcome the planned
pointer at any time.
- Does the reverse pass not touch either the safe or the plan pointer blocks? The plan pointer only
allows the velocity profile within it to be altered, but not the entry speed, so the reverse pass
ignores this block. The safe pointer is the same way, where the entry speed does not change, but
the velocity profile within it does.
- The planned pointer can exist anywhere in a given plan, except for the planner buffer head, if everything
operates as anticipated. Since the planner buffer can be executed by the stepper algorithm as any
rate and could empty the planner buffer quickly, the planner tail can overtake the planned pointer
at any time, but will never go around the ring buffer and re-encounter itself, the plan itself is not
changed by adding a new block or something else.
- The planner recalculate function should always reset the planned pointer at the proper break points
or when it encounters the safe block pointer, but will only do so when there are more than one block
in the buffer. In the case of single blocks, the planned pointer should always be set to the first
write-able block in the buffer, aka safe block.
- When does this not work? There might be an issue when the planned pointer moves from the tail to the
next head as a new block is being added and planned. Otherwise, the planned pointer should remain
static within the ring buffer no matter what the buffer is doing: being executed, adding new blocks,
or both simultaneously. Need to make sure that this case is covered.
*/
// Recompute plan only when there is more than one planner block in the buffer. Can't do anything with one.
// NOTE: block_buffer_safe can be equal to block_buffer_head if the segment buffer has completely queued up
// the remainder of the planner buffer. In this case, a new planner block will be treated as a single block.
if (block_buffer_head == block_buffer_safe) { // Also catches head = tail
// Just set block_buffer_planned pointer.
block_buffer_planned = block_buffer_head;
printString("z");
// TODO: Feedrate override of one block needs to update the partial block with an exit speed of zero. For
// a single added block and recalculate after a feed hold, we don't need to compute this, since we already
// know that the velocity starts and ends at zero. With an override, we can be traveling at some midblock
// rate, and we have to calculate the new velocity profile from it.
// plan_update_partial_block(block_index,0.0);
} else {
// TODO: need to account for the two block condition better. If the currently executing block
// is not safe, do we wait until its done? Can we treat the buffer head differently?
// TODO: If the nominal speeds change during a feedrate override, we need to recompute the max entry speeds for
// all junctions before proceeding.
// Calculate trapezoid for the last/newest block.
current->entry_speed_sqr = min( current->max_entry_speed_sqr,
MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED + 2*current->acceleration*current->millimeters);
// calculate_trapezoid_for_block(current, current->entry_speed_sqr, MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED);
// Initialize planner buffer pointers and indexing.
uint8_t block_index = block_buffer_head;
plan_block_t *current = &block_buffer[block_index];
// Calculate maximum entry speed for last block in buffer, where the exit speed is always zero.
current->entry_speed_sqr = min( current->max_entry_speed_sqr, 2*current->acceleration*current->millimeters);
// Reverse Pass: Back plan the deceleration curve from the last block in buffer. Cease
// planning when: (1) the last optimal planned pointer is reached. (2) the safe block
// pointer is reached, whereby the planned pointer is updated.
// Reverse Pass: Coarsely maximize all possible deceleration curves back-planning from the last
// block in buffer. Cease planning when: (1) the last optimal planned pointer is reached.
// (2) the safe block pointer is reached, whereby the planned pointer is updated.
// NOTE: Forward pass will later refine and correct the reverse pass to create an optimal plan.
// NOTE: If the safe block is encountered before the planned block pointer, we know the safe block
// will be recomputed within the plan. So, we need to update it if it is partially completed.
float entry_speed_sqr;
plan_block_t *next;
block_index = prev_block_index(block_index);
if (block_index == block_buffer_safe) { // !! OR plan pointer? Yes I think so.
// Only two plannable blocks in buffer. Compute previous block based on
// !!! May only work if a new block is being added. Not for an override. The exit speed isn't zero.
// !!! Need to make the current entry speed calculation after this.
plan_update_partial_block(block_index, 0.0);
block_buffer_planned = block_index;
printString("y");
} else {
// Three or more plan-able
while (block_index != block_buffer_planned) {
next = current;
current = &block_buffer[block_index];
// Exit loop and update planned pointer when the tail/safe block is reached.
// Increment block index early to check if the safe block is before the current block. If encountered,
// this is an exit condition as we can't go further than this block in the reverse pass.
block_index = prev_block_index(block_index);
if (block_index == block_buffer_safe) {
block_buffer_planned = block_buffer_safe;
break;
// Check if the safe block is partially completed. If so, update it before its exit speed
// (=current->entry speed) is over-written.
// TODO: The update breaks with feedrate overrides, because the replanning process no longer has
// the previous nominal speed to update this block with. There will need to be something along the
// lines of a nominal speed change check and send the correct value to this function.
plan_update_partial_block(block_index,current->entry_speed_sqr);
printString("x");
// Set planned pointer at safe block and for loop exit after following computation is done.
block_buffer_planned = block_index;
}
// Crudely maximize deceleration curve from the end of the non-optimally planned buffer to
// the optimal plan pointer. Forward pass will adjust and finish optimizing the plan.
// Compute maximum entry speed decelerating over the current block from its exit speed.
if (current->entry_speed_sqr != current->max_entry_speed_sqr) {
entry_speed_sqr = next->entry_speed_sqr + 2*current->acceleration*current->millimeters;
if (entry_speed_sqr < current->max_entry_speed_sqr) {
@ -179,13 +328,14 @@ static void planner_recalculate()
current->entry_speed_sqr = current->max_entry_speed_sqr;
}
}
block_index = prev_block_index(block_index);
}
}
// Forward Pass: Forward plan the acceleration curve from the planned pointer onward.
// Also scans for optimal plan breakpoints and appropriately updates the planned pointer.
block_index = block_buffer_planned; // Begin at buffer planned pointer
next = &block_buffer[prev_block_index(block_buffer_planned)]; // Set up for while loop
next = &block_buffer[block_buffer_planned]; // Begin at buffer planned pointer
block_index = next_block_index(block_buffer_planned);
while (block_index != next_buffer_head) {
current = next;
next = &block_buffer[block_index];
@ -194,22 +344,22 @@ static void planner_recalculate()
// pointer forward, since everything before this is all optimal. In other words, nothing
// can improve the plan from the buffer tail to the planned pointer by logic.
if (current->entry_speed_sqr < next->entry_speed_sqr) {
block_buffer_planned = block_index;
entry_speed_sqr = current->entry_speed_sqr + 2*current->acceleration*current->millimeters;
// If true, current block is full-acceleration and we can move the planned pointer forward.
if (entry_speed_sqr < next->entry_speed_sqr) {
next->entry_speed_sqr = entry_speed_sqr; // Always <= max_entry_speed_sqr. Backward pass set this.
next->entry_speed_sqr = entry_speed_sqr; // Always <= max_entry_speed_sqr. Backward pass sets this.
block_buffer_planned = block_index; // Set optimal plan pointer.
}
}
// Any block set at its maximum entry speed also creates an optimal plan up to this
// point in the buffer. The optimally planned pointer is updated.
// point in the buffer. When the plan is bracketed by either the beginning of the
// buffer and a maximum entry speed or two maximum entry speeds, every block in between
// cannot logically be further improved. Hence, we don't have to recompute them anymore.
if (next->entry_speed_sqr == next->max_entry_speed_sqr) {
block_buffer_planned = block_index;
block_buffer_planned = block_index; // Set optimal plan pointer
}
// Automatically recalculate trapezoid for all buffer blocks from last plan's optimal planned
// pointer to the end of the buffer, except the last block.
// calculate_trapezoid_for_block(current, current->entry_speed_sqr, next->entry_speed_sqr);
block_index = next_block_index( block_index );
}
@ -218,19 +368,24 @@ static void planner_recalculate()
}
void plan_reset_buffer()
{
block_buffer_planned = block_buffer_tail;
}
void plan_init()
{
block_buffer_head = 0;
block_buffer_tail = block_buffer_head;
next_buffer_head = next_block_index(block_buffer_head);
block_buffer_planned = block_buffer_head;
block_buffer_tail = 0;
block_buffer_head = 0; // Empty = tail
next_buffer_head = 1; // next_block_index(block_buffer_head)
plan_reset_buffer();
memset(&pl, 0, sizeof(pl)); // Clear planner struct
}
void plan_discard_current_block()
{
if (block_buffer_head != block_buffer_tail) {
if (block_buffer_head != block_buffer_tail) { // Discard non-empty buffer.
block_buffer_tail = next_block_index( block_buffer_tail );
}
}
@ -238,7 +393,10 @@ void plan_discard_current_block()
plan_block_t *plan_get_current_block()
{
if (block_buffer_head == block_buffer_tail) { return(NULL); }
if (block_buffer_head == block_buffer_tail) { // Buffer empty
plan_reset_buffer();
return(NULL);
}
return(&block_buffer[block_buffer_tail]);
}
@ -289,6 +447,8 @@ void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate)
block->acceleration = SOME_LARGE_VALUE; // Scaled down to maximum acceleration later
// Compute and store initial move distance data.
// TODO: After this for-loop, we don't touch the stepper algorithm data. Might be a good idea
// to try to keep these types of things completely separate from the planner for portability.
int32_t target_steps[N_AXIS];
float unit_vec[N_AXIS], delta_mm;
uint8_t idx;
@ -313,7 +473,7 @@ void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate)
}
block->millimeters = sqrt(block->millimeters); // Complete millimeters calculation with sqrt()
// Bail if this is a zero-length block
// Bail if this is a zero-length block. Highly unlikely to occur.
if (block->step_event_count == 0) { return; }
// Adjust feed_rate value to mm/min depending on type of rate input (normal, inverse time, or rapids)
@ -346,7 +506,21 @@ void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate)
}
}
/* Compute maximum allowable entry speed at junction by centripetal acceleration approximation.
// TODO: Need to check this method handling zero junction speeds when starting from rest.
if (block_buffer_head == block_buffer_tail) {
// Initialize block entry speed as zero. Assume it will be starting from rest. Planner will correct this later.
// !!! Ensures when the first block starts from zero speed. If we do this in the planner, this will break
// feedrate overrides later, as you can override this single block and it maybe moving already at a given rate.
// Better to do it here and make it clean.
// !!! Shouldn't need this for anything other than a single block.
block->entry_speed_sqr = 0.0;
block->max_junction_speed_sqr = 0.0; // Starting from rest. Enforce start from zero velocity.
} else {
/*
Compute maximum allowable entry speed at junction by centripetal acceleration approximation.
Let a circle be tangent to both previous and current path line segments, where the junction
deviation is defined as the distance from the junction to the closest edge of the circle,
colinear with the circle center. The circular segment joining the two paths represents the
@ -355,31 +529,35 @@ void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate)
path width or max_jerk in the previous grbl version. This approach does not actually deviate
from path, but used as a robust way to compute cornering speeds, as it takes into account the
nonlinearities of both the junction angle and junction velocity.
NOTE: If the junction deviation value is finite, Grbl executes the motions in an exact path
mode (G61). If the junction deviation value is zero, Grbl will execute the motion in an exact
stop mode (G61.1) manner. In the future, if continuous mode (G64) is desired, the math here
is exactly the same. Instead of motioning all the way to junction point, the machine will
just follow the arc circle defined here. The Arduino doesn't have the CPU cycles to perform
a continuous mode path, but ARM-based microcontrollers most certainly do.
NOTE: The max junction speed is a fixed value, since machine acceleration limits cannot be
changed dynamically during operation nor can the line segment geometry. This must be kept in
memory in the event of a feedrate override changing the nominal speeds of blocks, which can
change the overall maximum entry speed conditions of all blocks.
*/
// TODO: Acceleration need to be limited by the minimum of the two junctions.
// TODO: Need to setup a method to handle zero junction speeds when starting from rest.
if (block_buffer_head == block_buffer_tail) {
block->max_entry_speed_sqr = MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED;
} else {
// NOTE: Computed without any expensive trig, sin() or acos(), by trig half angle identity of cos(theta).
float sin_theta_d2 = sqrt(0.5*(1.0-junction_cos_theta)); // Trig half angle identity. Always positive.
block->max_entry_speed_sqr = (block->acceleration * settings.junction_deviation * sin_theta_d2)/(1.0-sin_theta_d2);
// TODO: Acceleration used in calculation needs to be limited by the minimum of the two junctions.
block->max_junction_speed_sqr = max( MINIMUM_JUNCTION_SPEED*MINIMUM_JUNCTION_SPEED,
(block->acceleration * settings.junction_deviation * sin_theta_d2)/(1.0-sin_theta_d2) );
}
// Store block nominal speed and rate
// Store block nominal speed
block->nominal_speed_sqr = feed_rate*feed_rate; // (mm/min). Always > 0
// block->nominal_rate = ceil(feed_rate*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic)
//
// // Compute and store acceleration and distance traveled per step event.
// block->rate_delta = ceil(block->acceleration*
// ((INV_TIME_MULTIPLIER/(60.0*60.0))/(ISR_TICKS_PER_SECOND*ACCELERATION_TICKS_PER_SECOND))); // (mult*mm/isr_tic/accel_tic)
// block->d_next = ceil((block->millimeters*INV_TIME_MULTIPLIER)/block->step_event_count); // (mult*mm/step)
// Compute the junction maximum entry based on the minimum of the junction speed and neighboring nominal speeds.
// TODO: Should call a function to determine this. The function can be used elsewhere for feedrate overrides later.
block->max_entry_speed_sqr = min(block->max_junction_speed_sqr,
min(block->nominal_speed_sqr,pl.previous_nominal_speed_sqr));
// Update previous path unit_vector and nominal speed (squared)
memcpy(pl.previous_unit_vec, unit_vec, sizeof(unit_vec)); // pl.previous_unit_vec[] = unit_vec[]
@ -390,11 +568,17 @@ void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate)
planner_recalculate();
// Update buffer head and next buffer head indices.
// NOTE: The buffer head update is atomic since it's one byte. Performed after the new plan
// calculations to help prevent overwriting scenarios with adding a new block to a low buffer.
// Update buffer head and next buffer head indices. Advance only after new plan has been computed.
block_buffer_head = next_buffer_head;
next_buffer_head = next_block_index(block_buffer_head);
int32_t blength = block_buffer_head - block_buffer_tail;
if (blength < 0) { blength += BLOCK_BUFFER_SIZE; }
printInteger(blength);
}
@ -408,6 +592,8 @@ void plan_sync_position()
}
/* STEPPER VELOCITY PROFILE DEFINITION
less than nominal rate-> +
+--------+ <- nominal_rate /|\
@ -419,31 +605,35 @@ void plan_sync_position()
| | | |
decelerate distance decelerate distance
Calculates trapezoid parameters for stepper algorithm. Each block velocity profiles can be
described as either a trapezoidal or a triangular shape. The trapezoid occurs when the block
reaches the nominal speed of the block and cruises for a period of time. A triangle occurs
when the nominal speed is not reached within the block. Some other special cases exist,
such as pure ac/de-celeration velocity profiles from beginning to end or a trapezoid that
has no deceleration period when the next block resumes acceleration.
Calculates the "trapezoid" velocity profile parameters of a planner block for the stepper
algorithm. The planner computes the entry and exit speeds of each block, but does not bother to
determine the details of the velocity profiles within them, as they aren't needed for computing
an optimal plan. When the stepper algorithm begins to execute a block, the block velocity profiles
are computed ad hoc.
Each block velocity profiles can be described as either a trapezoidal or a triangular shape. The
trapezoid occurs when the block reaches the nominal speed of the block and cruises for a period of
time. A triangle occurs when the nominal speed is not reached within the block. Both of these
velocity profiles may also be truncated on either end with no acceleration or deceleration ramps,
as they can be influenced by the conditions of neighboring blocks.
The following function determines the type of velocity profile and stores the minimum required
information for the stepper algorithm to execute the calculated profiles. In this case, only
the new initial rate and n_steps until deceleration are computed, since the stepper algorithm
already handles acceleration and cruising and just needs to know when to start decelerating.
information for the stepper algorithm to execute the calculated profiles. Since the stepper
algorithm always assumes to begin accelerating from the initial_rate and cruise if the nominal_rate
is reached, we only need to know when to begin deceleration to the end of the block. Hence, only
the distance from the end of the block to begin a deceleration ramp are computed.
*/
int32_t calculate_trapezoid_for_block(uint8_t block_index)
float plan_calculate_velocity_profile(uint8_t block_index)
{
plan_block_t *current_block = &block_buffer[block_index];
// Determine current block exit speed
float exit_speed_sqr;
uint8_t next_index = next_block_index(block_index);
plan_block_t *next_block = plan_get_block_by_index(next_index);
if (next_block == NULL) { exit_speed_sqr = 0; } // End of planner buffer. Zero speed.
else { exit_speed_sqr = next_block->entry_speed_sqr; } // Entry speed of next block
float exit_speed_sqr = 0.0; // Initialize for end of planner buffer. Zero speed.
plan_block_t *next_block = plan_get_block_by_index(next_block_index(block_index));
if (next_block != NULL) { exit_speed_sqr = next_block->entry_speed_sqr; } // Exit speed is the entry speed of next buffer block
// First determine intersection distance (in steps) from the exit point for a triangular profile.
// Computes: steps_intersect = steps/mm * ( distance/2 + (v_entry^2-v_exit^2)/(4*acceleration) )
// Computes: d_intersect = distance/2 + (v_entry^2-v_exit^2)/(4*acceleration)
float intersect_distance = 0.5*( current_block->millimeters + (current_block->entry_speed_sqr-exit_speed_sqr)/(2*current_block->acceleration) );
// Check if this is a pure acceleration block by a intersection distance less than zero. Also
@ -452,18 +642,17 @@ int32_t calculate_trapezoid_for_block(uint8_t block_index)
float decelerate_distance;
// Determine deceleration distance (in steps) from nominal speed to exit speed for a trapezoidal profile.
// Value is never negative. Nominal speed is always greater than or equal to the exit speed.
// Computes: steps_decelerate = steps/mm * ( (v_nominal^2 - v_exit^2)/(2*acceleration) )
// Computes: d_decelerate = (v_nominal^2 - v_exit^2)/(2*acceleration)
decelerate_distance = (current_block->nominal_speed_sqr - exit_speed_sqr)/(2*current_block->acceleration);
// The lesser of the two triangle and trapezoid distances always defines the velocity profile.
if (decelerate_distance > intersect_distance) { decelerate_distance = intersect_distance; }
// Finally, check if this is a pure deceleration block.
if (decelerate_distance > current_block->millimeters) { decelerate_distance = current_block->millimeters; }
return(ceil(((current_block->millimeters-decelerate_distance)*current_block->step_event_count)/ current_block->millimeters));
if (decelerate_distance > current_block->millimeters) { return(0.0); }
else { return( (current_block->millimeters-decelerate_distance) ); }
}
return(0);
return( current_block->millimeters ); // No deceleration in velocity profile.
}
@ -481,7 +670,7 @@ void plan_cycle_reinitialize(int32_t step_events_remaining)
// Re-plan from a complete stop. Reset planner entry speeds and buffer planned pointer.
block->entry_speed_sqr = 0.0;
block->max_entry_speed_sqr = MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED;
block->max_entry_speed_sqr = 0.0;
block_buffer_planned = block_buffer_tail;
planner_recalculate();
}

20
planner.h
View File

@ -33,19 +33,19 @@
typedef struct {
// Fields used by the bresenham algorithm for tracing the line
// NOTE: Do not change any of these values once set. The stepper algorithm uses them to execute the block correctly.
uint8_t direction_bits; // The direction bit set for this block (refers to *_DIRECTION_BIT in config.h)
int32_t steps[N_AXIS]; // Step count along each axis
int32_t step_event_count; // The number of step events required to complete this block
int32_t step_event_count; // The maximum step axis count and number of steps required to complete this block.
// Fields used by the motion planner to manage acceleration
float entry_speed_sqr; // The current planned entry speed at block junction in (mm/min)^2
float max_entry_speed_sqr; // Maximum allowable entry speed based on the minimum of junction limit and
// neighboring nominal speeds with overrides in (mm/min)^2
float max_junction_speed_sqr; // Junction entry speed limit based on direction vectors in (mm/min)^2
float nominal_speed_sqr; // Axis-limit adjusted nominal speed for this block in (mm/min)^2
float entry_speed_sqr; // Entry speed at previous-current block junction in (mm/min)^2
float max_entry_speed_sqr; // Maximum allowable junction entry speed in (mm/min)^2
float acceleration; // Axes-limit adjusted line acceleration in mm/min^2
float millimeters; // The total travel for this block to be executed in mm
// Settings for the trapezoid generator
// int32_t decelerate_after; // The index of the step event on which to start decelerating
float acceleration; // Axis-limit adjusted line acceleration in mm/min^2
float millimeters; // The remaining distance for this block to be executed in mm
} plan_block_t;
@ -66,7 +66,9 @@ plan_block_t *plan_get_current_block();
plan_block_t *plan_get_block_by_index(uint8_t block_index);
int32_t calculate_trapezoid_for_block(uint8_t block_index);
float plan_calculate_velocity_profile(uint8_t block_index);
// void plan_update_partial_block(uint8_t block_index, float millimeters_remaining, uint8_t is_decelerating);
// Reset the planner position vector (in steps)
void plan_sync_position();

12
serial.c
View File

@ -91,11 +91,7 @@ void serial_write(uint8_t data) {
}
// Data Register Empty Interrupt handler
#ifdef __AVR_ATmega644P__
ISR(USART0_UDRE_vect)
#else
ISR(USART_UDRE_vect)
#endif
ISR(SERIAL_UDRE)
{
// Temporary tx_buffer_tail (to optimize for volatile)
uint8_t tail = tx_buffer_tail;
@ -144,11 +140,7 @@ uint8_t serial_read()
}
}
#ifdef __AVR_ATmega644P__
ISR(USART0_RX_vect)
#else
ISR(USART_RX_vect)
#endif
ISR(SERIAL_RX)
{
uint8_t data = UDR0;
uint8_t next_head;

237
stepper.c
View File

@ -42,7 +42,7 @@
#define ST_DECEL 2
#define ST_DECEL_EOB 3
#define SEGMENT_BUFFER_SIZE 10
#define SEGMENT_BUFFER_SIZE 6
// Stepper state variable. Contains running data and trapezoid variables.
typedef struct {
@ -50,7 +50,7 @@ typedef struct {
int32_t counter_x, // Counter variables for the bresenham line tracer
counter_y,
counter_z;
uint8_t segment_steps_remaining; // Steps remaining in line motion
uint8_t segment_steps_remaining; // Steps remaining in line segment motion
// Used by inverse time algorithm to track step rate
int32_t counter_d; // Inverse time distance traveled since last step event
@ -68,7 +68,9 @@ typedef struct {
} stepper_t;
static stepper_t st;
// Stores stepper buffer common data. Can change planner mid-block in special conditions.
// Stores stepper buffer common data for a planner block. Data can change mid-block when the planner
// updates the remaining block velocity profile with a more optimal plan or a feedrate override occurs.
// NOTE: Normally, this buffer is only partially used, but can fill up completely in certain conditions.
typedef struct {
int32_t step_events_remaining; // Tracks step event count for the executing planner block
uint32_t d_next; // Scaled distance to next step
@ -80,16 +82,17 @@ typedef struct {
} st_data_t;
static st_data_t segment_data[SEGMENT_BUFFER_SIZE];
// Primary stepper motion buffer
// Primary stepper buffer. Contains small, short line segments for the stepper algorithm to execute checked
// out incrementally from the first block in the planner buffer. These step segments
typedef struct {
uint8_t n_step;
uint8_t st_data_index;
uint8_t flag;
uint8_t n_step; // Number of step events to be executed for this segment
uint8_t st_data_index; // Stepper buffer common data index. Uses this information to execute this segment.
uint8_t flag; // Stepper algorithm execution flag to notify special conditions.
} st_segment_t;
static st_segment_t segment_buffer[SEGMENT_BUFFER_SIZE];
static volatile uint8_t segment_buffer_tail;
static uint8_t segment_buffer_head;
static volatile uint8_t segment_buffer_head;
static uint8_t segment_next_head;
static volatile uint8_t busy; // Used to avoid ISR nesting of the "Stepper Driver Interrupt". Should never occur though.
@ -97,11 +100,16 @@ static plan_block_t *pl_current_block; // A pointer to the planner block curren
static st_segment_t *st_current_segment;
static st_data_t *st_current_data;
// Pointers for the step segment being prepped from the planner buffer. Accessed only by the
// main program. Pointers may be planning segments or planner blocks ahead of what being executed.
static plan_block_t *pl_prep_block; // A pointer to the planner block being prepped into the stepper buffer
static uint8_t pl_prep_index;
static st_data_t *st_prep_data;
static uint8_t st_data_prep_index;
static uint8_t pl_partial_block_flag;
// Returns the index of the next block in the ring buffer
// NOTE: Removed modulo (%) operator, which uses an expensive divide and multiplication.
@ -115,7 +123,7 @@ static uint8_t next_block_index(uint8_t block_index)
static uint8_t next_block_pl_index(uint8_t block_index)
{
block_index++;
if (block_index == 18) { block_index = 0; }
if (block_index == BLOCK_BUFFER_SIZE) { block_index = 0; }
return(block_index);
}
@ -255,22 +263,20 @@ ISR(TIMER2_COMPA_vect)
// Initialize inverse time and step rate counter data
st.counter_d = st_current_data->d_next; // d_next always greater than delta_d.
if (st.delta_d < MINIMUM_STEP_RATE) { st.d_per_tick = MINIMUM_STEP_RATE; }
else { st.d_per_tick = st.delta_d; }
// During feed hold, do not update rate or ramp type. Keep decelerating.
// if (sys.state == STATE_CYCLE) {
st.delta_d = st_current_data->initial_rate;
// if (st.delta_d == st_current_data->nominal_rate) {
// st.ramp_type = RAMP_NOOP_CRUISE;
st.ramp_type = RAMP_ACCEL;
st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; // Set ramp counter for trapezoid
if (st.delta_d == st_current_data->nominal_rate) { st.ramp_type = RAMP_NOOP_CRUISE; }
else { st.ramp_type = RAMP_ACCEL; }
// }
// }
if (st.delta_d < MINIMUM_STEP_RATE) { st.d_per_tick = MINIMUM_STEP_RATE; }
else { st.d_per_tick = st.delta_d; }
}
// Acceleration and cruise handled by ramping. Just check for deceleration.
// Acceleration and cruise handled by ramping. Just check if deceleration needs to begin.
if (st_current_segment->flag == ST_DECEL || st_current_segment->flag == ST_DECEL_EOB) {
if (st.ramp_type == RAMP_NOOP_CRUISE) {
st.ramp_count = ISR_TICKS_PER_ACCELERATION_TICK/2; // Set ramp counter for trapezoid
@ -292,6 +298,8 @@ ISR(TIMER2_COMPA_vect)
}
// Adjust inverse time counter for ac/de-celerations
// NOTE: Accelerations are handled by the stepper algorithm as it's thought to be more computationally
// efficient on the Arduino AVR. This could change eventually, but it definitely will with ARM development.
if (st.ramp_type) {
st.ramp_count--; // Tick acceleration ramp counter
if (st.ramp_count == 0) { // Adjust step rate when its time
@ -396,14 +404,20 @@ ISR(TIMER0_OVF_vect)
void st_reset()
{
memset(&st, 0, sizeof(st));
pl_current_block = NULL;
pl_prep_block = NULL;
pl_prep_index = 0;
st_data_prep_index = 0;
st.load_flag = LOAD_BLOCK;
busy = false;
pl_current_block = NULL; // Planner block pointer used by stepper algorithm
pl_prep_block = NULL; // Planner block pointer used by segment buffer
pl_prep_index = 0; // Planner buffer indices are also reset to zero.
st_data_prep_index = 0;
segment_buffer_tail = 0;
segment_buffer_head = 0; // empty = tail
segment_next_head = 1;
pl_partial_block_flag = false;
}
@ -485,7 +499,7 @@ void st_cycle_reinitialize()
}
/* Preps stepper buffer. Called from main program.
/* Prepares step segment buffer. Continuously called from main program.
NOTE: There doesn't seem to be a great way to figure out how many steps occur within
a set number of ISR ticks. Numerical round-off and CPU overhead always seems to be a
@ -510,41 +524,75 @@ void st_cycle_reinitialize()
been doing here limit this phase issue by truncating some of the ramp timing for certain
events like deceleration initialization and end of block.
*/
// !!! Need to make sure when a single partially completed block can be re-computed here with
// new deceleration point and the segment manager begins accelerating again immediately.
void st_prep_buffer()
{
while (segment_buffer_tail != segment_next_head) { // Check if we need to fill the buffer.
// Determine if we need to load a new planner block.
if (pl_prep_block == NULL) {
pl_prep_block = plan_get_block_by_index(pl_prep_index);
if (pl_prep_block == NULL) { return; } // No more planner blocks. Let stepper finish out.
pl_prep_block = plan_get_block_by_index(pl_prep_index); // Query planner for a queued block
if (pl_prep_block == NULL) { return; } // No planner blocks. Exit.
// Prepare commonly shared planner block data for the ensuing step buffer moves
// Check if the planner has re-computed this block mid-execution. If so, push the old segment block
// data Otherwise, prepare a new segment block data.
if (pl_partial_block_flag) {
// Prepare new shared segment block data and copy the relevant last segment block data.
st_data_t *last_st_prep_data;
last_st_prep_data = &segment_data[st_data_prep_index];
st_data_prep_index = next_block_index(st_data_prep_index);
st_prep_data = &segment_data[st_data_prep_index];
st_prep_data->step_events_remaining = last_st_prep_data->step_events_remaining;
st_prep_data->rate_delta = last_st_prep_data->rate_delta;
st_prep_data->d_next = last_st_prep_data->d_next;
st_prep_data->nominal_rate = last_st_prep_data->nominal_rate; // TODO: Recompute with feedrate overrides.
st_prep_data->mm_per_step = last_st_prep_data->mm_per_step;
pl_partial_block_flag = false; // Reset flag
// TODO: If the planner updates this block, particularly from a deceleration to an acceleration,
// we must reload the initial rate data, such that the velocity profile is re-constructed correctly.
// The stepper algorithm must be flagged to adjust the acceleration counters.
} else {
// Prepare commonly shared planner block data for the ensuing segment buffer moves ad-hoc, since
// the planner buffer can dynamically change the velocity profile data as blocks are added.
st_data_prep_index = next_block_index(st_data_prep_index);
st_prep_data = &segment_data[st_data_prep_index];
// Initialize Bresenham variables
st_prep_data->step_events_remaining = pl_prep_block->step_event_count;
// Convert new block to stepper variables.
// NOTE: This data can change mid-block from normal planner updates and feedrate overrides. Must
// be maintained as these execute.
// TODO: If the planner updates this block, particularly from a deceleration to an acceleration,
// we must reload the initial rate data, such that the velocity profile is re-constructed correctly.
st_prep_data->initial_rate = ceil(sqrt(pl_prep_block->entry_speed_sqr)*(INV_TIME_MULTIPLIER/(60*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic)
// Convert planner block velocity profile data to stepper rate and step distance data.
st_prep_data->nominal_rate = ceil(sqrt(pl_prep_block->nominal_speed_sqr)*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic)
// This data doesn't change. Could be performed in the planner, but fits nicely here.
// Although, acceleration can change for S-curves. So keep it here.
st_prep_data->rate_delta = ceil(pl_prep_block->acceleration*
((INV_TIME_MULTIPLIER/(60.0*60.0))/(ISR_TICKS_PER_SECOND*ACCELERATION_TICKS_PER_SECOND))); // (mult*mm/isr_tic/accel_tic)
// This definitely doesn't change, but could be precalculated in a way to help some of the
// math in this handler, i.e. millimeters per step event data.
st_prep_data->d_next = ceil((pl_prep_block->millimeters*INV_TIME_MULTIPLIER)/pl_prep_block->step_event_count); // (mult*mm/step)
// TODO: Check if we really need to store this.
st_prep_data->mm_per_step = pl_prep_block->millimeters/pl_prep_block->step_event_count;
// Calculate trapezoid data from planner.
st_prep_data->decelerate_after = calculate_trapezoid_for_block(pl_prep_index);
}
// Convert planner entry speed to stepper initial rate.
st_prep_data->initial_rate = ceil(sqrt(pl_prep_block->entry_speed_sqr)*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic)
// TODO: Nominal rate changes with feedrate override.
// st_prep_data->nominal_rate = ceil(sqrt(pl_prep_block->nominal_speed_sqr)*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic)
// Calculate the planner block velocity profile type and determine deceleration point.
float mm_decelerate_after = plan_calculate_velocity_profile(pl_prep_index);
if (mm_decelerate_after == pl_prep_block->millimeters) {
st_prep_data->decelerate_after = st_prep_data->step_events_remaining;
} else {
st_prep_data->decelerate_after = ceil( mm_decelerate_after/st_prep_data->mm_per_step );
}
}
@ -564,77 +612,108 @@ void st_prep_buffer()
- From deceleration to acceleration, i.e. common with jogging when new blocks are added.
*/
st_segment_t *st_prep_segment = &segment_buffer[segment_buffer_head];
st_prep_segment->st_data_index = st_data_prep_index;
st_segment_t *new_segment = &segment_buffer[segment_buffer_head];
new_segment->st_data_index = st_data_prep_index;
// TODO: How do you cheaply compute n_step without a sqrt()? Could be performed as 'bins'.
st_prep_segment->n_step = 250; //floor( (exit_speed*approx_time)/mm_per_step );
// st_segment->n_step = max(st_segment->n_step,MINIMUM_STEPS_PER_BLOCK); // Ensure it moves for very slow motions?
// st_segment->n_step = min(st_segment->n_step,MAXIMUM_STEPS_PER_BLOCK); // Prevent unsigned int8 overflow.
// The basic equation is: s = u*t + 0.5*a*t^2
// For the most part, we can store the acceleration portion in the st_data buffer and all
// we would need to do is track the current approximate speed per loop with: v = u + a*t
// Each loop would require 3 multiplication and 2 additions, since most of the variables
// are constants and would get compiled out.
//!!! Doesn't work as is. Requires last_velocity and acceleration in terms of steps, not mm.
// new_segment->n_step = ceil(last_velocity*TIME_PER_SEGMENT/mm_per_step);
// if (st_prep_data->decelerate_after > 0) {
// new_segment->n_step += ceil(pl_prep_block->acceleration*(0.5*TIME_PER_SEGMENT*TIME_PER_SEGMENT/(60*60))/mm_per_step);
// } else {
// new_segment->n_step -= ceil(pl_prep_block->acceleration*(0.5*TIME_PER_SEGMENT*TIME_PER_SEGMENT/(60*60))/mm_per_step);
// }
new_segment->n_step = 7; //floor( (exit_speed*approx_time)/mm_per_step );
// new_segment->n_step = max(new_segment->n_step,MINIMUM_STEPS_PER_BLOCK); // Ensure it moves for very slow motions?
// new_segment->n_step = min(new_segment->n_step,MAXIMUM_STEPS_PER_BLOCK); // Prevent unsigned int8 overflow.
// Check if n_step exceeds steps remaining in planner block. If so, truncate.
if (st_prep_segment->n_step > st_prep_data->step_events_remaining) {
st_prep_segment->n_step = st_prep_data->step_events_remaining;
if (new_segment->n_step > st_prep_data->step_events_remaining) {
new_segment->n_step = st_prep_data->step_events_remaining;
// Don't need to compute last velocity, since it will be refreshed with a new block.
}
// Check if n_step exceeds decelerate point in block. Need to perform this so that the
// ramp counters are reset correctly in the stepper algorithm. Can be 1 step, but should
// be OK since it is likely moving at a fast rate already.
if (st_prep_data->decelerate_after > 0) {
if (st_prep_segment->n_step > st_prep_data->decelerate_after) {
st_prep_segment->n_step = st_prep_data->decelerate_after;
if (new_segment->n_step > st_prep_data->decelerate_after) {
new_segment->n_step = st_prep_data->decelerate_after;
}
// !!! Doesn't work. Remove if not using.
// if (last_velocity < last_nominal_v) {
// // !!! Doesn't work since distance changes and gets truncated.
// last_velocity += pl_prep_block->acceleration*(TIME_PER_SEGMENT/(60*60)); // In acceleration ramp.
// if {last_velocity > last_nominal_v) { last_velocity = last_nominal_v; } // Set to cruising.
// }
// } else { // In deceleration ramp
// last_velocity -= pl_prep_block->acceleration*(TIME_PER_SEGMENT/(60*60));
}
// float distance, exit_speed_sqr;
// distance = st_prep_segment->n_step*st_prep_data->mm_per_step; // Always greater than zero
// if (st_prep_data->step_events_remaining >= pl_prep_block->decelerate_after) {
// exit_speed_sqr = pl_prep_block->entry_speed_sqr - 2*pl_prep_block->acceleration*distance;
// } else { // Acceleration or cruising ramp
// if (pl_prep_block->entry_speed_sqr < pl_prep_block->nominal_speed_sqr) {
// exit_speed_sqr = pl_prep_block->entry_speed_sqr + 2*pl_prep_block->acceleration*distance;
// if (exit_speed_sqr > pl_prep_block->nominal_speed_sqr) { exit_speed_sqr = pl_prep_block->nominal_speed_sqr; }
// } else {
// exit_speed_sqr = pl_prep_block->nominal_speed_sqr;
// }
// }
// Update planner block variables.
// pl_prep_block->entry_speed_sqr = max(0.0,exit_speed_sqr);
// pl_prep_block->max_entry_speed_sqr = exit_speed_sqr; // ??? Overwrites the corner speed. May need separate variable.
// pl_prep_block->millimeters -= distance; // Potential round-off error near end of block.
// pl_prep_block->millimeters = max(0.0,pl_prep_block->millimeters); // Shouldn't matter.
// Update stepper block variables.
st_prep_data->step_events_remaining -= st_prep_segment->n_step;
st_prep_data->step_events_remaining -= new_segment->n_step;
if ( st_prep_data->step_events_remaining == 0 ) {
// Move planner pointer to next block
if (st_prep_data->decelerate_after == 0) {
st_prep_segment->flag = ST_DECEL_EOB;
new_segment->flag = ST_DECEL_EOB; // Flag when deceleration begins and ends at EOB. Could rewrite to use bit flags too.
} else {
st_prep_segment->flag = ST_END_OF_BLOCK;
new_segment->flag = ST_END_OF_BLOCK;
}
pl_prep_index = next_block_pl_index(pl_prep_index);
pl_prep_block = NULL;
printString("EOB");
} else {
// Current segment is mid-planner block. Just set the DECEL/NOOP acceleration flags.
if (st_prep_data->decelerate_after == 0) {
st_prep_segment->flag = ST_DECEL;
new_segment->flag = ST_DECEL;
} else {
st_prep_segment->flag = ST_NOOP;
new_segment->flag = ST_NOOP;
}
printString("x");
st_prep_data->decelerate_after -= new_segment->n_step;
}
st_prep_data->decelerate_after -= st_prep_segment->n_step;
// New step block completed. Increment step buffer indices.
// New step segment completed. Increment segment buffer indices.
segment_buffer_head = segment_next_head;
segment_next_head = next_block_index(segment_buffer_head);
printInteger((long)st_prep_segment->n_step);
printString(" ");
printInteger((long)st_prep_data->decelerate_after);
printString(" ");
printInteger((long)st_prep_data->step_events_remaining);
}
}
uint8_t st_get_prep_block_index()
{
// Returns only the index but doesn't state if the block has been partially executed. How do we simply check for this?
return(pl_prep_index);
}
void st_fetch_partial_block_parameters(uint8_t block_index, float *millimeters_remaining, uint8_t *is_decelerating)
{
// if called, can we assume that this always changes and needs to be updated? if so, then
// we can perform all of the segment buffer setup tasks here to make sure the next time
// the segments are loaded, the st_data buffer is updated correctly.
// !!! Make sure that this is always pointing to the correct st_prep_data block.
// When a mid-block acceleration occurs, we have to make sure the ramp counters are updated
// correctly, much in the same fashion as the deceleration counters. Need to think about this
// make sure this is right, but i'm pretty sure it is.
// TODO: NULL means that the segment buffer has completed the block. Need to clean this up a bit.
if (pl_prep_block != NULL) {
*millimeters_remaining = st_prep_data->step_events_remaining*st_prep_data->mm_per_step;
if (st_prep_data->decelerate_after > 0) { *is_decelerating = false; }
else { *is_decelerating = true; }
// Flag for new prep_block when st_prep_buffer() is called after the planner recomputes.
pl_partial_block_flag = true;
pl_prep_block = NULL;
}
return;
}

4
stepper.h
View File

@ -47,4 +47,8 @@ void st_feed_hold();
void st_prep_buffer();
uint8_t st_get_prep_block_index();
void st_fetch_partial_block_parameters(uint8_t block_index, float *millimeters_remaining, uint8_t *is_decelerating);
#endif