123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826827828829830831832833834835836837838839840841842843844845846847848849850851852853854855856857858859860861862863864865866867868869870871872873874875876877878879880881882883884885886887888889890891892893894895896897898899900901902903904905906907908909910911912913914915916917918919920921922923924925926927928929930931932933934935936937938939940941942943944945946947948949950951952953954955956957958959960961962963964965966967968969970971972973974975976977978979980981982983984985986987988989990991992993994995996997998999100010011002100310041005100610071008100910101011101210131014101510161017101810191020102110221023102410251026102710281029103010311032103310341035103610371038103910401041104210431044104510461047104810491050105110521053105410551056105710581059106010611062106310641065106610671068106910701071107210731074107510761077107810791080108110821083108410851086108710881089109010911092109310941095109610971098109911001101110211031104110511061107110811091110111111121113111411151116111711181119112011211122112311241125112611271128112911301131113211331134113511361137113811391140114111421143114411451146114711481149115011511152115311541155115611571158115911601161116211631164116511661167116811691170117111721173117411751176117711781179118011811182118311841185118611871188118911901191119211931194119511961197119811991200120112021203120412051206120712081209121012111212121312141215121612171218121912201221122212231224122512261227122812291230123112321233123412351236123712381239124012411242124312441245124612471248124912501251125212531254125512561257125812591260126112621263126412651266126712681269127012711272127312741275127612771278127912801281128212831284128512861287128812891290129112921293129412951296129712981299130013011302130313041305130613071308130913101311131213131314131513161317131813191320132113221323132413251326132713281329133013311332133313341335133613371338133913401341134213431344134513461347134813491350135113521353 |
-
-
-
-
-
-
- #include "Marlin.h"
- #include "stepper.h"
- #include "endstops.h"
- #include "planner.h"
- #include "temperature.h"
- #include "ultralcd.h"
- #include "language.h"
- #include "cardreader.h"
- #include "speed_lookuptable.h"
-
- #if HAS_DIGIPOTSS
- #include <SPI.h>
- #endif
-
- Stepper stepper;
-
-
-
- block_t* Stepper::current_block = NULL;
-
- #if ENABLED(ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED)
- bool Stepper::abort_on_endstop_hit = false;
- #endif
-
- #if ENABLED(Z_DUAL_ENDSTOPS)
- bool Stepper::performing_homing = false;
- #endif
-
-
-
- unsigned char Stepper::last_direction_bits = 0;
- unsigned int Stepper::cleaning_buffer_counter = 0;
-
- #if ENABLED(Z_DUAL_ENDSTOPS)
- bool Stepper::locked_z_motor = false;
- bool Stepper::locked_z2_motor = false;
- #endif
-
- long Stepper::counter_X = 0,
- Stepper::counter_Y = 0,
- Stepper::counter_Z = 0,
- Stepper::counter_E = 0;
-
- volatile uint32_t Stepper::step_events_completed = 0;
-
- #if ENABLED(ADVANCE) || ENABLED(LIN_ADVANCE)
-
- unsigned char Stepper::old_OCR0A = 0;
- volatile unsigned char Stepper::eISR_Rate = 200;
-
- #if ENABLED(LIN_ADVANCE)
- volatile long Stepper::e_steps[E_STEPPERS];
- int Stepper::extruder_advance_k = LIN_ADVANCE_K,
- Stepper::final_estep_rate,
- Stepper::current_estep_rate[E_STEPPERS],
- Stepper::current_adv_steps[E_STEPPERS];
- #else
- long Stepper::e_steps[E_STEPPERS],
- Stepper::final_advance = 0,
- Stepper::old_advance = 0,
- Stepper::advance_rate,
- Stepper::advance;
- #endif
- #endif
-
- long Stepper::acceleration_time, Stepper::deceleration_time;
-
- volatile long Stepper::count_position[NUM_AXIS] = { 0 };
- volatile signed char Stepper::count_direction[NUM_AXIS] = { 1, 1, 1, 1 };
-
- #if ENABLED(MIXING_EXTRUDER)
- long Stepper::counter_m[MIXING_STEPPERS];
- #endif
-
- unsigned short Stepper::acc_step_rate;
- uint8_t Stepper::step_loops, Stepper::step_loops_nominal;
- unsigned short Stepper::OCR1A_nominal;
-
- volatile long Stepper::endstops_trigsteps[XYZ];
-
- #if ENABLED(X_DUAL_STEPPER_DRIVERS)
- #define X_APPLY_DIR(v,Q) do{ X_DIR_WRITE(v); X2_DIR_WRITE((v) != INVERT_X2_VS_X_DIR); }while(0)
- #define X_APPLY_STEP(v,Q) do{ X_STEP_WRITE(v); X2_STEP_WRITE(v); }while(0)
- #elif ENABLED(DUAL_X_CARRIAGE)
- #define X_APPLY_DIR(v,ALWAYS) \
- if (extruder_duplication_enabled || ALWAYS) { \
- X_DIR_WRITE(v); \
- X2_DIR_WRITE(v); \
- } \
- else { \
- if (current_block->active_extruder) X2_DIR_WRITE(v); else X_DIR_WRITE(v); \
- }
- #define X_APPLY_STEP(v,ALWAYS) \
- if (extruder_duplication_enabled || ALWAYS) { \
- X_STEP_WRITE(v); \
- X2_STEP_WRITE(v); \
- } \
- else { \
- if (current_block->active_extruder != 0) X2_STEP_WRITE(v); else X_STEP_WRITE(v); \
- }
- #else
- #define X_APPLY_DIR(v,Q) X_DIR_WRITE(v)
- #define X_APPLY_STEP(v,Q) X_STEP_WRITE(v)
- #endif
-
- #if ENABLED(Y_DUAL_STEPPER_DRIVERS)
- #define Y_APPLY_DIR(v,Q) do{ Y_DIR_WRITE(v); Y2_DIR_WRITE((v) != INVERT_Y2_VS_Y_DIR); }while(0)
- #define Y_APPLY_STEP(v,Q) do{ Y_STEP_WRITE(v); Y2_STEP_WRITE(v); }while(0)
- #else
- #define Y_APPLY_DIR(v,Q) Y_DIR_WRITE(v)
- #define Y_APPLY_STEP(v,Q) Y_STEP_WRITE(v)
- #endif
-
- #if ENABLED(Z_DUAL_STEPPER_DRIVERS)
- #define Z_APPLY_DIR(v,Q) do{ Z_DIR_WRITE(v); Z2_DIR_WRITE(v); }while(0)
- #if ENABLED(Z_DUAL_ENDSTOPS)
- #define Z_APPLY_STEP(v,Q) \
- if (performing_homing) { \
- if (Z_HOME_DIR > 0) {\
- if (!(TEST(endstops.old_endstop_bits, Z_MAX) && (count_direction[Z_AXIS] > 0)) && !locked_z_motor) Z_STEP_WRITE(v); \
- if (!(TEST(endstops.old_endstop_bits, Z2_MAX) && (count_direction[Z_AXIS] > 0)) && !locked_z2_motor) Z2_STEP_WRITE(v); \
- } \
- else { \
- if (!(TEST(endstops.old_endstop_bits, Z_MIN) && (count_direction[Z_AXIS] < 0)) && !locked_z_motor) Z_STEP_WRITE(v); \
- if (!(TEST(endstops.old_endstop_bits, Z2_MIN) && (count_direction[Z_AXIS] < 0)) && !locked_z2_motor) Z2_STEP_WRITE(v); \
- } \
- } \
- else { \
- Z_STEP_WRITE(v); \
- Z2_STEP_WRITE(v); \
- }
- #else
- #define Z_APPLY_STEP(v,Q) do{ Z_STEP_WRITE(v); Z2_STEP_WRITE(v); }while(0)
- #endif
- #else
- #define Z_APPLY_DIR(v,Q) Z_DIR_WRITE(v)
- #define Z_APPLY_STEP(v,Q) Z_STEP_WRITE(v)
- #endif
-
- #if DISABLED(MIXING_EXTRUDER)
- #define E_APPLY_STEP(v,Q) E_STEP_WRITE(v)
- #endif
-
-
-
-
-
-
-
-
-
-
-
- #define MultiU24X32toH16(intRes, longIn1, longIn2) \
- asm volatile ( \
- "clr r26 \n\t" \
- "mul %A1, %B2 \n\t" \
- "mov r27, r1 \n\t" \
- "mul %B1, %C2 \n\t" \
- "movw %A0, r0 \n\t" \
- "mul %C1, %C2 \n\t" \
- "add %B0, r0 \n\t" \
- "mul %C1, %B2 \n\t" \
- "add %A0, r0 \n\t" \
- "adc %B0, r1 \n\t" \
- "mul %A1, %C2 \n\t" \
- "add r27, r0 \n\t" \
- "adc %A0, r1 \n\t" \
- "adc %B0, r26 \n\t" \
- "mul %B1, %B2 \n\t" \
- "add r27, r0 \n\t" \
- "adc %A0, r1 \n\t" \
- "adc %B0, r26 \n\t" \
- "mul %C1, %A2 \n\t" \
- "add r27, r0 \n\t" \
- "adc %A0, r1 \n\t" \
- "adc %B0, r26 \n\t" \
- "mul %B1, %A2 \n\t" \
- "add r27, r1 \n\t" \
- "adc %A0, r26 \n\t" \
- "adc %B0, r26 \n\t" \
- "lsr r27 \n\t" \
- "adc %A0, r26 \n\t" \
- "adc %B0, r26 \n\t" \
- "mul %D2, %A1 \n\t" \
- "add %A0, r0 \n\t" \
- "adc %B0, r1 \n\t" \
- "mul %D2, %B1 \n\t" \
- "add %B0, r0 \n\t" \
- "clr r1 \n\t" \
- : \
- "=&r" (intRes) \
- : \
- "d" (longIn1), \
- "d" (longIn2) \
- : \
- "r26" , "r27" \
- )
-
-
-
- #define ENABLE_STEPPER_DRIVER_INTERRUPT() SBI(TIMSK1, OCIE1A)
- #define DISABLE_STEPPER_DRIVER_INTERRUPT() CBI(TIMSK1, OCIE1A)
-
-
- void Stepper::wake_up() {
-
- ENABLE_STEPPER_DRIVER_INTERRUPT();
- }
-
-
- void Stepper::set_directions() {
-
- #define SET_STEP_DIR(AXIS) \
- if (motor_direction(AXIS ##_AXIS)) { \
- AXIS ##_APPLY_DIR(INVERT_## AXIS ##_DIR, false); \
- count_direction[AXIS ##_AXIS] = -1; \
- } \
- else { \
- AXIS ##_APPLY_DIR(!INVERT_## AXIS ##_DIR, false); \
- count_direction[AXIS ##_AXIS] = 1; \
- }
-
- #if HAS_X_DIR
- SET_STEP_DIR(X);
- #endif
- #if HAS_Y_DIR
- SET_STEP_DIR(Y);
- #endif
- #if HAS_Z_DIR
- SET_STEP_DIR(Z);
- #endif
-
- #if DISABLED(ADVANCE) && DISABLED(LIN_ADVANCE)
- if (motor_direction(E_AXIS)) {
- REV_E_DIR();
- count_direction[E_AXIS] = -1;
- }
- else {
- NORM_E_DIR();
- count_direction[E_AXIS] = 1;
- }
- #endif
- }
-
-
-
- ISR(TIMER1_COMPA_vect) { Stepper::isr(); }
-
- void Stepper::isr() {
- if (cleaning_buffer_counter) {
- current_block = NULL;
- planner.discard_current_block();
- #ifdef SD_FINISHED_RELEASECOMMAND
- if ((cleaning_buffer_counter == 1) && (SD_FINISHED_STEPPERRELEASE)) enqueue_and_echo_commands_P(PSTR(SD_FINISHED_RELEASECOMMAND));
- #endif
- cleaning_buffer_counter--;
- OCR1A = 200;
- return;
- }
-
-
- if (!current_block) {
-
- current_block = planner.get_current_block();
- if (current_block) {
- current_block->busy = true;
- trapezoid_generator_reset();
-
-
- counter_X = counter_Y = counter_Z = counter_E = -(current_block->step_event_count >> 1);
-
- #if ENABLED(MIXING_EXTRUDER)
- MIXING_STEPPERS_LOOP(i)
- counter_m[i] = -(current_block->mix_event_count[i] >> 1);
- #endif
-
- step_events_completed = 0;
-
- #if ENABLED(Z_LATE_ENABLE)
- if (current_block->steps[Z_AXIS] > 0) {
- enable_z();
- OCR1A = 2000;
- return;
- }
- #endif
-
-
-
-
- }
- else {
- OCR1A = 2000;
- return;
- }
- }
-
-
- if (endstops.enabled
- #if HAS_BED_PROBE
- || endstops.z_probe_enabled
- #endif
- ) endstops.update();
-
-
- bool all_steps_done = false;
- for (int8_t i = 0; i < step_loops; i++) {
- #ifndef USBCON
- customizedSerial.checkRx();
- #endif
-
- #if ENABLED(LIN_ADVANCE)
-
- counter_E += current_block->steps[E_AXIS];
- if (counter_E > 0) {
- counter_E -= current_block->step_event_count;
- #if DISABLED(MIXING_EXTRUDER)
-
- count_position[E_AXIS] += count_direction[E_AXIS];
- motor_direction(E_AXIS) ? --e_steps[TOOL_E_INDEX] : ++e_steps[TOOL_E_INDEX];
- #endif
- }
-
- #if ENABLED(MIXING_EXTRUDER)
-
- bool dir = motor_direction(E_AXIS);
- MIXING_STEPPERS_LOOP(j) {
- counter_m[j] += current_block->steps[E_AXIS];
- if (counter_m[j] > 0) {
- counter_m[j] -= current_block->mix_event_count[j];
- dir ? --e_steps[j] : ++e_steps[j];
- }
- }
- #endif
-
- if (current_block->use_advance_lead) {
- int delta_adv_steps = (((long)extruder_advance_k * current_estep_rate[TOOL_E_INDEX]) >> 9) - current_adv_steps[TOOL_E_INDEX];
- #if ENABLED(MIXING_EXTRUDER)
-
- MIXING_STEPPERS_LOOP(j) {
- int steps = delta_adv_steps * current_block->step_event_count / current_block->mix_event_count[j];
- e_steps[j] += steps;
- current_adv_steps[j] += steps;
- }
- #else
-
- e_steps[TOOL_E_INDEX] += delta_adv_steps;
- current_adv_steps[TOOL_E_INDEX] += delta_adv_steps;
- #endif
- }
-
- #elif ENABLED(ADVANCE)
-
-
- counter_E += current_block->steps[E_AXIS];
- if (counter_E > 0) {
- counter_E -= current_block->step_event_count;
- #if DISABLED(MIXING_EXTRUDER)
-
- motor_direction(E_AXIS) ? --e_steps[TOOL_E_INDEX] : ++e_steps[TOOL_E_INDEX];
- #endif
- }
-
- #if ENABLED(MIXING_EXTRUDER)
-
-
- bool dir = motor_direction(E_AXIS);
- MIXING_STEPPERS_LOOP(j) {
- counter_m[j] += current_block->steps[E_AXIS];
- if (counter_m[j] > 0) {
- counter_m[j] -= current_block->mix_event_count[j];
- dir ? --e_steps[j] : ++e_steps[j];
- }
- }
-
- #endif
-
- #endif
-
- #define _COUNTER(AXIS) counter_## AXIS
- #define _APPLY_STEP(AXIS) AXIS ##_APPLY_STEP
- #define _INVERT_STEP_PIN(AXIS) INVERT_## AXIS ##_STEP_PIN
-
-
- #define PULSE_START(AXIS) \
- _COUNTER(AXIS) += current_block->steps[_AXIS(AXIS)]; \
- if (_COUNTER(AXIS) > 0) { _APPLY_STEP(AXIS)(!_INVERT_STEP_PIN(AXIS),0); }
-
-
- #define PULSE_STOP(AXIS) \
- if (_COUNTER(AXIS) > 0) { \
- _COUNTER(AXIS) -= current_block->step_event_count; \
- count_position[_AXIS(AXIS)] += count_direction[_AXIS(AXIS)]; \
- _APPLY_STEP(AXIS)(_INVERT_STEP_PIN(AXIS),0); \
- }
-
- #define CYCLES_EATEN_BY_CODE 240
-
-
- #if STEP_PULSE_CYCLES > CYCLES_EATEN_BY_CODE
- static uint32_t pulse_start;
- pulse_start = TCNT0;
- #endif
-
- #if HAS_X_STEP
- PULSE_START(X);
- #endif
- #if HAS_Y_STEP
- PULSE_START(Y);
- #endif
- #if HAS_Z_STEP
- PULSE_START(Z);
- #endif
-
-
- #if DISABLED(ADVANCE) && DISABLED(LIN_ADVANCE)
- #if ENABLED(MIXING_EXTRUDER)
-
- counter_E += current_block->steps[E_AXIS];
-
- MIXING_STEPPERS_LOOP(j) {
-
- counter_m[j] += current_block->steps[E_AXIS];
-
- if (counter_m[j] > 0) En_STEP_WRITE(j, !INVERT_E_STEP_PIN);
- }
- #else
- PULSE_START(E);
- #endif
- #endif
-
-
- #if STEP_PULSE_CYCLES > CYCLES_EATEN_BY_CODE
- while ((uint32_t)(TCNT0 - pulse_start) < STEP_PULSE_CYCLES - CYCLES_EATEN_BY_CODE) { }
- #endif
-
- #if HAS_X_STEP
- PULSE_STOP(X);
- #endif
- #if HAS_Y_STEP
- PULSE_STOP(Y);
- #endif
- #if HAS_Z_STEP
- PULSE_STOP(Z);
- #endif
-
- #if DISABLED(ADVANCE) && DISABLED(LIN_ADVANCE)
- #if ENABLED(MIXING_EXTRUDER)
-
- if (counter_E > 0) {
- counter_E -= current_block->step_event_count;
- count_position[E_AXIS] += count_direction[E_AXIS];
- }
- MIXING_STEPPERS_LOOP(j) {
- if (counter_m[j] > 0) {
- counter_m[j] -= current_block->mix_event_count[j];
- En_STEP_WRITE(j, INVERT_E_STEP_PIN);
- }
- }
- #else
- PULSE_STOP(E);
- #endif
- #endif
-
- if (++step_events_completed >= current_block->step_event_count) {
- all_steps_done = true;
- break;
- }
- }
-
- #if ENABLED(ADVANCE) || ENABLED(LIN_ADVANCE)
-
- if (e_steps[TOOL_E_INDEX]) OCR0A = TCNT0 + 2;
- #endif
-
-
- uint16_t timer, step_rate;
- if (step_events_completed <= (uint32_t)current_block->accelerate_until) {
-
- MultiU24X32toH16(acc_step_rate, acceleration_time, current_block->acceleration_rate);
- acc_step_rate += current_block->initial_rate;
-
-
- NOMORE(acc_step_rate, current_block->nominal_rate);
-
-
- timer = calc_timer(acc_step_rate);
- OCR1A = timer;
- acceleration_time += timer;
-
- #if ENABLED(LIN_ADVANCE)
-
- if (current_block->use_advance_lead)
- current_estep_rate[TOOL_E_INDEX] = ((uint32_t)acc_step_rate * current_block->e_speed_multiplier8) >> 8;
-
- if (current_block->use_advance_lead) {
- #if ENABLED(MIXING_EXTRUDER)
- MIXING_STEPPERS_LOOP(j)
- current_estep_rate[j] = ((uint32_t)acc_step_rate * current_block->e_speed_multiplier8 * current_block->step_event_count / current_block->mix_event_count[j]) >> 8;
- #else
- current_estep_rate[TOOL_E_INDEX] = ((uint32_t)acc_step_rate * current_block->e_speed_multiplier8) >> 8;
- #endif
- }
-
- #elif ENABLED(ADVANCE)
-
- advance += advance_rate * step_loops;
-
-
- long advance_whole = advance >> 8,
- advance_factor = advance_whole - old_advance;
-
-
- #if ENABLED(MIXING_EXTRUDER)
-
- MIXING_STEPPERS_LOOP(j)
- e_steps[j] += advance_factor * current_block->step_event_count / current_block->mix_event_count[j];
- #else
-
- e_steps[TOOL_E_INDEX] += advance_factor;
- #endif
-
- old_advance = advance_whole;
-
- #endif
-
- #if ENABLED(ADVANCE) || ENABLED(LIN_ADVANCE)
- eISR_Rate = (timer >> 2) * step_loops / abs(e_steps[TOOL_E_INDEX]);
- #endif
- }
- else if (step_events_completed > (uint32_t)current_block->decelerate_after) {
- MultiU24X32toH16(step_rate, deceleration_time, current_block->acceleration_rate);
-
- if (step_rate < acc_step_rate) {
- step_rate = acc_step_rate - step_rate;
- NOLESS(step_rate, current_block->final_rate);
- }
- else
- step_rate = current_block->final_rate;
-
-
- timer = calc_timer(step_rate);
- OCR1A = timer;
- deceleration_time += timer;
-
- #if ENABLED(LIN_ADVANCE)
-
- if (current_block->use_advance_lead) {
- #if ENABLED(MIXING_EXTRUDER)
- MIXING_STEPPERS_LOOP(j)
- current_estep_rate[j] = ((uint32_t)step_rate * current_block->e_speed_multiplier8 * current_block->step_event_count / current_block->mix_event_count[j]) >> 8;
- #else
- current_estep_rate[TOOL_E_INDEX] = ((uint32_t)step_rate * current_block->e_speed_multiplier8) >> 8;
- #endif
- }
-
- #elif ENABLED(ADVANCE)
-
- advance -= advance_rate * step_loops;
- NOLESS(advance, final_advance);
-
-
- long advance_whole = advance >> 8,
- advance_factor = advance_whole - old_advance;
-
- #if ENABLED(MIXING_EXTRUDER)
- MIXING_STEPPERS_LOOP(j)
- e_steps[j] += advance_factor * current_block->step_event_count / current_block->mix_event_count[j];
- #else
- e_steps[TOOL_E_INDEX] += advance_factor;
- #endif
-
- old_advance = advance_whole;
-
- #endif
-
- #if ENABLED(ADVANCE) || ENABLED(LIN_ADVANCE)
- eISR_Rate = (timer >> 2) * step_loops / abs(e_steps[TOOL_E_INDEX]);
- #endif
- }
- else {
-
- #if ENABLED(LIN_ADVANCE)
-
- if (current_block->use_advance_lead)
- current_estep_rate[TOOL_E_INDEX] = final_estep_rate;
-
- eISR_Rate = (OCR1A_nominal >> 2) * step_loops_nominal / abs(e_steps[TOOL_E_INDEX]);
-
- #endif
-
- OCR1A = OCR1A_nominal;
-
- step_loops = step_loops_nominal;
- }
-
- NOLESS(OCR1A, TCNT1 + 16);
-
-
- if (all_steps_done) {
- current_block = NULL;
- planner.discard_current_block();
- }
- }
-
- #if ENABLED(ADVANCE) || ENABLED(LIN_ADVANCE)
-
-
-
- ISR(TIMER0_COMPA_vect) { Stepper::advance_isr(); }
-
- void Stepper::advance_isr() {
-
- old_OCR0A += eISR_Rate;
- OCR0A = old_OCR0A;
-
- #define SET_E_STEP_DIR(INDEX) \
- if (e_steps[INDEX]) E## INDEX ##_DIR_WRITE(e_steps[INDEX] < 0 ? INVERT_E## INDEX ##_DIR : !INVERT_E## INDEX ##_DIR)
-
- #define START_E_PULSE(INDEX) \
- if (e_steps[INDEX]) E## INDEX ##_STEP_WRITE(!INVERT_E_STEP_PIN)
-
- #define STOP_E_PULSE(INDEX) \
- if (e_steps[INDEX]) { \
- e_steps[INDEX] < 0 ? ++e_steps[INDEX] : --e_steps[INDEX]; \
- E## INDEX ##_STEP_WRITE(INVERT_E_STEP_PIN); \
- }
-
- SET_E_STEP_DIR(0);
- #if E_STEPPERS > 1
- SET_E_STEP_DIR(1);
- #if E_STEPPERS > 2
- SET_E_STEP_DIR(2);
- #if E_STEPPERS > 3
- SET_E_STEP_DIR(3);
- #endif
- #endif
- #endif
-
- #define CYCLES_EATEN_BY_E 60
-
-
- for (uint8_t i = 0; i < step_loops; i++) {
-
- #if STEP_PULSE_CYCLES > CYCLES_EATEN_BY_E
- static uint32_t pulse_start;
- pulse_start = TCNT0;
- #endif
-
- START_E_PULSE(0);
- #if E_STEPPERS > 1
- START_E_PULSE(1);
- #if E_STEPPERS > 2
- START_E_PULSE(2);
- #if E_STEPPERS > 3
- START_E_PULSE(3);
- #endif
- #endif
- #endif
-
-
- #if STEP_PULSE_CYCLES > CYCLES_EATEN_BY_E
- while ((uint32_t)(TCNT0 - pulse_start) < STEP_PULSE_CYCLES - CYCLES_EATEN_BY_E) { }
- #endif
-
- STOP_E_PULSE(0);
- #if E_STEPPERS > 1
- STOP_E_PULSE(1);
- #if E_STEPPERS > 2
- STOP_E_PULSE(2);
- #if E_STEPPERS > 3
- STOP_E_PULSE(3);
- #endif
- #endif
- #endif
- }
-
- }
-
- #endif
-
- void Stepper::init() {
-
-
- #if HAS_DIGIPOTSS || HAS_MOTOR_CURRENT_PWM
- digipot_init();
- #endif
-
-
- #if HAS_MICROSTEPS
- microstep_init();
- #endif
-
-
- #if ENABLED(HAVE_TMCDRIVER)
- tmc_init();
- #endif
-
-
- #if ENABLED(HAVE_L6470DRIVER)
- L6470_init();
- #endif
-
-
- #if HAS_X_DIR
- X_DIR_INIT;
- #endif
- #if HAS_X2_DIR
- X2_DIR_INIT;
- #endif
- #if HAS_Y_DIR
- Y_DIR_INIT;
- #if ENABLED(Y_DUAL_STEPPER_DRIVERS) && HAS_Y2_DIR
- Y2_DIR_INIT;
- #endif
- #endif
- #if HAS_Z_DIR
- Z_DIR_INIT;
- #if ENABLED(Z_DUAL_STEPPER_DRIVERS) && HAS_Z2_DIR
- Z2_DIR_INIT;
- #endif
- #endif
- #if HAS_E0_DIR
- E0_DIR_INIT;
- #endif
- #if HAS_E1_DIR
- E1_DIR_INIT;
- #endif
- #if HAS_E2_DIR
- E2_DIR_INIT;
- #endif
- #if HAS_E3_DIR
- E3_DIR_INIT;
- #endif
-
-
- #if HAS_X_ENABLE
- X_ENABLE_INIT;
- if (!X_ENABLE_ON) X_ENABLE_WRITE(HIGH);
- #if ENABLED(DUAL_X_CARRIAGE) && HAS_X2_ENABLE
- X2_ENABLE_INIT;
- if (!X_ENABLE_ON) X2_ENABLE_WRITE(HIGH);
- #endif
- #endif
- #if HAS_Y_ENABLE
- Y_ENABLE_INIT;
- if (!Y_ENABLE_ON) Y_ENABLE_WRITE(HIGH);
- #if ENABLED(Y_DUAL_STEPPER_DRIVERS) && HAS_Y2_ENABLE
- Y2_ENABLE_INIT;
- if (!Y_ENABLE_ON) Y2_ENABLE_WRITE(HIGH);
- #endif
- #endif
- #if HAS_Z_ENABLE
- Z_ENABLE_INIT;
- if (!Z_ENABLE_ON) Z_ENABLE_WRITE(HIGH);
- #if ENABLED(Z_DUAL_STEPPER_DRIVERS) && HAS_Z2_ENABLE
- Z2_ENABLE_INIT;
- if (!Z_ENABLE_ON) Z2_ENABLE_WRITE(HIGH);
- #endif
- #endif
- #if HAS_E0_ENABLE
- E0_ENABLE_INIT;
- if (!E_ENABLE_ON) E0_ENABLE_WRITE(HIGH);
- #endif
- #if HAS_E1_ENABLE
- E1_ENABLE_INIT;
- if (!E_ENABLE_ON) E1_ENABLE_WRITE(HIGH);
- #endif
- #if HAS_E2_ENABLE
- E2_ENABLE_INIT;
- if (!E_ENABLE_ON) E2_ENABLE_WRITE(HIGH);
- #endif
- #if HAS_E3_ENABLE
- E3_ENABLE_INIT;
- if (!E_ENABLE_ON) E3_ENABLE_WRITE(HIGH);
- #endif
-
-
- endstops.init();
-
- #define _STEP_INIT(AXIS) AXIS ##_STEP_INIT
- #define _WRITE_STEP(AXIS, HIGHLOW) AXIS ##_STEP_WRITE(HIGHLOW)
- #define _DISABLE(axis) disable_## axis()
-
- #define AXIS_INIT(axis, AXIS, PIN) \
- _STEP_INIT(AXIS); \
- _WRITE_STEP(AXIS, _INVERT_STEP_PIN(PIN)); \
- _DISABLE(axis)
-
- #define E_AXIS_INIT(NUM) AXIS_INIT(e## NUM, E## NUM, E)
-
-
- #if HAS_X_STEP
- #if ENABLED(X_DUAL_STEPPER_DRIVERS) || ENABLED(DUAL_X_CARRIAGE)
- X2_STEP_INIT;
- X2_STEP_WRITE(INVERT_X_STEP_PIN);
- #endif
- AXIS_INIT(x, X, X);
- #endif
-
- #if HAS_Y_STEP
- #if ENABLED(Y_DUAL_STEPPER_DRIVERS)
- Y2_STEP_INIT;
- Y2_STEP_WRITE(INVERT_Y_STEP_PIN);
- #endif
- AXIS_INIT(y, Y, Y);
- #endif
-
- #if HAS_Z_STEP
- #if ENABLED(Z_DUAL_STEPPER_DRIVERS)
- Z2_STEP_INIT;
- Z2_STEP_WRITE(INVERT_Z_STEP_PIN);
- #endif
- AXIS_INIT(z, Z, Z);
- #endif
-
- #if HAS_E0_STEP
- E_AXIS_INIT(0);
- #endif
- #if HAS_E1_STEP
- E_AXIS_INIT(1);
- #endif
- #if HAS_E2_STEP
- E_AXIS_INIT(2);
- #endif
- #if HAS_E3_STEP
- E_AXIS_INIT(3);
- #endif
-
-
- CBI(TCCR1B, WGM13);
- SBI(TCCR1B, WGM12);
- CBI(TCCR1A, WGM11);
- CBI(TCCR1A, WGM10);
-
-
- TCCR1A &= ~(3 << COM1A0);
- TCCR1A &= ~(3 << COM1B0);
-
-
-
-
-
- TCCR1B = (TCCR1B & ~(0x07 << CS10)) | (2 << CS10);
-
- OCR1A = 0x4000;
- TCNT1 = 0;
- ENABLE_STEPPER_DRIVER_INTERRUPT();
-
- #if ENABLED(ADVANCE) || ENABLED(LIN_ADVANCE)
-
- for (int i = 0; i < E_STEPPERS; i++) {
- e_steps[i] = 0;
- #if ENABLED(LIN_ADVANCE)
- current_adv_steps[i] = 0;
- #endif
- }
-
- #if defined(TCCR0A) && defined(WGM01)
- CBI(TCCR0A, WGM01);
- CBI(TCCR0A, WGM00);
- #endif
- SBI(TIMSK0, OCIE0A);
-
- #endif
-
- endstops.enable(true);
- sei();
-
- set_directions();
- }
-
-
-
- void Stepper::synchronize() { while (planner.blocks_queued()) idle(); }
-
-
- void Stepper::set_position(const long& x, const long& y, const long& z, const long& e) {
-
- synchronize();
-
- CRITICAL_SECTION_START;
-
- #if ENABLED(COREXY)
-
-
- count_position[A_AXIS] = x + y;
- count_position[B_AXIS] = x - y;
- count_position[Z_AXIS] = z;
- #elif ENABLED(COREXZ)
-
- count_position[A_AXIS] = x + z;
- count_position[Y_AXIS] = y;
- count_position[C_AXIS] = x - z;
- #elif ENABLED(COREYZ)
-
- count_position[X_AXIS] = x;
- count_position[B_AXIS] = y + z;
- count_position[C_AXIS] = y - z;
- #else
-
- count_position[X_AXIS] = x;
- count_position[Y_AXIS] = y;
- count_position[Z_AXIS] = z;
- #endif
-
- count_position[E_AXIS] = e;
- CRITICAL_SECTION_END;
- }
-
- void Stepper::set_position(const AxisEnum &axis, const long& v) {
- CRITICAL_SECTION_START;
- count_position[axis] = v;
- CRITICAL_SECTION_END;
- }
-
- void Stepper::set_e_position(const long& e) {
- CRITICAL_SECTION_START;
- count_position[E_AXIS] = e;
- CRITICAL_SECTION_END;
- }
-
-
- long Stepper::position(AxisEnum axis) {
- CRITICAL_SECTION_START;
- long count_pos = count_position[axis];
- CRITICAL_SECTION_END;
- return count_pos;
- }
-
-
- float Stepper::get_axis_position_mm(AxisEnum axis) {
- float axis_steps;
- #if ENABLED(COREXY) || ENABLED(COREXZ) || ENABLED(COREYZ)
-
- if (axis == CORE_AXIS_1 || axis == CORE_AXIS_2) {
- CRITICAL_SECTION_START;
- long pos1 = count_position[CORE_AXIS_1],
- pos2 = count_position[CORE_AXIS_2];
- CRITICAL_SECTION_END;
-
-
- axis_steps = (pos1 + ((axis == CORE_AXIS_1) ? pos2 : -pos2)) * 0.5f;
- }
- else
- axis_steps = position(axis);
- #else
- axis_steps = position(axis);
- #endif
- return axis_steps * planner.steps_to_mm[axis];
- }
-
- void Stepper::finish_and_disable() {
- synchronize();
- disable_all_steppers();
- }
-
- void Stepper::quick_stop() {
- cleaning_buffer_counter = 5000;
- DISABLE_STEPPER_DRIVER_INTERRUPT();
- while (planner.blocks_queued()) planner.discard_current_block();
- current_block = NULL;
- ENABLE_STEPPER_DRIVER_INTERRUPT();
- }
-
- void Stepper::endstop_triggered(AxisEnum axis) {
-
- #if ENABLED(COREXY) || ENABLED(COREXZ) || ENABLED(COREYZ)
-
- float axis_pos = count_position[axis];
- if (axis == CORE_AXIS_1)
- axis_pos = (axis_pos + count_position[CORE_AXIS_2]) * 0.5;
- else if (axis == CORE_AXIS_2)
- axis_pos = (count_position[CORE_AXIS_1] - axis_pos) * 0.5;
- endstops_trigsteps[axis] = axis_pos;
-
- #else
-
- endstops_trigsteps[axis] = count_position[axis];
-
- #endif
-
- kill_current_block();
- }
-
- void Stepper::report_positions() {
- CRITICAL_SECTION_START;
- long xpos = count_position[X_AXIS],
- ypos = count_position[Y_AXIS],
- zpos = count_position[Z_AXIS];
- CRITICAL_SECTION_END;
-
- #if ENABLED(COREXY) || ENABLED(COREXZ) || IS_SCARA
- SERIAL_PROTOCOLPGM(MSG_COUNT_A);
- #else
- SERIAL_PROTOCOLPGM(MSG_COUNT_X);
- #endif
- SERIAL_PROTOCOL(xpos);
-
- #if ENABLED(COREXY) || ENABLED(COREYZ) || IS_SCARA
- SERIAL_PROTOCOLPGM(" B:");
- #else
- SERIAL_PROTOCOLPGM(" Y:");
- #endif
- SERIAL_PROTOCOL(ypos);
-
- #if ENABLED(COREXZ) || ENABLED(COREYZ)
- SERIAL_PROTOCOLPGM(" C:");
- #else
- SERIAL_PROTOCOLPGM(" Z:");
- #endif
- SERIAL_PROTOCOL(zpos);
-
- SERIAL_EOL;
- }
-
- #if ENABLED(BABYSTEPPING)
-
-
-
- void Stepper::babystep(const uint8_t axis, const bool direction) {
-
- #define _ENABLE(axis) enable_## axis()
- #define _READ_DIR(AXIS) AXIS ##_DIR_READ
- #define _INVERT_DIR(AXIS) INVERT_## AXIS ##_DIR
- #define _APPLY_DIR(AXIS, INVERT) AXIS ##_APPLY_DIR(INVERT, true)
-
- #define BABYSTEP_AXIS(axis, AXIS, INVERT) { \
- _ENABLE(axis); \
- uint8_t old_pin = _READ_DIR(AXIS); \
- _APPLY_DIR(AXIS, _INVERT_DIR(AXIS)^direction^INVERT); \
- _APPLY_STEP(AXIS)(!_INVERT_STEP_PIN(AXIS), true); \
- delayMicroseconds(2); \
- _APPLY_STEP(AXIS)(_INVERT_STEP_PIN(AXIS), true); \
- _APPLY_DIR(AXIS, old_pin); \
- }
-
- switch (axis) {
-
- case X_AXIS:
- BABYSTEP_AXIS(x, X, false);
- break;
-
- case Y_AXIS:
- BABYSTEP_AXIS(y, Y, false);
- break;
-
- case Z_AXIS: {
-
- #if DISABLED(DELTA)
-
- BABYSTEP_AXIS(z, Z, BABYSTEP_INVERT_Z);
-
- #else
-
- bool z_direction = direction ^ BABYSTEP_INVERT_Z;
-
- enable_x();
- enable_y();
- enable_z();
- uint8_t old_x_dir_pin = X_DIR_READ,
- old_y_dir_pin = Y_DIR_READ,
- old_z_dir_pin = Z_DIR_READ;
-
- X_DIR_WRITE(INVERT_X_DIR ^ z_direction);
- Y_DIR_WRITE(INVERT_Y_DIR ^ z_direction);
- Z_DIR_WRITE(INVERT_Z_DIR ^ z_direction);
-
- X_STEP_WRITE(!INVERT_X_STEP_PIN);
- Y_STEP_WRITE(!INVERT_Y_STEP_PIN);
- Z_STEP_WRITE(!INVERT_Z_STEP_PIN);
- delayMicroseconds(2);
- X_STEP_WRITE(INVERT_X_STEP_PIN);
- Y_STEP_WRITE(INVERT_Y_STEP_PIN);
- Z_STEP_WRITE(INVERT_Z_STEP_PIN);
-
- X_DIR_WRITE(old_x_dir_pin);
- Y_DIR_WRITE(old_y_dir_pin);
- Z_DIR_WRITE(old_z_dir_pin);
-
- #endif
-
- } break;
-
- default: break;
- }
- }
-
- #endif
-
-
-
- #if HAS_DIGIPOTSS
-
-
- void Stepper::digitalPotWrite(int address, int value) {
- WRITE(DIGIPOTSS_PIN, LOW);
- SPI.transfer(address);
- SPI.transfer(value);
- WRITE(DIGIPOTSS_PIN, HIGH);
-
- }
-
- #endif
-
- #if HAS_DIGIPOTSS || HAS_MOTOR_CURRENT_PWM
-
- void Stepper::digipot_init() {
- #if HAS_DIGIPOTSS
- static const uint8_t digipot_motor_current[] = DIGIPOT_MOTOR_CURRENT;
- SPI.begin();
- SET_OUTPUT(DIGIPOTSS_PIN);
- for (uint8_t i = 0; i < COUNT(digipot_motor_current); i++) {
-
- digipot_current(i, digipot_motor_current[i]);
- }
- #elif HAS_MOTOR_CURRENT_PWM
- #if PIN_EXISTS(MOTOR_CURRENT_PWM_XY)
- SET_OUTPUT(MOTOR_CURRENT_PWM_XY_PIN);
- digipot_current(0, motor_current_setting[0]);
- #endif
- #if PIN_EXISTS(MOTOR_CURRENT_PWM_Z)
- SET_OUTPUT(MOTOR_CURRENT_PWM_Z_PIN);
- digipot_current(1, motor_current_setting[1]);
- #endif
- #if PIN_EXISTS(MOTOR_CURRENT_PWM_E)
- SET_OUTPUT(MOTOR_CURRENT_PWM_E_PIN);
- digipot_current(2, motor_current_setting[2]);
- #endif
-
- TCCR5B = (TCCR5B & ~(_BV(CS50) | _BV(CS51) | _BV(CS52))) | _BV(CS50);
- #endif
- }
-
- void Stepper::digipot_current(uint8_t driver, int current) {
- #if HAS_DIGIPOTSS
- const uint8_t digipot_ch[] = DIGIPOT_CHANNELS;
- digitalPotWrite(digipot_ch[driver], current);
- #elif HAS_MOTOR_CURRENT_PWM
- #define _WRITE_CURRENT_PWM(P) analogWrite(P, 255L * current / (MOTOR_CURRENT_PWM_RANGE))
- switch (driver) {
- #if PIN_EXISTS(MOTOR_CURRENT_PWM_XY)
- case 0: _WRITE_CURRENT_PWM(MOTOR_CURRENT_PWM_XY_PIN); break;
- #endif
- #if PIN_EXISTS(MOTOR_CURRENT_PWM_Z)
- case 1: _WRITE_CURRENT_PWM(MOTOR_CURRENT_PWM_Z_PIN); break;
- #endif
- #if PIN_EXISTS(MOTOR_CURRENT_PWM_E)
- case 2: _WRITE_CURRENT_PWM(MOTOR_CURRENT_PWM_E_PIN); break;
- #endif
- }
- #endif
- }
-
- #endif
-
- #if HAS_MICROSTEPS
-
-
-
-
- void Stepper::microstep_init() {
- SET_OUTPUT(X_MS1_PIN);
- SET_OUTPUT(X_MS2_PIN);
- #if HAS_MICROSTEPS_Y
- SET_OUTPUT(Y_MS1_PIN);
- SET_OUTPUT(Y_MS2_PIN);
- #endif
- #if HAS_MICROSTEPS_Z
- SET_OUTPUT(Z_MS1_PIN);
- SET_OUTPUT(Z_MS2_PIN);
- #endif
- #if HAS_MICROSTEPS_E0
- SET_OUTPUT(E0_MS1_PIN);
- SET_OUTPUT(E0_MS2_PIN);
- #endif
- #if HAS_MICROSTEPS_E1
- SET_OUTPUT(E1_MS1_PIN);
- SET_OUTPUT(E1_MS2_PIN);
- #endif
- static const uint8_t microstep_modes[] = MICROSTEP_MODES;
- for (uint16_t i = 0; i < COUNT(microstep_modes); i++)
- microstep_mode(i, microstep_modes[i]);
- }
-
- void Stepper::microstep_ms(uint8_t driver, int8_t ms1, int8_t ms2) {
- if (ms1 >= 0) switch (driver) {
- case 0: digitalWrite(X_MS1_PIN, ms1); break;
- #if HAS_MICROSTEPS_Y
- case 1: digitalWrite(Y_MS1_PIN, ms1); break;
- #endif
- #if HAS_MICROSTEPS_Z
- case 2: digitalWrite(Z_MS1_PIN, ms1); break;
- #endif
- #if HAS_MICROSTEPS_E0
- case 3: digitalWrite(E0_MS1_PIN, ms1); break;
- #endif
- #if HAS_MICROSTEPS_E1
- case 4: digitalWrite(E1_MS1_PIN, ms1); break;
- #endif
- }
- if (ms2 >= 0) switch (driver) {
- case 0: digitalWrite(X_MS2_PIN, ms2); break;
- #if HAS_MICROSTEPS_Y
- case 1: digitalWrite(Y_MS2_PIN, ms2); break;
- #endif
- #if HAS_MICROSTEPS_Z
- case 2: digitalWrite(Z_MS2_PIN, ms2); break;
- #endif
- #if HAS_MICROSTEPS_E0
- case 3: digitalWrite(E0_MS2_PIN, ms2); break;
- #endif
- #if HAS_MICROSTEPS_E1
- case 4: digitalWrite(E1_MS2_PIN, ms2); break;
- #endif
- }
- }
-
- void Stepper::microstep_mode(uint8_t driver, uint8_t stepping_mode) {
- switch (stepping_mode) {
- case 1: microstep_ms(driver, MICROSTEP1); break;
- case 2: microstep_ms(driver, MICROSTEP2); break;
- case 4: microstep_ms(driver, MICROSTEP4); break;
- case 8: microstep_ms(driver, MICROSTEP8); break;
- case 16: microstep_ms(driver, MICROSTEP16); break;
- }
- }
-
- void Stepper::microstep_readings() {
- SERIAL_PROTOCOLLNPGM("MS1,MS2 Pins");
- SERIAL_PROTOCOLPGM("X: ");
- SERIAL_PROTOCOL(READ(X_MS1_PIN));
- SERIAL_PROTOCOLLN(READ(X_MS2_PIN));
- #if HAS_MICROSTEPS_Y
- SERIAL_PROTOCOLPGM("Y: ");
- SERIAL_PROTOCOL(READ(Y_MS1_PIN));
- SERIAL_PROTOCOLLN(READ(Y_MS2_PIN));
- #endif
- #if HAS_MICROSTEPS_Z
- SERIAL_PROTOCOLPGM("Z: ");
- SERIAL_PROTOCOL(READ(Z_MS1_PIN));
- SERIAL_PROTOCOLLN(READ(Z_MS2_PIN));
- #endif
- #if HAS_MICROSTEPS_E0
- SERIAL_PROTOCOLPGM("E0: ");
- SERIAL_PROTOCOL(READ(E0_MS1_PIN));
- SERIAL_PROTOCOLLN(READ(E0_MS2_PIN));
- #endif
- #if HAS_MICROSTEPS_E1
- SERIAL_PROTOCOLPGM("E1: ");
- SERIAL_PROTOCOL(READ(E1_MS1_PIN));
- SERIAL_PROTOCOLLN(READ(E1_MS2_PIN));
- #endif
- }
-
- #endif
-
- #if ENABLED(LIN_ADVANCE)
-
- void Stepper::advance_M905(const float &k) {
- if (k >= 0) extruder_advance_k = k;
- SERIAL_ECHO_START;
- SERIAL_ECHOPAIR("Advance factor: ", extruder_advance_k);
- SERIAL_EOL;
- }
-
- #endif
|