pcomplete, fully untested, support for accelleration management with look ahead optimization, finally
This commit is contained in:
Simen Svale Skogsrud
parent
d89b86d3bf
commit
5880e55ce9
2
Makefile
2
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
|
||||
|
||||
|
||||
6
config.c
6
config.c
@ -26,6 +26,8 @@
|
||||
#include "wiring_serial.h"
|
||||
#include <avr/pgmspace.h>
|
||||
|
||||
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"));
|
||||
}
|
||||
|
||||
|
||||
4
config.h
4
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
|
||||
|
||||
4
main.c
4
main.c
@ -21,7 +21,7 @@
|
||||
#include <avr/io.h>
|
||||
#include <avr/sleep.h>
|
||||
#include <util/delay.h>
|
||||
#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
|
||||
|
||||
@ -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.
|
||||
|
||||
@ -29,7 +29,7 @@
|
||||
#include <util/delay.h>
|
||||
#include "nuts_bolts.h"
|
||||
#include <avr/interrupt.h>
|
||||
#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();
|
||||
}
|
||||
|
||||
@ -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 <math.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
#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_factor<entry_factor) {
|
||||
double max_entry_speed = estimate_max_speed(-settings.acceleration,current->nominal_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
|
||||
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#ifndef motion_plan_h
|
||||
#define motion_plan_h
|
||||
#ifndef stepper_plan_h
|
||||
#define stepper_plan_h
|
||||
|
||||
#include <inttypes.h>
|
||||
|
||||
@ -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
|
||||
Loading…
Reference in New Issue
Block a user