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

Updated limit/homing routine. Works, but needs more TLC.

Browse Source 
- Added acceleration to the homing routine.

- Homing now accounts for different step rates when moving multiple
axes without exceeding acceleration limits.

- Homing now updates all internal positioning variables to machine zero
after completion.

- "Poor-man's" debounce delay added.

- Updated the delay_us() function to perform faster and more accurate
microsecond delays. Previously, the single increments would add
noticeable time drift for larger delays.

- Fix a bug in the stepper.c prescalar calculations that was changed in
the last commit.

- Other minor fixes.
This commit is contained in:
Sonny Jeon 2012-09-30 19:57:10 -06:00
parent 4224ab4999
commit d30cb906f8
10 changed files with 201 additions and 88 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

51
config.h
View File

@ -40,29 +40,32 @@
#define STEPPERS_DISABLE_PORT PORTB
#define STEPPERS_DISABLE_BIT 0 // Uno Digital Pin 8
#define LIMIT_DDR DDRB
#define LIMIT_PIN PINB
#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_DDR DDRB
#define LIMIT_PIN PINB
#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 settings
// #define LIMIT_INT_vect PCINT0_vect
// #define LIMIT_PCMSK PCMSK0
#define SPINDLE_ENABLE_DDR DDRB
#define SPINDLE_ENABLE_PORT PORTB
#define SPINDLE_ENABLE_BIT 4 // Uno Digital Pin 12
#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
#define SPINDLE_DIRECTION_DDR DDRB
#define SPINDLE_DIRECTION_PORT PORTB
#define SPINDLE_DIRECTION_BIT 5 // Uno Digital Pin 13
#define COOLANT_FLOOD_DDR DDRC
#define COOLANT_FLOOD_PORT PORTC
#define COOLANT_FLOOD_BIT 0 // Uno Analog Pin 0
#define COOLANT_FLOOD_PORT PORTC
#define COOLANT_FLOOD_BIT 0 // Uno Analog Pin 0
// #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 1 // Uno Analog Pin 1
#define COOLANT_MIST_PORT PORTC
#define COOLANT_MIST_BIT 1 // Uno Analog Pin 1
#endif
// Define runtime command special characters. These characters are 'picked-off' directly from the
@ -86,7 +89,7 @@
// entering g-code into grbl, i.e. locating part zero or simple manual machining. If the axes drift,
// grbl has no way to know this has happened, since stepper motors are open-loop control. Depending
// on the machine, this parameter may need to be larger or smaller than the default time.
// NOTE: If the define commented, the delay will not be compiled.
// NOTE: If the define commented, the stepper lock will be disabled upon compiling.
#define STEPPER_IDLE_LOCK_TIME 25 // (milliseconds) - Integer > 0
// The temporal resolution of the acceleration management subsystem. Higher number give smoother
@ -150,14 +153,15 @@
// of your successes or difficulties, as we will monitor this and possibly integrate this as a
// standard feature for future releases. However, we suggest to first try our direction delay
// hack/solution posted in the Wiki involving inverting the stepper pin mask.
// NOTE: Uncomment to enable. The recommended delay should be > 3us but not exceed a total
// time of 127us when added with the Grbl settings pulse microsecond.
// NOTE: Uncomment to enable. The recommended delay should be > 3us and the total step pulse
// time, which includes the Grbl settings pulse microseconds, should not exceed 127us.
// #define STEP_PULSE_DELAY 5 // Step pulse delay in microseconds. Default disabled.
// ---------------------------------------------------------------------------------------
// TODO: The following options are set as compile-time options for now, until the next EEPROM
// settings version has solidified.
// settings version has solidified. This is to prevent having to support dozens of different
// incremental settings versions.
#define CYCLE_AUTO_START 1 // Cycle auto-start boolean flag for the planner.
#define BLOCK_DELETE_ENABLE 0 // Block delete enable/disable flag during g-code parsing
#define REPORT_INCH_MODE 0 // Status reporting unit mode (1 = inch, 0 = mm)
@ -170,11 +174,8 @@
#endif
// Limit step rate for homing
#define LIMIT_STEP_RATE 1 // (mm/min)
// Debounce delay is the time delay the controller waits for a "good" signal from the limit switch.
// A delay of 3ms to 5ms is a good starting value.
#define LIMIT_DEBOUNCE_DELAY 5 // (milliseconds)
#define LIMIT_DEBOUNCE 50 // Limit switch debounce delay (in ms)
// #define LIMIT_INVERT_MASK 0 //
// #define LIMIT_NORMAL_HIGH 1 // Normal low 0 or normal high 1
#endif

