Added endstop reporting

This commit is contained in:
Erik van der Zalm 2011-11-20 14:50:08 +01:00
parent 85c8a87e70
commit 4e5becfc51
4 changed files with 138 additions and 146 deletions

View File

@ -465,24 +465,22 @@ inline bool code_seen(char code)
destination[LETTER##_AXIS] = 1.5 * LETTER##_MAX_LENGTH * LETTER##_HOME_DIR; \ destination[LETTER##_AXIS] = 1.5 * LETTER##_MAX_LENGTH * LETTER##_HOME_DIR; \
feedrate = homing_feedrate[LETTER##_AXIS]; \ feedrate = homing_feedrate[LETTER##_AXIS]; \
prepare_move(); \ prepare_move(); \
st_synchronize();\
\ \
current_position[LETTER##_AXIS] = 0;\ current_position[LETTER##_AXIS] = 0;\
plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);\ 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;\ destination[LETTER##_AXIS] = -5 * LETTER##_HOME_DIR;\
prepare_move(); \ prepare_move(); \
st_synchronize();\
\ \
destination[LETTER##_AXIS] = 10 * LETTER##_HOME_DIR;\ destination[LETTER##_AXIS] = 10 * LETTER##_HOME_DIR;\
feedrate = homing_feedrate[LETTER##_AXIS]/2 ; \ feedrate = homing_feedrate[LETTER##_AXIS]/2 ; \
prepare_move(); \ prepare_move(); \
st_synchronize();\
\ \
current_position[LETTER##_AXIS] = (LETTER##_HOME_DIR == -1) ? 0 : LETTER##_MAX_LENGTH;\ 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]);\ 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];\ destination[LETTER##_AXIS] = current_position[LETTER##_AXIS];\
feedrate = 0.0;\ feedrate = 0.0;\
st_synchronize();\ 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();\ endstops_hit_on_purpose();\
} }
@ -680,7 +678,7 @@ inline void process_commands()
case 140: // M140 set bed temp case 140: // M140 set bed temp
if (code_seen('S')) setTargetBed(code_value()); if (code_seen('S')) setTargetBed(code_value());
break; break;
case 105: // M105 case 105 : // M105
//SERIAL_ECHOLN(freeMemory()); //SERIAL_ECHOLN(freeMemory());
//test watchdog: //test watchdog:
//delay(20000); //delay(20000);
@ -817,7 +815,7 @@ inline void process_commands()
axis_relative_modes[3] = true; axis_relative_modes[3] = true;
break; break;
case 18: //compatibility case 18: //compatibility
case 84: case 84: // M84
if(code_seen('S')){ if(code_seen('S')){
stepper_inactive_time = code_value() * 1000; stepper_inactive_time = code_value() * 1000;
} }
@ -854,14 +852,14 @@ inline void process_commands()
SERIAL_PROTOCOL(current_position[Z_AXIS]); SERIAL_PROTOCOL(current_position[Z_AXIS]);
SERIAL_PROTOCOLPGM("E:"); SERIAL_PROTOCOLPGM("E:");
SERIAL_PROTOCOL(current_position[E_AXIS]); SERIAL_PROTOCOL(current_position[E_AXIS]);
#ifdef DEBUG_STEPS
SERIAL_PROTOCOLPGM(" Count X:"); SERIAL_PROTOCOLPGM(" Count X:");
SERIAL_PROTOCOL(float(count_position[X_AXIS])/axis_steps_per_unit[X_AXIS]); SERIAL_PROTOCOL(float(st_get_position(X_AXIS))/axis_steps_per_unit[X_AXIS]);
SERIAL_PROTOCOLPGM("Y:"); SERIAL_PROTOCOLPGM("Y:");
SERIAL_PROTOCOL(float(count_position[Y_AXIS])/axis_steps_per_unit[Y_AXIS]); SERIAL_PROTOCOL(float(st_get_position(Y_AXIS))/axis_steps_per_unit[Y_AXIS]);
SERIAL_PROTOCOLPGM("Z:"); SERIAL_PROTOCOLPGM("Z:");
SERIAL_PROTOCOL(float(count_position[Z_AXIS])/axis_steps_per_unit[Z_AXIS]); SERIAL_PROTOCOL(float(st_get_position(Z_AXIS))/axis_steps_per_unit[Z_AXIS]);
#endif
SERIAL_PROTOCOLLN(""); SERIAL_PROTOCOLLN("");
break; break;
case 119: // M119 case 119: // M119

View File

@ -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[Y_AXIS] = lround(y*axis_steps_per_unit[Y_AXIS]);
position[Z_AXIS] = lround(z*axis_steps_per_unit[Z_AXIS]); position[Z_AXIS] = lround(z*axis_steps_per_unit[Z_AXIS]);
position[E_AXIS] = lround(e*axis_steps_per_unit[E_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_nominal_speed = 0.0; // Resets planner junction speeds. Assumes start from rest.
previous_speed[0] = 0.0; previous_speed[0] = 0.0;
previous_speed[1] = 0.0; previous_speed[1] = 0.0;

View File

@ -66,14 +66,12 @@ static char step_loops;
volatile long endstops_trigsteps[3]={0,0,0}; volatile long endstops_trigsteps[3]={0,0,0};
volatile long endstops_stepsTotal,endstops_stepsDone; 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. volatile long count_position[NUM_AXIS] = { 0, 0, 0, 0};
// for debugging purposes only, should be disabled by default volatile char count_direction[NUM_AXIS] = { 1, 1, 1, 1};
#ifdef DEBUG_STEPS
volatile long count_position[NUM_AXIS] = { 0, 0, 0, 0};
volatile int count_direction[NUM_AXIS] = { 1, 1, 1, 1};
#endif
//=========================================================================== //===========================================================================
//=============================functions ============================ //=============================functions ============================
@ -155,49 +153,32 @@ asm volatile ( \
#define ENABLE_STEPPER_DRIVER_INTERRUPT() TIMSK1 |= (1<<OCIE1A) #define ENABLE_STEPPER_DRIVER_INTERRUPT() TIMSK1 |= (1<<OCIE1A)
#define DISABLE_STEPPER_DRIVER_INTERRUPT() TIMSK1 &= ~(1<<OCIE1A) #define DISABLE_STEPPER_DRIVER_INTERRUPT() TIMSK1 &= ~(1<<OCIE1A)
inline void endstops_triggered(const unsigned long &stepstaken)
{
//this will only work if there is no bufferig
//however, if you perform a move at which the endstops should be triggered, and wait for it to complete, i.e. by blocking command, it should work
//yes, it uses floats, but: if endstops are triggered, thats hopefully not critical anymore anyways.
//endstops_triggerpos;
if(endstops_hit) //hitting a second time while the first hit is not reported
return;
if(current_block == NULL)
return;
endstops_stepsTotal=current_block->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() void checkHitEndstops()
{ {
if( !endstops_hit) if( endstop_x_hit || endstop_y_hit || endstop_z_hit) {
return; SERIAL_ECHO_START;
float endstops_triggerpos[3]={0,0,0}; SERIAL_ECHOPGM("endstops hit: ");
float ratiodone=endstops_stepsDone/float(endstops_stepsTotal); //ratio of current_block thas was performed if(endstop_x_hit) {
SERIAL_ECHOPAIR(" X:",(float)endstops_trigsteps[X_AXIS]/axis_steps_per_unit[X_AXIS]);
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]); if(endstop_y_hit) {
endstops_triggerpos[2]=current_position[2]-(endstops_trigsteps[2]*ratiodone)/float(axis_steps_per_unit[2]); SERIAL_ECHOPAIR(" Y:",(float)endstops_trigsteps[Y_AXIS]/axis_steps_per_unit[Y_AXIS]);
SERIAL_ECHO_START; }
SERIAL_ECHOPGM("endstops hit: "); if(endstop_z_hit) {
SERIAL_ECHOPAIR(" X:",endstops_triggerpos[0]); SERIAL_ECHOPAIR(" Z:",(float)endstops_trigsteps[Z_AXIS]/axis_steps_per_unit[Z_AXIS]);
SERIAL_ECHOPAIR(" Y:",endstops_triggerpos[1]); }
SERIAL_ECHOPAIR(" Z:",endstops_triggerpos[2]); SERIAL_ECHOLN("");
SERIAL_ECHOLN(""); endstop_x_hit=false;
endstops_hit=false; endstop_y_hit=false;
endstop_z_hit=false;
}
} }
void endstops_hit_on_purpose() 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 // Set direction en check limit switches
if ((out_bits & (1<<X_AXIS)) != 0) { // -direction if ((out_bits & (1<<X_AXIS)) != 0) { // -direction
WRITE(X_DIR_PIN, INVERT_X_DIR); WRITE(X_DIR_PIN, INVERT_X_DIR);
#ifdef DEBUG_STEPS count_direction[X_AXIS]=-1;
count_direction[X_AXIS]=-1;
#endif
#if X_MIN_PIN > -1 #if X_MIN_PIN > -1
if(READ(X_MIN_PIN) != ENDSTOPS_INVERTING) { if((READ(X_MIN_PIN) != ENDSTOPS_INVERTING) && (current_block->steps_x > 0)) {
// endstops_triggered(step_events_completed); endstops_trigsteps[X_AXIS] = count_position[X_AXIS];
endstop_x_hit=true;
step_events_completed = current_block->step_event_count; step_events_completed = current_block->step_event_count;
} }
#endif #endif
} }
else { // +direction else { // +direction
WRITE(X_DIR_PIN,!INVERT_X_DIR); WRITE(X_DIR_PIN,!INVERT_X_DIR);
#ifdef DEBUG_STEPS count_direction[X_AXIS]=1;
count_direction[X_AXIS]=1;
#endif
#if X_MAX_PIN > -1 #if X_MAX_PIN > -1
if((READ(X_MAX_PIN) != ENDSTOPS_INVERTING) && (current_block->steps_x >0)){ if((READ(X_MAX_PIN) != ENDSTOPS_INVERTING) && (current_block->steps_x > 0)){
// endstops_triggered(step_events_completed); endstops_trigsteps[X_AXIS] = count_position[X_AXIS];
endstop_x_hit=true;
step_events_completed = current_block->step_event_count; step_events_completed = current_block->step_event_count;
} }
#endif #endif
@ -337,24 +316,22 @@ ISR(TIMER1_COMPA_vect)
if ((out_bits & (1<<Y_AXIS)) != 0) { // -direction if ((out_bits & (1<<Y_AXIS)) != 0) { // -direction
WRITE(Y_DIR_PIN,INVERT_Y_DIR); WRITE(Y_DIR_PIN,INVERT_Y_DIR);
#ifdef DEBUG_STEPS count_direction[Y_AXIS]=-1;
count_direction[Y_AXIS]=-1;
#endif
#if Y_MIN_PIN > -1 #if Y_MIN_PIN > -1
if(READ(Y_MIN_PIN) != ENDSTOPS_INVERTING) { if((READ(Y_MIN_PIN) != ENDSTOPS_INVERTING) && (current_block->steps_y > 0)) {
// endstops_triggered(step_events_completed); endstops_trigsteps[Y_AXIS] = count_position[Y_AXIS];
endstop_y_hit=true;
step_events_completed = current_block->step_event_count; step_events_completed = current_block->step_event_count;
} }
#endif #endif
} }
else { // +direction else { // +direction
WRITE(Y_DIR_PIN,!INVERT_Y_DIR); WRITE(Y_DIR_PIN,!INVERT_Y_DIR);
#ifdef DEBUG_STEPS count_direction[Y_AXIS]=1;
count_direction[Y_AXIS]=1;
#endif
#if Y_MAX_PIN > -1 #if Y_MAX_PIN > -1
if((READ(Y_MAX_PIN) != ENDSTOPS_INVERTING) && (current_block->steps_y >0)){ if((READ(Y_MAX_PIN) != ENDSTOPS_INVERTING) && (current_block->steps_y > 0)){
// endstops_triggered(step_events_completed); endstops_trigsteps[Y_AXIS] = count_position[Y_AXIS];
endstop_y_hit=true;
step_events_completed = current_block->step_event_count; step_events_completed = current_block->step_event_count;
} }
#endif #endif
@ -362,34 +339,36 @@ ISR(TIMER1_COMPA_vect)
if ((out_bits & (1<<Z_AXIS)) != 0) { // -direction if ((out_bits & (1<<Z_AXIS)) != 0) { // -direction
WRITE(Z_DIR_PIN,INVERT_Z_DIR); WRITE(Z_DIR_PIN,INVERT_Z_DIR);
#ifdef DEBUG_STEPS
count_direction[Z_AXIS]=-1; count_direction[Z_AXIS]=-1;
#endif
#if Z_MIN_PIN > -1 #if Z_MIN_PIN > -1
if(READ(Z_MIN_PIN) != ENDSTOPS_INVERTING) { if((READ(Z_MIN_PIN) != ENDSTOPS_INVERTING) && (current_block->steps_z > 0)) {
// endstops_triggered(step_events_completed); endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS];
endstop_z_hit=true;
step_events_completed = current_block->step_event_count; step_events_completed = current_block->step_event_count;
} }
#endif #endif
} }
else { // +direction else { // +direction
WRITE(Z_DIR_PIN,!INVERT_Z_DIR); WRITE(Z_DIR_PIN,!INVERT_Z_DIR);
#ifdef DEBUG_STEPS
count_direction[Z_AXIS]=1; count_direction[Z_AXIS]=1;
#endif
#if Z_MAX_PIN > -1 #if Z_MAX_PIN > -1
if((READ(Z_MAX_PIN) != ENDSTOPS_INVERTING) && (current_block->steps_z >0)){ if((READ(Z_MAX_PIN) != ENDSTOPS_INVERTING) && (current_block->steps_z > 0)){
// endstops_triggered(step_events_completed); endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS];
endstop_z_hit=true;
step_events_completed = current_block->step_event_count; step_events_completed = current_block->step_event_count;
} }
#endif #endif
} }
#ifndef ADVANCE #ifndef ADVANCE
if ((out_bits & (1<<E_AXIS)) != 0) // -direction if ((out_bits & (1<<E_AXIS)) != 0) { // -direction
WRITE(E_DIR_PIN,INVERT_E_DIR); WRITE(E_DIR_PIN,INVERT_E_DIR);
else // +direction count_direction[E_AXIS]=-1;
}
else { // +direction
WRITE(E_DIR_PIN,!INVERT_E_DIR); WRITE(E_DIR_PIN,!INVERT_E_DIR);
count_direction[E_AXIS]=-1;
}
#endif //!ADVANCE #endif //!ADVANCE
for(int8_t i=0; i < step_loops; i++) { // Take multiple steps per interrupt (For high speed moves) for(int8_t i=0; i < step_loops; i++) { // Take multiple steps per interrupt (For high speed moves)
@ -422,9 +401,7 @@ ISR(TIMER1_COMPA_vect)
WRITE(X_STEP_PIN, HIGH); WRITE(X_STEP_PIN, HIGH);
counter_x -= current_block->step_event_count; counter_x -= current_block->step_event_count;
WRITE(X_STEP_PIN, LOW); WRITE(X_STEP_PIN, LOW);
#ifdef DEBUG_STEPS count_position[X_AXIS]+=count_direction[X_AXIS];
count_position[X_AXIS]+=count_direction[X_AXIS];
#endif
} }
counter_y += current_block->steps_y; counter_y += current_block->steps_y;
@ -432,9 +409,7 @@ ISR(TIMER1_COMPA_vect)
WRITE(Y_STEP_PIN, HIGH); WRITE(Y_STEP_PIN, HIGH);
counter_y -= current_block->step_event_count; counter_y -= current_block->step_event_count;
WRITE(Y_STEP_PIN, LOW); WRITE(Y_STEP_PIN, LOW);
#ifdef DEBUG_STEPS count_position[Y_AXIS]+=count_direction[Y_AXIS];
count_position[Y_AXIS]+=count_direction[Y_AXIS];
#endif
} }
counter_z += current_block->steps_z; counter_z += current_block->steps_z;
@ -442,9 +417,7 @@ ISR(TIMER1_COMPA_vect)
WRITE(Z_STEP_PIN, HIGH); WRITE(Z_STEP_PIN, HIGH);
counter_z -= current_block->step_event_count; counter_z -= current_block->step_event_count;
WRITE(Z_STEP_PIN, LOW); WRITE(Z_STEP_PIN, LOW);
#ifdef DEBUG_STEPS count_position[Z_AXIS]+=count_direction[Z_AXIS];
count_position[Z_AXIS]+=count_direction[Z_AXIS];
#endif
} }
#ifndef ADVANCE #ifndef ADVANCE
@ -453,6 +426,7 @@ ISR(TIMER1_COMPA_vect)
WRITE(E_STEP_PIN, HIGH); WRITE(E_STEP_PIN, HIGH);
counter_e -= current_block->step_event_count; counter_e -= current_block->step_event_count;
WRITE(E_STEP_PIN, LOW); WRITE(E_STEP_PIN, LOW);
count_position[E_AXIS]+=count_direction[E_AXIS];
} }
#endif //!ADVANCE #endif //!ADVANCE
step_events_completed += 1; step_events_completed += 1;
@ -669,3 +643,22 @@ void st_synchronize()
LCD_STATUS; 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;
}

