melch
/
Marlin
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Extract update_endstops from stepper ISR (PR#2474)

Browse Source
2.0.x
Scott Lahteine 9 years ago committed by Richard Wackerbarth
parent adfcfcba95
commit 000a6fce46
1 changed files with 179 additions and 178 deletions
Show Stats Download Patch File Download Diff File

375
Marlin/stepper.cpp
Unescape Escape View File

@ -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++) {

Write Preview
Loading…
Cancel
Save