Extract update_endstops from stepper ISR (PR#2474)
This commit is contained in:
Scott Lahteine
Richard Wackerbarth
parent
adfcfcba95
commit
000a6fce46
@ -286,193 +286,8 @@ void checkHitEndstops() {
|
|||||||
|
|
||||||
void enable_endstops(bool check) { check_endstops = check; }
|
void enable_endstops(bool check) { check_endstops = check; }
|
||||||
|
|
||||||
// __________________________
|
|
||||||
// /| |\ _________________ ^
|
|
||||||
// / | | \ /| |\ |
|
|
||||||
// / | | \ / | | \ s
|
|
||||||
// / | | | | | \ p
|
|
||||||
// / | | | | | \ e
|
|
||||||
// +-----+------------------------+---+--+---------------+----+ e
|
|
||||||
// | BLOCK 1 | BLOCK 2 | d
|
|
||||||
//
|
|
||||||
// time ----->
|
|
||||||
//
|
|
||||||
// The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates
|
|
||||||
// first block->accelerate_until step_events_completed, then keeps going at constant speed until
|
|
||||||
// step_events_completed reaches block->decelerate_after after which it decelerates until the trapezoid generator is reset.
|
|
||||||
// The slope of acceleration is calculated using v = u + at where t is the accumulated timer values of the steps so far.
|
|
||||||
|
|
||||||
void st_wake_up() {
|
|
||||||
// TCNT1 = 0;
|
|
||||||
ENABLE_STEPPER_DRIVER_INTERRUPT();
|
|
||||||
}
|
|
||||||
|
|
||||||
FORCE_INLINE unsigned short calc_timer(unsigned short step_rate) {
|
|
||||||
unsigned short timer;
|
|
||||||
if (step_rate > MAX_STEP_FREQUENCY) step_rate = MAX_STEP_FREQUENCY;
|
|
||||||
|
|
||||||
if (step_rate > 20000) { // If steprate > 20kHz >> step 4 times
|
|
||||||
step_rate = (step_rate >> 2) & 0x3fff;
|
|
||||||
step_loops = 4;
|
|
||||||
}
|
|
||||||
else if (step_rate > 10000) { // If steprate > 10kHz >> step 2 times
|
|
||||||
step_rate = (step_rate >> 1) & 0x7fff;
|
|
||||||
step_loops = 2;
|
|
||||||
}
|
|
||||||
else {
|
|
||||||
step_loops = 1;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (step_rate < (F_CPU / 500000)) step_rate = (F_CPU / 500000);
|
|
||||||
step_rate -= (F_CPU / 500000); // Correct for minimal speed
|
|
||||||
if (step_rate >= (8 * 256)) { // higher step rate
|
|
||||||
unsigned short table_address = (unsigned short)&speed_lookuptable_fast[(unsigned char)(step_rate>>8)][0];
|
|
||||||
unsigned char tmp_step_rate = (step_rate & 0x00ff);
|
|
||||||
unsigned short gain = (unsigned short)pgm_read_word_near(table_address+2);
|
|
||||||
MultiU16X8toH16(timer, tmp_step_rate, gain);
|
|
||||||
timer = (unsigned short)pgm_read_word_near(table_address) - timer;
|
|
||||||
}
|
|
||||||
else { // lower step rates
|
|
||||||
unsigned short table_address = (unsigned short)&speed_lookuptable_slow[0][0];
|
|
||||||
table_address += ((step_rate)>>1) & 0xfffc;
|
|
||||||
timer = (unsigned short)pgm_read_word_near(table_address);
|
|
||||||
timer -= (((unsigned short)pgm_read_word_near(table_address+2) * (unsigned char)(step_rate & 0x0007))>>3);
|
|
||||||
}
|
|
||||||
if (timer < 100) { timer = 100; MYSERIAL.print(MSG_STEPPER_TOO_HIGH); MYSERIAL.println(step_rate); }//(20kHz this should never happen)
|
|
||||||
return timer;
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Set the stepper direction of each axis
|
|
||||||
*
|
|
||||||
* X_AXIS=A_AXIS and Y_AXIS=B_AXIS for COREXY
|
|
||||||
* X_AXIS=A_AXIS and Z_AXIS=C_AXIS for COREXZ
|
|
||||||
*/
|
|
||||||
void set_stepper_direction() {
|
|
||||||
|
|
||||||
if (TEST(out_bits, X_AXIS)) { // A_AXIS
|
|
||||||
X_APPLY_DIR(INVERT_X_DIR, 0);
|
|
||||||
count_direction[X_AXIS] = -1;
|
|
||||||
}
|
|
||||||
else {
|
|
||||||
X_APPLY_DIR(!INVERT_X_DIR, 0);
|
|
||||||
count_direction[X_AXIS] = 1;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (TEST(out_bits, Y_AXIS)) { // B_AXIS
|
|
||||||
Y_APPLY_DIR(INVERT_Y_DIR, 0);
|
|
||||||
count_direction[Y_AXIS] = -1;
|
|
||||||
}
|
|
||||||
else {
|
|
||||||
Y_APPLY_DIR(!INVERT_Y_DIR, 0);
|
|
||||||
count_direction[Y_AXIS] = 1;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (TEST(out_bits, Z_AXIS)) { // C_AXIS
|
|
||||||
Z_APPLY_DIR(INVERT_Z_DIR, 0);
|
|
||||||
count_direction[Z_AXIS] = -1;
|
|
||||||
}
|
|
||||||
else {
|
|
||||||
Z_APPLY_DIR(!INVERT_Z_DIR, 0);
|
|
||||||
count_direction[Z_AXIS] = 1;
|
|
||||||
}
|
|
||||||
|
|
||||||
#ifndef ADVANCE
|
|
||||||
if (TEST(out_bits, E_AXIS)) {
|
|
||||||
REV_E_DIR();
|
|
||||||
count_direction[E_AXIS] = -1;
|
|
||||||
}
|
|
||||||
else {
|
|
||||||
NORM_E_DIR();
|
|
||||||
count_direction[E_AXIS] = 1;
|
|
||||||
}
|
|
||||||
#endif //!ADVANCE
|
|
||||||
}
|
|
||||||
|
|
||||||
// Initializes the trapezoid generator from the current block. Called whenever a new
|
|
||||||
// block begins.
|
|
||||||
FORCE_INLINE void trapezoid_generator_reset() {
|
|
||||||
|
|
||||||
if (current_block->direction_bits != out_bits) {
|
|
||||||
out_bits = current_block->direction_bits;
|
|
||||||
set_stepper_direction();
|
|
||||||
}
|
|
||||||
|
|
||||||
#ifdef ADVANCE
|
|
||||||
advance = current_block->initial_advance;
|
|
||||||
final_advance = current_block->final_advance;
|
|
||||||
// Do E steps + advance steps
|
|
||||||
e_steps[current_block->active_extruder] += ((advance >>8) - old_advance);
|
|
||||||
old_advance = advance >>8;
|
|
||||||
#endif
|
|
||||||
deceleration_time = 0;
|
|
||||||
// step_rate to timer interval
|
|
||||||
OCR1A_nominal = calc_timer(current_block->nominal_rate);
|
|
||||||
// make a note of the number of step loops required at nominal speed
|
|
||||||
step_loops_nominal = step_loops;
|
|
||||||
acc_step_rate = current_block->initial_rate;
|
|
||||||
acceleration_time = calc_timer(acc_step_rate);
|
|
||||||
OCR1A = acceleration_time;
|
|
||||||
|
|
||||||
// SERIAL_ECHO_START;
|
|
||||||
// SERIAL_ECHOPGM("advance :");
|
|
||||||
// SERIAL_ECHO(current_block->advance/256.0);
|
|
||||||
// SERIAL_ECHOPGM("advance rate :");
|
|
||||||
// SERIAL_ECHO(current_block->advance_rate/256.0);
|
|
||||||
// SERIAL_ECHOPGM("initial advance :");
|
|
||||||
// SERIAL_ECHO(current_block->initial_advance/256.0);
|
|
||||||
// SERIAL_ECHOPGM("final advance :");
|
|
||||||
// SERIAL_ECHOLN(current_block->final_advance/256.0);
|
|
||||||
}
|
|
||||||
|
|
||||||
// "The Stepper Driver Interrupt" - This timer interrupt is the workhorse.
|
|
||||||
// It pops blocks from the block_buffer and executes them by pulsing the stepper pins appropriately.
|
|
||||||
ISR(TIMER1_COMPA_vect) {
|
|
||||||
|
|
||||||
if (cleaning_buffer_counter)
|
|
||||||
{
|
|
||||||
current_block = NULL;
|
|
||||||
plan_discard_current_block();
|
|
||||||
#ifdef SD_FINISHED_RELEASECOMMAND
|
|
||||||
if ((cleaning_buffer_counter == 1) && (SD_FINISHED_STEPPERRELEASE)) enqueuecommands_P(PSTR(SD_FINISHED_RELEASECOMMAND));
|
|
||||||
#endif
|
|
||||||
cleaning_buffer_counter--;
|
|
||||||
OCR1A = 200;
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
// If there is no current block, attempt to pop one from the buffer
|
|
||||||
if (!current_block) {
|
|
||||||
// Anything in the buffer?
|
|
||||||
current_block = plan_get_current_block();
|
|
||||||
if (current_block) {
|
|
||||||
current_block->busy = true;
|
|
||||||
trapezoid_generator_reset();
|
|
||||||
counter_x = -(current_block->step_event_count >> 1);
|
|
||||||
counter_y = counter_z = counter_e = counter_x;
|
|
||||||
step_events_completed = 0;
|
|
||||||
|
|
||||||
#ifdef Z_LATE_ENABLE
|
|
||||||
if (current_block->steps[Z_AXIS] > 0) {
|
|
||||||
enable_z();
|
|
||||||
OCR1A = 2000; //1ms wait
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
|
|
||||||
// #ifdef ADVANCE
|
|
||||||
// e_steps[current_block->active_extruder] = 0;
|
|
||||||
// #endif
|
|
||||||
}
|
|
||||||
else {
|
|
||||||
OCR1A = 2000; // 1kHz.
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
if (current_block != NULL) {
|
|
||||||
|
|
||||||
// Check endstops
|
// Check endstops
|
||||||
if (check_endstops) {
|
inline void update_endstops() {
|
||||||
|
|
||||||
#ifdef Z_DUAL_ENDSTOPS
|
#ifdef Z_DUAL_ENDSTOPS
|
||||||
uint16_t
|
uint16_t
|
||||||
@ -647,6 +462,192 @@ ISR(TIMER1_COMPA_vect) {
|
|||||||
old_endstop_bits = current_endstop_bits;
|
old_endstop_bits = current_endstop_bits;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// __________________________
|
||||||
|
// /| |\ _________________ ^
|
||||||
|
// / | | \ /| |\ |
|
||||||
|
// / | | \ / | | \ s
|
||||||
|
// / | | | | | \ p
|
||||||
|
// / | | | | | \ e
|
||||||
|
// +-----+------------------------+---+--+---------------+----+ e
|
||||||
|
// | BLOCK 1 | BLOCK 2 | d
|
||||||
|
//
|
||||||
|
// time ----->
|
||||||
|
//
|
||||||
|
// The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates
|
||||||
|
// first block->accelerate_until step_events_completed, then keeps going at constant speed until
|
||||||
|
// step_events_completed reaches block->decelerate_after after which it decelerates until the trapezoid generator is reset.
|
||||||
|
// The slope of acceleration is calculated using v = u + at where t is the accumulated timer values of the steps so far.
|
||||||
|
|
||||||
|
void st_wake_up() {
|
||||||
|
// TCNT1 = 0;
|
||||||
|
ENABLE_STEPPER_DRIVER_INTERRUPT();
|
||||||
|
}
|
||||||
|
|
||||||
|
FORCE_INLINE unsigned short calc_timer(unsigned short step_rate) {
|
||||||
|
unsigned short timer;
|
||||||
|
if (step_rate > MAX_STEP_FREQUENCY) step_rate = MAX_STEP_FREQUENCY;
|
||||||
|
|
||||||
|
if (step_rate > 20000) { // If steprate > 20kHz >> step 4 times
|
||||||
|
step_rate = (step_rate >> 2) & 0x3fff;
|
||||||
|
step_loops = 4;
|
||||||
|
}
|
||||||
|
else if (step_rate > 10000) { // If steprate > 10kHz >> step 2 times
|
||||||
|
step_rate = (step_rate >> 1) & 0x7fff;
|
||||||
|
step_loops = 2;
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
step_loops = 1;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (step_rate < (F_CPU / 500000)) step_rate = (F_CPU / 500000);
|
||||||
|
step_rate -= (F_CPU / 500000); // Correct for minimal speed
|
||||||
|
if (step_rate >= (8 * 256)) { // higher step rate
|
||||||
|
unsigned short table_address = (unsigned short)&speed_lookuptable_fast[(unsigned char)(step_rate>>8)][0];
|
||||||
|
unsigned char tmp_step_rate = (step_rate & 0x00ff);
|
||||||
|
unsigned short gain = (unsigned short)pgm_read_word_near(table_address+2);
|
||||||
|
MultiU16X8toH16(timer, tmp_step_rate, gain);
|
||||||
|
timer = (unsigned short)pgm_read_word_near(table_address) - timer;
|
||||||
|
}
|
||||||
|
else { // lower step rates
|
||||||
|
unsigned short table_address = (unsigned short)&speed_lookuptable_slow[0][0];
|
||||||
|
table_address += ((step_rate)>>1) & 0xfffc;
|
||||||
|
timer = (unsigned short)pgm_read_word_near(table_address);
|
||||||
|
timer -= (((unsigned short)pgm_read_word_near(table_address+2) * (unsigned char)(step_rate & 0x0007))>>3);
|
||||||
|
}
|
||||||
|
if (timer < 100) { timer = 100; MYSERIAL.print(MSG_STEPPER_TOO_HIGH); MYSERIAL.println(step_rate); }//(20kHz this should never happen)
|
||||||
|
return timer;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Set the stepper direction of each axis
|
||||||
|
*
|
||||||
|
* X_AXIS=A_AXIS and Y_AXIS=B_AXIS for COREXY
|
||||||
|
* X_AXIS=A_AXIS and Z_AXIS=C_AXIS for COREXZ
|
||||||
|
*/
|
||||||
|
void set_stepper_direction() {
|
||||||
|
|
||||||
|
if (TEST(out_bits, X_AXIS)) { // A_AXIS
|
||||||
|
X_APPLY_DIR(INVERT_X_DIR, 0);
|
||||||
|
count_direction[X_AXIS] = -1;
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
X_APPLY_DIR(!INVERT_X_DIR, 0);
|
||||||
|
count_direction[X_AXIS] = 1;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (TEST(out_bits, Y_AXIS)) { // B_AXIS
|
||||||
|
Y_APPLY_DIR(INVERT_Y_DIR, 0);
|
||||||
|
count_direction[Y_AXIS] = -1;
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
Y_APPLY_DIR(!INVERT_Y_DIR, 0);
|
||||||
|
count_direction[Y_AXIS] = 1;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (TEST(out_bits, Z_AXIS)) { // C_AXIS
|
||||||
|
Z_APPLY_DIR(INVERT_Z_DIR, 0);
|
||||||
|
count_direction[Z_AXIS] = -1;
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
Z_APPLY_DIR(!INVERT_Z_DIR, 0);
|
||||||
|
count_direction[Z_AXIS] = 1;
|
||||||
|
}
|
||||||
|
|
||||||
|
#ifndef ADVANCE
|
||||||
|
if (TEST(out_bits, E_AXIS)) {
|
||||||
|
REV_E_DIR();
|
||||||
|
count_direction[E_AXIS] = -1;
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
NORM_E_DIR();
|
||||||
|
count_direction[E_AXIS] = 1;
|
||||||
|
}
|
||||||
|
#endif //!ADVANCE
|
||||||
|
}
|
||||||
|
|
||||||
|
// Initializes the trapezoid generator from the current block. Called whenever a new
|
||||||
|
// block begins.
|
||||||
|
FORCE_INLINE void trapezoid_generator_reset() {
|
||||||
|
|
||||||
|
if (current_block->direction_bits != out_bits) {
|
||||||
|
out_bits = current_block->direction_bits;
|
||||||
|
set_stepper_direction();
|
||||||
|
}
|
||||||
|
|
||||||
|
#ifdef ADVANCE
|
||||||
|
advance = current_block->initial_advance;
|
||||||
|
final_advance = current_block->final_advance;
|
||||||
|
// Do E steps + advance steps
|
||||||
|
e_steps[current_block->active_extruder] += ((advance >>8) - old_advance);
|
||||||
|
old_advance = advance >>8;
|
||||||
|
#endif
|
||||||
|
deceleration_time = 0;
|
||||||
|
// step_rate to timer interval
|
||||||
|
OCR1A_nominal = calc_timer(current_block->nominal_rate);
|
||||||
|
// make a note of the number of step loops required at nominal speed
|
||||||
|
step_loops_nominal = step_loops;
|
||||||
|
acc_step_rate = current_block->initial_rate;
|
||||||
|
acceleration_time = calc_timer(acc_step_rate);
|
||||||
|
OCR1A = acceleration_time;
|
||||||
|
|
||||||
|
// SERIAL_ECHO_START;
|
||||||
|
// SERIAL_ECHOPGM("advance :");
|
||||||
|
// SERIAL_ECHO(current_block->advance/256.0);
|
||||||
|
// SERIAL_ECHOPGM("advance rate :");
|
||||||
|
// SERIAL_ECHO(current_block->advance_rate/256.0);
|
||||||
|
// SERIAL_ECHOPGM("initial advance :");
|
||||||
|
// SERIAL_ECHO(current_block->initial_advance/256.0);
|
||||||
|
// SERIAL_ECHOPGM("final advance :");
|
||||||
|
// SERIAL_ECHOLN(current_block->final_advance/256.0);
|
||||||
|
}
|
||||||
|
|
||||||
|
// "The Stepper Driver Interrupt" - This timer interrupt is the workhorse.
|
||||||
|
// It pops blocks from the block_buffer and executes them by pulsing the stepper pins appropriately.
|
||||||
|
ISR(TIMER1_COMPA_vect) {
|
||||||
|
|
||||||
|
if (cleaning_buffer_counter) {
|
||||||
|
current_block = NULL;
|
||||||
|
plan_discard_current_block();
|
||||||
|
#ifdef SD_FINISHED_RELEASECOMMAND
|
||||||
|
if ((cleaning_buffer_counter == 1) && (SD_FINISHED_STEPPERRELEASE)) enqueuecommands_P(PSTR(SD_FINISHED_RELEASECOMMAND));
|
||||||
|
#endif
|
||||||
|
cleaning_buffer_counter--;
|
||||||
|
OCR1A = 200;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// If there is no current block, attempt to pop one from the buffer
|
||||||
|
if (!current_block) {
|
||||||
|
// Anything in the buffer?
|
||||||
|
current_block = plan_get_current_block();
|
||||||
|
if (current_block) {
|
||||||
|
current_block->busy = true;
|
||||||
|
trapezoid_generator_reset();
|
||||||
|
counter_x = -(current_block->step_event_count >> 1);
|
||||||
|
counter_y = counter_z = counter_e = counter_x;
|
||||||
|
step_events_completed = 0;
|
||||||
|
|
||||||
|
#ifdef Z_LATE_ENABLE
|
||||||
|
if (current_block->steps[Z_AXIS] > 0) {
|
||||||
|
enable_z();
|
||||||
|
OCR1A = 2000; //1ms wait
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
// #ifdef ADVANCE
|
||||||
|
// e_steps[current_block->active_extruder] = 0;
|
||||||
|
// #endif
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
OCR1A = 2000; // 1kHz.
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (current_block != NULL) {
|
||||||
|
|
||||||
|
// Update endstops state, if enabled
|
||||||
|
if (check_endstops) update_endstops();
|
||||||
|
|
||||||
// Take multiple steps per interrupt (For high speed moves)
|
// Take multiple steps per interrupt (For high speed moves)
|
||||||
for (int8_t i = 0; i < step_loops; i++) {
|
for (int8_t i = 0; i < step_loops; i++) {
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user