From d30cb906f886e71987ce986effbd1c9368aca479 Mon Sep 17 00:00:00 2001 From: Sonny Jeon Date: Sun, 30 Sep 2012 19:57:10 -0600 Subject: [PATCH] Updated limit/homing routine. Works, but needs more TLC. - 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. --- config.h | 51 +++++++------- gcode.c | 8 ++- gcode.h | 5 +- limits.c | 172 ++++++++++++++++++++++++++++++++++------------- motion_control.c | 10 ++- nuts_bolts.c | 19 +++++- planner.c | 6 ++ planner.h | 3 + serial.c | 10 +-- stepper.c | 5 +- 10 files changed, 201 insertions(+), 88 deletions(-) diff --git a/config.h b/config.h index 910b490..2ac197f 100755 --- a/config.h +++ b/config.h @@ -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 diff --git a/gcode.c b/gcode.c index f9d46fc..171eef8 100755 --- a/gcode.c +++ b/gcode.c @@ -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: diff --git a/gcode.h b/gcode.h index f3d86f1..c4325d1 100755 --- a/gcode.h +++ b/gcode.h @@ -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 diff --git a/limits.c b/limits.c index 64ff53f..5e8bfa1 100755 --- a/limits.c +++ b/limits.c @@ -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 #include +#include #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<> 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< 0) { + if (LIMIT_PIN & (1< 0) { + if (LIMIT_PIN & (1< 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< #include "settings.h" #include "config.h" +#include "gcode.h" #include "motion_control.h" #include #include @@ -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); + limits_go_home(); + // Upon completion, reset all internal position vectors (g-code parser, planner, system) + gc_clear_position(); + plan_clear_position(); + clear_vector_double(sys.position); } diff --git a/nuts_bolts.c b/nuts_bolts.c index 79cc41d..2e2bfe4 100755 --- a/nuts_bolts.c +++ b/nuts_bolts.c @@ -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; + } + } } diff --git a/planner.c b/planner.c index dac2d62..750af75 100755 --- a/planner.c +++ b/planner.c @@ -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) diff --git a/planner.h b/planner.h index 3f1ba01..a1adc26 100755 --- a/planner.h +++ b/planner.h @@ -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); diff --git a/serial.c b/serial.c index 44907c5..aa35237 100755 --- a/serial.c +++ b/serial.c @@ -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 } diff --git a/stepper.c b/stepper.c index e4534ed..912419b 100755 --- a/stepper.c +++ b/stepper.c @@ -31,7 +31,6 @@ #include "nuts_bolts.h" #include #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