From 4e5becfc51492599d191e23b384c17dc675d6c57 Mon Sep 17 00:00:00 2001 From: Erik van der Zalm Date: Sun, 20 Nov 2011 14:50:08 +0100 Subject: [PATCH] Added endstop reporting --- Marlin/Marlin.pde | 24 ++++--- Marlin/planner.cpp | 1 + Marlin/stepper.cpp | 159 ++++++++++++++++++++++----------------------- Marlin/stepper.h | 100 ++++++++++++++-------------- 4 files changed, 138 insertions(+), 146 deletions(-) diff --git a/Marlin/Marlin.pde b/Marlin/Marlin.pde index 754dc3618..8ee165693 100644 --- a/Marlin/Marlin.pde +++ b/Marlin/Marlin.pde @@ -465,24 +465,22 @@ inline bool code_seen(char code) destination[LETTER##_AXIS] = 1.5 * LETTER##_MAX_LENGTH * LETTER##_HOME_DIR; \ feedrate = homing_feedrate[LETTER##_AXIS]; \ prepare_move(); \ - st_synchronize();\ \ current_position[LETTER##_AXIS] = 0;\ plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);\ destination[LETTER##_AXIS] = -5 * LETTER##_HOME_DIR;\ prepare_move(); \ - st_synchronize();\ \ destination[LETTER##_AXIS] = 10 * LETTER##_HOME_DIR;\ feedrate = homing_feedrate[LETTER##_AXIS]/2 ; \ prepare_move(); \ - st_synchronize();\ \ current_position[LETTER##_AXIS] = (LETTER##_HOME_DIR == -1) ? 0 : LETTER##_MAX_LENGTH;\ plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);\ destination[LETTER##_AXIS] = current_position[LETTER##_AXIS];\ feedrate = 0.0;\ st_synchronize();\ + plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);\ endstops_hit_on_purpose();\ } @@ -680,7 +678,7 @@ inline void process_commands() case 140: // M140 set bed temp if (code_seen('S')) setTargetBed(code_value()); break; - case 105: // M105 + case 105 : // M105 //SERIAL_ECHOLN(freeMemory()); //test watchdog: //delay(20000); @@ -817,7 +815,7 @@ inline void process_commands() axis_relative_modes[3] = true; break; case 18: //compatibility - case 84: + case 84: // M84 if(code_seen('S')){ stepper_inactive_time = code_value() * 1000; } @@ -854,14 +852,14 @@ inline void process_commands() SERIAL_PROTOCOL(current_position[Z_AXIS]); SERIAL_PROTOCOLPGM("E:"); SERIAL_PROTOCOL(current_position[E_AXIS]); - #ifdef DEBUG_STEPS - SERIAL_PROTOCOLPGM(" Count X:"); - SERIAL_PROTOCOL(float(count_position[X_AXIS])/axis_steps_per_unit[X_AXIS]); - SERIAL_PROTOCOLPGM("Y:"); - SERIAL_PROTOCOL(float(count_position[Y_AXIS])/axis_steps_per_unit[Y_AXIS]); - SERIAL_PROTOCOLPGM("Z:"); - SERIAL_PROTOCOL(float(count_position[Z_AXIS])/axis_steps_per_unit[Z_AXIS]); - #endif + + SERIAL_PROTOCOLPGM(" Count X:"); + SERIAL_PROTOCOL(float(st_get_position(X_AXIS))/axis_steps_per_unit[X_AXIS]); + SERIAL_PROTOCOLPGM("Y:"); + SERIAL_PROTOCOL(float(st_get_position(Y_AXIS))/axis_steps_per_unit[Y_AXIS]); + SERIAL_PROTOCOLPGM("Z:"); + SERIAL_PROTOCOL(float(st_get_position(Z_AXIS))/axis_steps_per_unit[Z_AXIS]); + SERIAL_PROTOCOLLN(""); break; case 119: // M119 diff --git a/Marlin/planner.cpp b/Marlin/planner.cpp index b7f98b6d1..a59945da8 100644 --- a/Marlin/planner.cpp +++ b/Marlin/planner.cpp @@ -754,6 +754,7 @@ void plan_set_position(const float &x, const float &y, const float &z, const flo position[Y_AXIS] = lround(y*axis_steps_per_unit[Y_AXIS]); position[Z_AXIS] = lround(z*axis_steps_per_unit[Z_AXIS]); position[E_AXIS] = lround(e*axis_steps_per_unit[E_AXIS]); + st_set_position(position[X_AXIS], position[Y_AXIS], position[Z_AXIS], position[E_AXIS]); previous_nominal_speed = 0.0; // Resets planner junction speeds. Assumes start from rest. previous_speed[0] = 0.0; previous_speed[1] = 0.0; diff --git a/Marlin/stepper.cpp b/Marlin/stepper.cpp index b08e61ae4..771bccfd4 100644 --- a/Marlin/stepper.cpp +++ b/Marlin/stepper.cpp @@ -66,14 +66,12 @@ static char step_loops; volatile long endstops_trigsteps[3]={0,0,0}; volatile long endstops_stepsTotal,endstops_stepsDone; -static volatile bool endstops_hit=false; +static volatile bool endstop_x_hit=false; +static volatile bool endstop_y_hit=false; +static volatile bool endstop_z_hit=false; -// if DEBUG_STEPS is enabled, M114 can be used to compare two methods of determining the X,Y,Z position of the printer. -// for debugging purposes only, should be disabled by default -#ifdef DEBUG_STEPS - volatile long count_position[NUM_AXIS] = { 0, 0, 0, 0}; - volatile int count_direction[NUM_AXIS] = { 1, 1, 1, 1}; -#endif +volatile long count_position[NUM_AXIS] = { 0, 0, 0, 0}; +volatile char count_direction[NUM_AXIS] = { 1, 1, 1, 1}; //=========================================================================== //=============================functions ============================ @@ -155,49 +153,32 @@ asm volatile ( \ #define ENABLE_STEPPER_DRIVER_INTERRUPT() TIMSK1 |= (1<step_event_count; - endstops_stepsDone=stepstaken; - endstops_trigsteps[0]=current_block->steps_x; - endstops_trigsteps[1]=current_block->steps_y; - endstops_trigsteps[2]=current_block->steps_z; - - endstops_hit=true; -} - void checkHitEndstops() { - if( !endstops_hit) - return; - float endstops_triggerpos[3]={0,0,0}; - float ratiodone=endstops_stepsDone/float(endstops_stepsTotal); //ratio of current_block thas was performed - - endstops_triggerpos[0]=current_position[0]-(endstops_trigsteps[0]*ratiodone)/float(axis_steps_per_unit[0]); - endstops_triggerpos[1]=current_position[1]-(endstops_trigsteps[1]*ratiodone)/float(axis_steps_per_unit[1]); - endstops_triggerpos[2]=current_position[2]-(endstops_trigsteps[2]*ratiodone)/float(axis_steps_per_unit[2]); - SERIAL_ECHO_START; - SERIAL_ECHOPGM("endstops hit: "); - SERIAL_ECHOPAIR(" X:",endstops_triggerpos[0]); - SERIAL_ECHOPAIR(" Y:",endstops_triggerpos[1]); - SERIAL_ECHOPAIR(" Z:",endstops_triggerpos[2]); - SERIAL_ECHOLN(""); - endstops_hit=false; + if( endstop_x_hit || endstop_y_hit || endstop_z_hit) { + SERIAL_ECHO_START; + SERIAL_ECHOPGM("endstops hit: "); + if(endstop_x_hit) { + SERIAL_ECHOPAIR(" X:",(float)endstops_trigsteps[X_AXIS]/axis_steps_per_unit[X_AXIS]); + } + if(endstop_y_hit) { + SERIAL_ECHOPAIR(" Y:",(float)endstops_trigsteps[Y_AXIS]/axis_steps_per_unit[Y_AXIS]); + } + if(endstop_z_hit) { + SERIAL_ECHOPAIR(" Z:",(float)endstops_trigsteps[Z_AXIS]/axis_steps_per_unit[Z_AXIS]); + } + SERIAL_ECHOLN(""); + endstop_x_hit=false; + endstop_y_hit=false; + endstop_z_hit=false; + } } void endstops_hit_on_purpose() { - endstops_hit=false; + endstop_x_hit=false; + endstop_y_hit=false; + endstop_z_hit=false; } // __________________________ @@ -312,24 +293,22 @@ ISR(TIMER1_COMPA_vect) // Set direction en check limit switches if ((out_bits & (1< -1 - if(READ(X_MIN_PIN) != ENDSTOPS_INVERTING) { - // endstops_triggered(step_events_completed); + if((READ(X_MIN_PIN) != ENDSTOPS_INVERTING) && (current_block->steps_x > 0)) { + endstops_trigsteps[X_AXIS] = count_position[X_AXIS]; + endstop_x_hit=true; step_events_completed = current_block->step_event_count; } #endif } else { // +direction WRITE(X_DIR_PIN,!INVERT_X_DIR); - #ifdef DEBUG_STEPS - count_direction[X_AXIS]=1; - #endif + count_direction[X_AXIS]=1; #if X_MAX_PIN > -1 - if((READ(X_MAX_PIN) != ENDSTOPS_INVERTING) && (current_block->steps_x >0)){ - // endstops_triggered(step_events_completed); + if((READ(X_MAX_PIN) != ENDSTOPS_INVERTING) && (current_block->steps_x > 0)){ + endstops_trigsteps[X_AXIS] = count_position[X_AXIS]; + endstop_x_hit=true; step_events_completed = current_block->step_event_count; } #endif @@ -337,24 +316,22 @@ ISR(TIMER1_COMPA_vect) if ((out_bits & (1< -1 - if(READ(Y_MIN_PIN) != ENDSTOPS_INVERTING) { -// endstops_triggered(step_events_completed); + if((READ(Y_MIN_PIN) != ENDSTOPS_INVERTING) && (current_block->steps_y > 0)) { + endstops_trigsteps[Y_AXIS] = count_position[Y_AXIS]; + endstop_y_hit=true; step_events_completed = current_block->step_event_count; } #endif } else { // +direction WRITE(Y_DIR_PIN,!INVERT_Y_DIR); - #ifdef DEBUG_STEPS - count_direction[Y_AXIS]=1; - #endif + count_direction[Y_AXIS]=1; #if Y_MAX_PIN > -1 - if((READ(Y_MAX_PIN) != ENDSTOPS_INVERTING) && (current_block->steps_y >0)){ - // endstops_triggered(step_events_completed); + if((READ(Y_MAX_PIN) != ENDSTOPS_INVERTING) && (current_block->steps_y > 0)){ + endstops_trigsteps[Y_AXIS] = count_position[Y_AXIS]; + endstop_y_hit=true; step_events_completed = current_block->step_event_count; } #endif @@ -362,34 +339,36 @@ ISR(TIMER1_COMPA_vect) if ((out_bits & (1< -1 - if(READ(Z_MIN_PIN) != ENDSTOPS_INVERTING) { - // endstops_triggered(step_events_completed); + if((READ(Z_MIN_PIN) != ENDSTOPS_INVERTING) && (current_block->steps_z > 0)) { + endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS]; + endstop_z_hit=true; step_events_completed = current_block->step_event_count; } #endif } else { // +direction WRITE(Z_DIR_PIN,!INVERT_Z_DIR); - #ifdef DEBUG_STEPS count_direction[Z_AXIS]=1; - #endif #if Z_MAX_PIN > -1 - if((READ(Z_MAX_PIN) != ENDSTOPS_INVERTING) && (current_block->steps_z >0)){ - // endstops_triggered(step_events_completed); + if((READ(Z_MAX_PIN) != ENDSTOPS_INVERTING) && (current_block->steps_z > 0)){ + endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS]; + endstop_z_hit=true; step_events_completed = current_block->step_event_count; } #endif } #ifndef ADVANCE - if ((out_bits & (1<step_event_count; WRITE(X_STEP_PIN, LOW); - #ifdef DEBUG_STEPS - count_position[X_AXIS]+=count_direction[X_AXIS]; - #endif + count_position[X_AXIS]+=count_direction[X_AXIS]; } counter_y += current_block->steps_y; @@ -432,9 +409,7 @@ ISR(TIMER1_COMPA_vect) WRITE(Y_STEP_PIN, HIGH); counter_y -= current_block->step_event_count; WRITE(Y_STEP_PIN, LOW); - #ifdef DEBUG_STEPS - count_position[Y_AXIS]+=count_direction[Y_AXIS]; - #endif + count_position[Y_AXIS]+=count_direction[Y_AXIS]; } counter_z += current_block->steps_z; @@ -442,9 +417,7 @@ ISR(TIMER1_COMPA_vect) WRITE(Z_STEP_PIN, HIGH); counter_z -= current_block->step_event_count; WRITE(Z_STEP_PIN, LOW); - #ifdef DEBUG_STEPS - count_position[Z_AXIS]+=count_direction[Z_AXIS]; - #endif + count_position[Z_AXIS]+=count_direction[Z_AXIS]; } #ifndef ADVANCE @@ -453,6 +426,7 @@ ISR(TIMER1_COMPA_vect) WRITE(E_STEP_PIN, HIGH); counter_e -= current_block->step_event_count; WRITE(E_STEP_PIN, LOW); + count_position[E_AXIS]+=count_direction[E_AXIS]; } #endif //!ADVANCE step_events_completed += 1; @@ -669,3 +643,22 @@ void st_synchronize() LCD_STATUS; } } + +void st_set_position(const long &x, const long &y, const long &z, const long &e) +{ + CRITICAL_SECTION_START; + count_position[X_AXIS] = x; + count_position[Y_AXIS] = y; + count_position[Z_AXIS] = z; + count_position[E_AXIS] = e; + CRITICAL_SECTION_END; +} + +long st_get_position(char axis) +{ + long count_pos; + CRITICAL_SECTION_START; + count_pos = count_position[axis]; + CRITICAL_SECTION_END; + return count_pos; +} diff --git a/Marlin/stepper.h b/Marlin/stepper.h index ecbc713e3..eb07fa9ce 100644 --- a/Marlin/stepper.h +++ b/Marlin/stepper.h @@ -1,51 +1,51 @@ -/* - stepper.h - stepper motor driver: executes motion plans of planner.c using the stepper motors - Part of Grbl - - Copyright (c) 2009-2011 Simen Svale Skogsrud - - Grbl is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - Grbl is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with Grbl. If not, see . -*/ - -#ifndef stepper_h -#define stepper_h - -#include "planner.h" - -// Initialize and start the stepper motor subsystem -void st_init(); - -// Block until all buffered steps are executed -void st_synchronize(); - -// The stepper subsystem goes to sleep when it runs out of things to execute. Call this -// to notify the subsystem that it is time to go to work. -void st_wake_up(); - -// if DEBUG_STEPS is enabled, M114 can be used to compare two methods of determining the X,Y,Z position of the printer. -// for debugging purposes only, should be disabled by default -#ifdef DEBUG_STEPS - extern volatile long count_position[NUM_AXIS]; - extern volatile int count_direction[NUM_AXIS]; -#endif - -void checkHitEndstops(); //call from somwhere to create an serial error message with the locations the endstops where hit, in case they were triggered -void endstops_hit_on_purpose(); //avoid creation of the message, i.e. after homeing and before a routine call of checkHitEndstops(); - - - -extern block_t *current_block; // A pointer to the block currently being traced - - +/* + stepper.h - stepper motor driver: executes motion plans of planner.c using the stepper motors + Part of Grbl + + Copyright (c) 2009-2011 Simen Svale Skogsrud + + Grbl is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + Grbl is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with Grbl. If not, see . +*/ + +#ifndef stepper_h +#define stepper_h + +#include "planner.h" + +// Initialize and start the stepper motor subsystem +void st_init(); + +// Block until all buffered steps are executed +void st_synchronize(); + +// Set current position in steps +void st_set_position(const long &x, const long &y, const long &z, const long &e); + +// Get current position in steps +long st_get_position(char axis); + +// The stepper subsystem goes to sleep when it runs out of things to execute. Call this +// to notify the subsystem that it is time to go to work. +void st_wake_up(); + + +void checkHitEndstops(); //call from somwhere to create an serial error message with the locations the endstops where hit, in case they were triggered +void endstops_hit_on_purpose(); //avoid creation of the message, i.e. after homeing and before a routine call of checkHitEndstops(); + + + +extern block_t *current_block; // A pointer to the block currently being traced + + #endif