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;
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_ECHO_START;
SERIAL_ECHOPGM("endstops hit: "); SERIAL_ECHOPGM("endstops hit: ");
SERIAL_ECHOPAIR(" X:",endstops_triggerpos[0]); if(endstop_x_hit) {
SERIAL_ECHOPAIR(" Y:",endstops_triggerpos[1]); SERIAL_ECHOPAIR(" X:",(float)endstops_trigsteps[X_AXIS]/axis_steps_per_unit[X_AXIS]);
SERIAL_ECHOPAIR(" Z:",endstops_triggerpos[2]); }
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(""); SERIAL_ECHOLN("");
endstops_hit=false; endstop_x_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

@ -29,16 +29,16 @@ void st_init();
// Block until all buffered steps are executed // Block until all buffered steps are executed
void st_synchronize(); 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 // 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. // to notify the subsystem that it is time to go to work.
void st_wake_up(); 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 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();