8
gcode.c
View File

@ -114,6 +114,12 @@ void gc_set_current_position(int32_t x, int32_t y, int32_t z)
gc.position[Z_AXIS] = z/settings.steps_per_mm[Z_AXIS];
}
// Clears and zeros g-code parser position. Called by homing routine.
void gc_clear_position()
{
clear_vector(gc.position);
}
static float to_millimeters(double value)
{
return(gc.inches_mode ? (value * MM_PER_INCH) : value);
@ -336,7 +342,7 @@ uint8_t gc_execute_line(char *line)
mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], settings.default_seek_rate, false);
}
mc_go_home();
clear_vector(gc.position); // Assumes home is at [0,0,0]
// clear_vector(gc.position); // Assumes home is at [0,0,0]
axis_words = 0; // Axis words used. Lock out from motion modes by clearing flags.
break;
case NON_MODAL_SET_COORDINATE_OFFSET:

5
gcode.h
View File

@ -1,5 +1,5 @@
/*
gcode.c - rs274/ngc parser.
gcode.h - rs274/ngc parser.
Part of Grbl
Copyright (c) 2009-2011 Simen Svale Skogsrud
@ -33,4 +33,7 @@ uint8_t gc_execute_line(char *line);
// Set g-code parser position. Input in steps.
void gc_set_current_position(int32_t x, int32_t y, int32_t z);
// Clear g-code parser position
void gc_clear_position();
#endif

166
limits.c
View File

@ -3,6 +3,7 @@
Part of Grbl
Copyright (c) 2009-2011 Simen Svale Skogsrud
Copyright (c) 2012 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
@ -20,89 +21,164 @@
#include <util/delay.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include "stepper.h"
#include "settings.h"
#include "nuts_bolts.h"
#include "config.h"
#include "spindle_control.h"
#include "motion_control.h"
#include "planner.h"
#include "protocol.h"
// TODO: Deprecated. Need to update for new version. Sys.position now tracks position relative
// to the home position. Limits should update this vector directly.
#define MICROSECONDS_PER_ACCELERATION_TICK (1000000/ACCELERATION_TICKS_PER_SECOND)
void limits_init() {
void limits_init()
{
LIMIT_DDR &= ~(LIMIT_MASK);
}
static void homing_cycle(bool x_axis, bool y_axis, bool z_axis, bool reverse_direction, uint32_t microseconds_per_pulse) {
// First home the Z axis
uint32_t step_delay = microseconds_per_pulse - settings.pulse_microseconds;
uint8_t out_bits = DIRECTION_MASK;
uint8_t limit_bits;
// Moves all specified axes in same specified direction (positive=true, negative=false)
// and at the homing rate. Homing is a special motion case, where there is only an
// acceleration followed by abrupt asynchronous stops by each axes reaching their limit
// switch independently. Instead of showhorning homing cycles into the main stepper
// algorithm and overcomplicate things, a stripped-down, lite version of the stepper
// algorithm is written here. This also lets users hack and tune this code freely for
// their own particular needs without affecting the rest of Grbl.
// NOTE: Only the abort runtime command can interrupt this process.
static void homing_cycle(bool x_axis, bool y_axis, bool z_axis, int8_t pos_dir, double homing_rate)
{
// Determine governing axes with finest step resolution per distance for the Bresenham
// algorithm. This solves the issue when homing multiple axes that have different
// resolutions without exceeding system acceleration setting. It doesn't have to be
// perfect since homing locates machine zero, but should create for a more consistent
// and speedy homing routine.
// NOTE: For each axes enabled, the following calculations assume they physically move
// an equal distance over each time step until they hit a limit switch, aka dogleg.
uint32_t steps[3];
clear_vector(steps);
if (x_axis) { steps[X_AXIS] = lround(settings.steps_per_mm[X_AXIS]); }
if (y_axis) { steps[Y_AXIS] = lround(settings.steps_per_mm[Y_AXIS]); }
if (z_axis) { steps[Z_AXIS] = lround(settings.steps_per_mm[Z_AXIS]); }
uint32_t step_event_count = max(steps[X_AXIS], max(steps[Y_AXIS], steps[Z_AXIS]));
if (x_axis) { out_bits |= (1<<X_STEP_BIT); }
if (y_axis) { out_bits |= (1<<Y_STEP_BIT); }
if (z_axis) { out_bits |= (1<<Z_STEP_BIT); }
// To ensure global acceleration is not exceeded, reduce the governing axes nominal rate
// by adjusting the actual axes distance traveled per step. This is the same procedure
// used in the main planner to account for distance traveled when moving multiple axes.
// NOTE: When axis acceleration independence is installed, this will be updated to move
// all axes at their maximum acceleration and rate.
double ds = step_event_count/sqrt(x_axis+y_axis+z_axis);
// Invert direction bits if this is a reverse homing_cycle
if (reverse_direction) {
out_bits ^= DIRECTION_MASK;
}
// Compute the adjusted step rate change with each acceleration tick. (in step/min/acceleration_tick)
uint32_t delta_rate = ceil( ds*settings.acceleration/(60*ACCELERATION_TICKS_PER_SECOND));
// Apply the global invert mask
out_bits ^= settings.invert_mask;
// Nominal and initial time increment per step. Nominal should always be greater then 3
// usec, since they are based on the same parameters as the main stepper routine. Initial
// is based on the MINIMUM_STEPS_PER_MINUTE config.
uint32_t dt_min = lround(1000000*60/(ds*homing_rate)); // Cruising (usec/step)
uint32_t dt = 1000000*60/MINIMUM_STEPS_PER_MINUTE; // Initial (usec/step)
// Set direction pins
STEPPING_PORT = (STEPPING_PORT & ~DIRECTION_MASK) | (out_bits & DIRECTION_MASK);
// Determine default out_bits set. Direction fixed and step pin inverted
uint8_t out_bits0 = DIRECTION_MASK;
out_bits0 ^= settings.invert_mask; // Apply the global step and direction invert mask
if (!pos_dir) { out_bits0 ^= DIRECTION_MASK; } // Invert bits, if negative dir.
// Initialize stepping variables
int32_t counter_x = -(step_event_count >> 1); // Bresenham counters
int32_t counter_y = counter_x;
int32_t counter_z = counter_x;
uint32_t step_delay = dt-settings.pulse_microseconds; // Step delay after pulse
uint32_t step_rate = 0; // Tracks step rate. Initialized from 0 rate. (in step/min)
uint32_t trap_counter = MICROSECONDS_PER_ACCELERATION_TICK/2; // Acceleration trapezoid counter
uint8_t out_bits;
for(;;) {
limit_bits = LIMIT_PIN;
if (reverse_direction) {
// Invert limit_bits if this is a reverse homing_cycle
limit_bits ^= LIMIT_MASK;
// Reset out bits. Both direction and step pins appropriately inverted and set.
out_bits = out_bits0;
// Set step pins by Bresenham line algorithm. If limit switch reached, disable and
// flag for completion.
if (x_axis) {
counter_x += steps[X_AXIS];
if (counter_x > 0) {
if (LIMIT_PIN & (1<<X_LIMIT_BIT)) { out_bits ^= (1<<X_STEP_BIT); }
else { x_axis = false; }
counter_x -= step_event_count;
}
}
if (x_axis && !(LIMIT_PIN & (1<<X_LIMIT_BIT))) {
x_axis = false;
out_bits ^= (1<<X_STEP_BIT);
if (y_axis) {
counter_y += steps[Y_AXIS];
if (counter_y > 0) {
if (LIMIT_PIN & (1<<Y_LIMIT_BIT)) { out_bits ^= (1<<Y_STEP_BIT); }
else { y_axis = false; }
counter_y -= step_event_count;
}
}
if (y_axis && !(LIMIT_PIN & (1<<Y_LIMIT_BIT))) {
y_axis = false;
out_bits ^= (1<<Y_STEP_BIT);
if (z_axis) {
counter_z += steps[Z_AXIS];
if (counter_z > 0) {
if (LIMIT_PIN & (1<<Z_LIMIT_BIT)) { out_bits ^= (1<<Z_STEP_BIT); }
else { z_axis = false; }
counter_z -= step_event_count;
}
}
if (z_axis && !(LIMIT_PIN & (1<<Z_LIMIT_BIT))) {
z_axis = false;
out_bits ^= (1<<Z_STEP_BIT);
}
// Check if we are done
if(!(x_axis || y_axis || z_axis)) { return; }
STEPPING_PORT |= out_bits & STEP_MASK;
// Check if we are done or for system abort
protocol_execute_runtime();
if (!(x_axis || y_axis || z_axis) || sys.abort) { return; }
// Perform step.
STEPPING_PORT = (STEPPING_PORT & ~STEP_MASK) | (out_bits & STEP_MASK);
delay_us(settings.pulse_microseconds);
STEPPING_PORT ^= out_bits & STEP_MASK;
STEPPING_PORT = out_bits0;
delay_us(step_delay);
// Track and set the next step delay, if required. This routine uses another Bresenham
// line algorithm to follow the constant acceleration line in the velocity and time
// domain. This is a lite version of the same routine used in the main stepper program.
if (dt > dt_min) { // Unless cruising, check for time update.
trap_counter += dt; // Track time passed since last update.
if (trap_counter > MICROSECONDS_PER_ACCELERATION_TICK) {
trap_counter -= MICROSECONDS_PER_ACCELERATION_TICK;
step_rate += delta_rate; // Increment velocity
dt = (1000000*60)/step_rate; // Compute new time increment
if (dt < dt_min) {dt = dt_min;} // If target rate reached, cruise.
step_delay = dt-settings.pulse_microseconds;
}
}
}
return;
}
static void approach_limit_switch(bool x, bool y, bool z) {
homing_cycle(x, y, z, false, 100000);
static void approach_limit_switch(bool x, bool y, bool z)
{
homing_cycle(x, y, z, true, settings.default_seek_rate);
}
static void leave_limit_switch(bool x, bool y, bool z) {
homing_cycle(x, y, z, true, 500000);
homing_cycle(x, y, z, false, settings.default_feed_rate);
}
void limits_go_home() {
plan_synchronize();
// Store the current limit switch state
uint8_t original_limit_state = LIMIT_PIN;
void limits_go_home()
{
plan_synchronize(); // Empty all motions in buffer.
// TODO: Need to come up a better way to manage and set limit switches.
uint8_t original_limit_state = LIMIT_PIN; // Store the current limit switch state
// Jog all axes toward home to engage their limit switches.
approach_limit_switch(false, false, true); // First home the z axis
approach_limit_switch(true, true, false); // Then home the x and y axis
delay_ms(LIMIT_DEBOUNCE); // Delay to debounce signal before leaving limit switches
// Xor previous and current limit switch state to determine which were high then but have become
// low now. These are the actual installed limit switches.
uint8_t limit_switches_present = (original_limit_state ^ LIMIT_PIN) & LIMIT_MASK;
// Now carefully leave the limit switches
leave_limit_switch(
limit_switches_present & (1<<X_LIMIT_BIT),
limit_switches_present & (1<<Y_LIMIT_BIT),
limit_switches_present & (1<<Z_LIMIT_BIT));
delay_ms(LIMIT_DEBOUNCE); // Delay to debounce signal before leaving limit switches
}

8
motion_control.c
View File

@ -23,6 +23,7 @@
#include <avr/io.h>
#include "settings.h"
#include "config.h"
#include "gcode.h"
#include "motion_control.h"
#include <util/delay.h>
#include <math.h>
@ -192,9 +193,12 @@ void mc_dwell(double seconds)
}
// TODO: Update limits and homing cycle subprograms for better integration with new features.
// Execute homing cycle to locate and set machine zero.
void mc_go_home()
{
limits_go_home();
plan_set_current_position(0,0,0);
// Upon completion, reset all internal position vectors (g-code parser, planner, system)
gc_clear_position();
plan_clear_position();
clear_vector_double(sys.position);
}

19
nuts_bolts.c
View File

@ -46,8 +46,23 @@ void delay_ms(uint16_t ms)
}
// Delays variable defined microseconds. Compiler compatibility fix for _delay_us(),
// which only accepts constants in future compiler releases.
// which only accepts constants in future compiler releases. Written to perform more
// efficiently with larger delays, as the counter adds parasitic time in each iteration.
void delay_us(uint16_t us)
{
while ( us-- ) { _delay_us(1); }
while (us) {
if (us < 10) {
_delay_us(1);
us--;
} else if (us < 100) {
_delay_us(10);
us -= 10;
} else if (us < 1000) {
_delay_us(100);
us -= 100;
} else {
_delay_ms(1);
us -= 1000;
}
}
}

6
planner.c
View File

@ -481,6 +481,12 @@ void plan_set_current_position(int32_t x, int32_t y, int32_t z)
pl.position[Z_AXIS] = z;
}
// Clear planner position vector. Called by homing routine.
void plan_clear_position()
{
clear_vector(pl.position);
}
// Re-initialize buffer plan with a partially completed block, assumed to exist at the buffer tail.
// Called after a steppers have come to a complete stop for a feed hold and the cycle is stopped.
void plan_cycle_reinitialize(int32_t step_events_remaining)

3
planner.h
View File

@ -69,6 +69,9 @@ block_t *plan_get_current_block();
// Reset the planner position vector (in steps)
void plan_set_current_position(int32_t x, int32_t y, int32_t z);
// Clear the planner position vector
void plan_clear_position();
// Reinitialize plan with a partially completed block
void plan_cycle_reinitialize(int32_t step_events_remaining);

10
serial.c
View File

@ -42,7 +42,7 @@ uint8_t tx_buffer[TX_BUFFER_SIZE];
uint8_t tx_buffer_head = 0;
volatile uint8_t tx_buffer_tail = 0;
#if ENABLE_XONXOFF
#ifdef ENABLE_XONXOFF
#define RX_BUFFER_FULL 96 // XOFF high watermark
#define RX_BUFFER_LOW 64 // XON low watermark
#define SEND_XOFF 1
@ -110,7 +110,7 @@ ISR(USART_UDRE_vect)
// Temporary tx_buffer_tail (to optimize for volatile)
uint8_t tail = tx_buffer_tail;
#if ENABLE_XONXOFF
#ifdef ENABLE_XONXOFF
if (flow_ctrl == SEND_XOFF) {
UDR0 = XOFF_CHAR;
flow_ctrl = XOFF_SENT;
@ -143,7 +143,7 @@ uint8_t serial_read()
rx_buffer_tail++;
if (rx_buffer_tail == RX_BUFFER_SIZE) { rx_buffer_tail = 0; }
#if ENABLE_XONXOFF
#ifdef ENABLE_XONXOFF
if ((get_rx_buffer_count() < RX_BUFFER_LOW) && flow_ctrl == XOFF_SENT) {
flow_ctrl = SEND_XON;
UCSR0B |= (1 << UDRIE0); // Force TX
@ -182,7 +182,7 @@ ISR(USART_RX_vect)
rx_buffer[rx_buffer_head] = data;
rx_buffer_head = next_head;
#if ENABLE_XONXOFF
#ifdef ENABLE_XONXOFF
if ((get_rx_buffer_count() >= RX_BUFFER_FULL) && flow_ctrl == XON_SENT) {
flow_ctrl = SEND_XOFF;
UCSR0B |= (1 << UDRIE0); // Force TX
@ -197,7 +197,7 @@ void serial_reset_read_buffer()
{
rx_buffer_tail = rx_buffer_head;
#if ENABLE_XONXOFF
#ifdef ENABLE_XONXOFF
flow_ctrl = XON_SENT;
#endif
}

5
stepper.c
View File

@ -31,7 +31,6 @@
#include "nuts_bolts.h"
#include <avr/interrupt.h>
#include "planner.h"
#include "limits.h"
// Some useful constants
#define TICKS_PER_MICROSECOND (F_CPU/1000000)
@ -299,7 +298,7 @@ ISR(TIMER1_COMPA_vect)
plan_discard_current_block();
}
}
out_bits ^= settings.invert_mask; // Apply stepper invert mask
out_bits ^= settings.invert_mask; // Apply step and direction invert mask
busy = false;
}
@ -396,7 +395,7 @@ static uint32_t config_step_timer(uint32_t cycles)
} else {
// Okay, that was slower than we actually go. Just set the slowest speed
ceiling = 0xffff;
prescaler = 6;
prescaler = 5;
actual_cycles = 0xffff * 1024;
}
// Set prescaler