View File

@ -1,51 +1,51 @@
/* /*
stepper.h - stepper motor driver: executes motion plans of planner.c using the stepper motors stepper.h - stepper motor driver: executes motion plans of planner.c using the stepper motors
Part of Grbl Part of Grbl
Copyright (c) 2009-2011 Simen Svale Skogsrud Copyright (c) 2009-2011 Simen Svale Skogsrud
Grbl is free software: you can redistribute it and/or modify Grbl is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or the Free Software Foundation, either version 3 of the License, or
(at your option) any later version. (at your option) any later version.
Grbl is distributed in the hope that it will be useful, Grbl is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details. GNU General Public License for more details.
You should have received a copy of the GNU General Public License You should have received a copy of the GNU General Public License
along with Grbl. If not, see <http://www.gnu.org/licenses/>. along with Grbl. If not, see <http://www.gnu.org/licenses/>.
*/ */
#ifndef stepper_h #ifndef stepper_h
#define stepper_h #define stepper_h
#include "planner.h" #include "planner.h"
// Initialize and start the stepper motor subsystem // Initialize and start the stepper motor subsystem
void st_init(); void st_init();
// Block until all buffered steps are executed // Block until all buffered steps are executed
void st_synchronize(); void st_synchronize();
// The stepper subsystem goes to sleep when it runs out of things to execute. Call this // Set current position in steps
// to notify the subsystem that it is time to go to work. void st_set_position(const long &x, const long &y, const long &z, const long &e);
void st_wake_up();
// Get current position in steps
// if DEBUG_STEPS is enabled, M114 can be used to compare two methods of determining the X,Y,Z position of the printer. long st_get_position(char axis);
// for debugging purposes only, should be disabled by default
#ifdef DEBUG_STEPS // The stepper subsystem goes to sleep when it runs out of things to execute. Call this
extern volatile long count_position[NUM_AXIS]; // to notify the subsystem that it is time to go to work.
extern volatile int count_direction[NUM_AXIS]; void st_wake_up();
#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 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(); 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 extern block_t *current_block; // A pointer to the block currently being traced
#endif #endif