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; }
|
||||
|
||||
// __________________________
|
||||
// /| |\ _________________ ^
|
||||
// / | | \ /| |\ |
|
||||
// / | | \ / | | \ 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
|
||||
if (check_endstops) {
|
||||
// Check endstops
|
||||
inline void update_endstops() {
|
||||
|
||||
#ifdef Z_DUAL_ENDSTOPS
|
||||
uint16_t
|
||||
@ -645,8 +460,194 @@ ISR(TIMER1_COMPA_vect) {
|
||||
#endif
|
||||
}
|
||||
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)
|
||||
for (int8_t i = 0; i < step_loops; i++) {
|
||||
|
||||
Loading…
Reference in New Issue
Block a user