stepper.cpp 52 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826827828829830831832833834835836837838839840841842843844845846847848849850851852853854855856857858859860861862863864865866867868869870871872873874875876877878879880881882883884885886887888889890891892893894895896897898899900901902903904905906907908909910911912913914915916917918919920921922923924925926927928929930931932933934935936937938939940941942943944945946947948949950951952953954955956957958959960961962963964965966967968969970971972973974975976977978979980981982983984985986987988989990991992993994995996997998999100010011002100310041005100610071008100910101011101210131014101510161017101810191020102110221023102410251026102710281029103010311032103310341035103610371038103910401041104210431044104510461047104810491050105110521053105410551056105710581059106010611062106310641065106610671068106910701071107210731074107510761077107810791080108110821083108410851086108710881089109010911092109310941095109610971098109911001101110211031104110511061107110811091110111111121113111411151116111711181119112011211122112311241125112611271128112911301131113211331134113511361137113811391140114111421143114411451146114711481149115011511152115311541155115611571158115911601161116211631164116511661167116811691170117111721173117411751176117711781179118011811182118311841185118611871188118911901191119211931194119511961197119811991200120112021203120412051206120712081209121012111212121312141215121612171218121912201221122212231224122512261227122812291230123112321233123412351236123712381239124012411242124312441245124612471248124912501251125212531254125512561257125812591260126112621263126412651266126712681269127012711272127312741275127612771278127912801281128212831284128512861287128812891290129112921293129412951296129712981299130013011302130313041305130613071308130913101311131213131314131513161317131813191320132113221323132413251326132713281329133013311332133313341335133613371338133913401341134213431344134513461347134813491350135113521353135413551356135713581359136013611362136313641365136613671368136913701371137213731374137513761377137813791380138113821383138413851386138713881389139013911392139313941395139613971398139914001401140214031404140514061407140814091410141114121413141414151416141714181419142014211422142314241425142614271428142914301431143214331434143514361437143814391440144114421443144414451446144714481449145014511452145314541455145614571458145914601461146214631464146514661467146814691470147114721473147414751476147714781479148014811482148314841485148614871488148914901491149214931494149514961497149814991500150115021503150415051506150715081509151015111512151315141515151615171518151915201521152215231524152515261527152815291530153115321533153415351536153715381539154015411542154315441545154615471548154915501551155215531554155515561557155815591560156115621563156415651566156715681569157015711572157315741575157615771578157915801581158215831584158515861587158815891590159115921593159415951596159715981599160016011602160316041605160616071608160916101611161216131614161516161617161816191620162116221623162416251626162716281629163016311632163316341635163616371638163916401641164216431644164516461647164816491650165116521653165416551656165716581659166016611662166316641665166616671668166916701671167216731674167516761677167816791680168116821683168416851686168716881689169016911692169316941695
  1. /*
  2. stepper.c - stepper motor driver: executes motion plans using stepper motors
  3. Part of Grbl
  4. Copyright (c) 2009-2011 Simen Svale Skogsrud
  5. Grbl is free software: you can redistribute it and/or modify
  6. it under the terms of the GNU General Public License as published by
  7. the Free Software Foundation, either version 3 of the License, or
  8. (at your option) any later version.
  9. Grbl is distributed in the hope that it will be useful,
  10. but WITHOUT ANY WARRANTY; without even the implied warranty of
  11. MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  12. GNU General Public License for more details.
  13. You should have received a copy of the GNU General Public License
  14. along with Grbl. If not, see <http://www.gnu.org/licenses/>.
  15. */
  16. /* The timer calculations of this module informed by the 'RepRap cartesian firmware' by Zack Smith
  17. and Philipp Tiefenbacher. */
  18. #include "Marlin.h"
  19. #include "stepper.h"
  20. #include "planner.h"
  21. #include "temperature.h"
  22. #include "ultralcd.h"
  23. #include "language.h"
  24. #include "cardreader.h"
  25. #include "speed_lookuptable.h"
  26. #if defined(DIGIPOTSS_PIN) && DIGIPOTSS_PIN > -1
  27. #include <SPI.h>
  28. #endif
  29. #ifdef TMC2130
  30. #include "tmc2130.h"
  31. #endif //TMC2130
  32. #if defined(FILAMENT_SENSOR) && defined(PAT9125)
  33. #include "fsensor.h"
  34. int fsensor_counter; //counter for e-steps
  35. #endif //FILAMENT_SENSOR
  36. #include "mmu.h"
  37. #include "ConfigurationStore.h"
  38. #ifdef DEBUG_STACK_MONITOR
  39. uint16_t SP_min = 0x21FF;
  40. #endif //DEBUG_STACK_MONITOR
  41. /*
  42. * Stepping macros
  43. */
  44. #define _STEP_PIN_X_AXIS X_STEP_PIN
  45. #define _STEP_PIN_Y_AXIS Y_STEP_PIN
  46. #define _STEP_PIN_Z_AXIS Z_STEP_PIN
  47. #define _STEP_PIN_E_AXIS E0_STEP_PIN
  48. #ifdef DEBUG_XSTEP_DUP_PIN
  49. #define _STEP_PIN_X_DUP_AXIS DEBUG_XSTEP_DUP_PIN
  50. #endif
  51. #ifdef DEBUG_YSTEP_DUP_PIN
  52. #define _STEP_PIN_Y_DUP_AXIS DEBUG_YSTEP_DUP_PIN
  53. #endif
  54. #ifdef Y_DUAL_STEPPER_DRIVERS
  55. #error Y_DUAL_STEPPER_DRIVERS not fully implemented
  56. #define _STEP_PIN_Y2_AXIS Y2_STEP_PIN
  57. #endif
  58. #ifdef Z_DUAL_STEPPER_DRIVERS
  59. #error Z_DUAL_STEPPER_DRIVERS not fully implemented
  60. #define _STEP_PIN_Z2_AXIS Z2_STEP_PIN
  61. #endif
  62. #ifdef TMC2130
  63. #define STEPPER_MINIMUM_PULSE TMC2130_MINIMUM_PULSE
  64. #define STEPPER_SET_DIR_DELAY TMC2130_SET_DIR_DELAY
  65. #define STEPPER_MINIMUM_DELAY TMC2130_MINIMUM_DELAY
  66. #else
  67. #define STEPPER_MINIMUM_PULSE 2
  68. #define STEPPER_SET_DIR_DELAY 100
  69. #define STEPPER_MINIMUM_DELAY delayMicroseconds(STEPPER_MINIMUM_PULSE)
  70. #endif
  71. #ifdef TMC2130_DEDGE_STEPPING
  72. static_assert(TMC2130_MINIMUM_DELAY 1, // this will fail to compile when non-empty
  73. "DEDGE implies/requires an empty TMC2130_MINIMUM_DELAY");
  74. #define STEP_NC_HI(axis) TOGGLE(_STEP_PIN_##axis)
  75. #define STEP_NC_LO(axis) //NOP
  76. #else
  77. #define _STEP_HI_X_AXIS !INVERT_X_STEP_PIN
  78. #define _STEP_LO_X_AXIS INVERT_X_STEP_PIN
  79. #define _STEP_HI_Y_AXIS !INVERT_Y_STEP_PIN
  80. #define _STEP_LO_Y_AXIS INVERT_Y_STEP_PIN
  81. #define _STEP_HI_Z_AXIS !INVERT_Z_STEP_PIN
  82. #define _STEP_LO_Z_AXIS INVERT_Z_STEP_PIN
  83. #define _STEP_HI_E_AXIS !INVERT_E_STEP_PIN
  84. #define _STEP_LO_E_AXIS INVERT_E_STEP_PIN
  85. #define STEP_NC_HI(axis) WRITE_NC(_STEP_PIN_##axis, _STEP_HI_##axis)
  86. #define STEP_NC_LO(axis) WRITE_NC(_STEP_PIN_##axis, _STEP_LO_##axis)
  87. #endif //TMC2130_DEDGE_STEPPING
  88. //===========================================================================
  89. //=============================public variables ============================
  90. //===========================================================================
  91. block_t *current_block; // A pointer to the block currently being traced
  92. bool x_min_endstop = false;
  93. bool x_max_endstop = false;
  94. bool y_min_endstop = false;
  95. bool y_max_endstop = false;
  96. bool z_min_endstop = false;
  97. bool z_max_endstop = false;
  98. //===========================================================================
  99. //=============================private variables ============================
  100. //===========================================================================
  101. //static makes it inpossible to be called from outside of this file by extern.!
  102. // Variables used by The Stepper Driver Interrupt
  103. static unsigned char out_bits; // The next stepping-bits to be output
  104. static dda_isteps_t
  105. counter_x, // Counter variables for the bresenham line tracer
  106. counter_y,
  107. counter_z,
  108. counter_e;
  109. volatile dda_usteps_t step_events_completed; // The number of step events executed in the current block
  110. static uint32_t acceleration_time, deceleration_time;
  111. static uint16_t acc_step_rate; // needed for deccelaration start point
  112. static uint8_t step_loops;
  113. static uint16_t OCR1A_nominal;
  114. static uint8_t step_loops_nominal;
  115. volatile long endstops_trigsteps[3]={0,0,0};
  116. static volatile bool endstop_x_hit=false;
  117. static volatile bool endstop_y_hit=false;
  118. static volatile bool endstop_z_hit=false;
  119. #ifdef ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED
  120. bool abort_on_endstop_hit = false;
  121. #endif
  122. #ifdef MOTOR_CURRENT_PWM_XY_PIN
  123. int motor_current_setting[3] = DEFAULT_PWM_MOTOR_CURRENT;
  124. int motor_current_setting_silent[3] = DEFAULT_PWM_MOTOR_CURRENT;
  125. int motor_current_setting_loud[3] = DEFAULT_PWM_MOTOR_CURRENT_LOUD;
  126. #endif
  127. #if ( (defined(X_MAX_PIN) && (X_MAX_PIN > -1)) || defined(TMC2130_SG_HOMING) ) && !defined(DEBUG_DISABLE_XMAXLIMIT)
  128. static bool old_x_max_endstop=false;
  129. #endif
  130. #if ( (defined(Y_MAX_PIN) && (Y_MAX_PIN > -1)) || defined(TMC2130_SG_HOMING) ) && !defined(DEBUG_DISABLE_YMAXLIMIT)
  131. static bool old_y_max_endstop=false;
  132. #endif
  133. static bool old_x_min_endstop=false;
  134. static bool old_y_min_endstop=false;
  135. static bool old_z_min_endstop=false;
  136. static bool old_z_max_endstop=false;
  137. static bool check_endstops = true;
  138. static bool check_z_endstop = false;
  139. static bool z_endstop_invert = false;
  140. volatile long count_position[NUM_AXIS] = { 0, 0, 0, 0};
  141. volatile signed char count_direction[NUM_AXIS] = { 1, 1, 1, 1};
  142. #ifdef LIN_ADVANCE
  143. void advance_isr_scheduler();
  144. void advance_isr();
  145. static const uint16_t ADV_NEVER = 0xFFFF;
  146. static const uint8_t ADV_INIT = 0b01; // initialize LA
  147. static const uint8_t ADV_ACC_VARY = 0b10; // varying acceleration phase
  148. static uint16_t nextMainISR;
  149. static uint16_t nextAdvanceISR;
  150. static uint16_t main_Rate;
  151. static uint16_t eISR_Rate;
  152. static uint32_t eISR_Err;
  153. static uint16_t current_adv_steps;
  154. static uint16_t target_adv_steps;
  155. static int8_t e_steps; // scheduled e-steps during each isr loop
  156. static uint8_t e_step_loops; // e-steps to execute at most in each isr loop
  157. static uint8_t e_extruding; // current move is an extrusion move
  158. static int8_t LA_phase; // LA compensation phase
  159. #define _NEXT_ISR(T) main_Rate = nextMainISR = T
  160. #else
  161. #define _NEXT_ISR(T) OCR1A = T
  162. #endif
  163. #ifdef DEBUG_STEPPER_TIMER_MISSED
  164. extern bool stepper_timer_overflow_state;
  165. extern uint16_t stepper_timer_overflow_last;
  166. #endif /* DEBUG_STEPPER_TIMER_MISSED */
  167. //===========================================================================
  168. //=============================functions ============================
  169. //===========================================================================
  170. void checkHitEndstops()
  171. {
  172. if( endstop_x_hit || endstop_y_hit || endstop_z_hit) {
  173. SERIAL_ECHO_START;
  174. SERIAL_ECHORPGM(MSG_ENDSTOPS_HIT);
  175. if(endstop_x_hit) {
  176. SERIAL_ECHOPAIR(" X:",(float)endstops_trigsteps[X_AXIS]/cs.axis_steps_per_unit[X_AXIS]);
  177. // LCD_MESSAGERPGM(CAT2((MSG_ENDSTOPS_HIT), PSTR("X")));
  178. }
  179. if(endstop_y_hit) {
  180. SERIAL_ECHOPAIR(" Y:",(float)endstops_trigsteps[Y_AXIS]/cs.axis_steps_per_unit[Y_AXIS]);
  181. // LCD_MESSAGERPGM(CAT2((MSG_ENDSTOPS_HIT), PSTR("Y")));
  182. }
  183. if(endstop_z_hit) {
  184. SERIAL_ECHOPAIR(" Z:",(float)endstops_trigsteps[Z_AXIS]/cs.axis_steps_per_unit[Z_AXIS]);
  185. // LCD_MESSAGERPGM(CAT2((MSG_ENDSTOPS_HIT),PSTR("Z")));
  186. }
  187. SERIAL_ECHOLN("");
  188. endstop_x_hit=false;
  189. endstop_y_hit=false;
  190. endstop_z_hit=false;
  191. #if defined(ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED) && defined(SDSUPPORT)
  192. if (abort_on_endstop_hit)
  193. {
  194. card.sdprinting = false;
  195. card.closefile();
  196. quickStop();
  197. setTargetHotend0(0);
  198. setTargetHotend1(0);
  199. setTargetHotend2(0);
  200. }
  201. #endif
  202. }
  203. }
  204. bool endstops_hit_on_purpose()
  205. {
  206. bool hit = endstop_x_hit || endstop_y_hit || endstop_z_hit;
  207. endstop_x_hit=false;
  208. endstop_y_hit=false;
  209. endstop_z_hit=false;
  210. return hit;
  211. }
  212. bool endstop_z_hit_on_purpose()
  213. {
  214. bool hit = endstop_z_hit;
  215. endstop_z_hit=false;
  216. return hit;
  217. }
  218. bool enable_endstops(bool check)
  219. {
  220. bool old = check_endstops;
  221. check_endstops = check;
  222. return old;
  223. }
  224. bool enable_z_endstop(bool check)
  225. {
  226. bool old = check_z_endstop;
  227. check_z_endstop = check;
  228. endstop_z_hit = false;
  229. return old;
  230. }
  231. void invert_z_endstop(bool endstop_invert)
  232. {
  233. z_endstop_invert = endstop_invert;
  234. }
  235. // __________________________
  236. // /| |\ _________________ ^
  237. // / | | \ /| |\ |
  238. // / | | \ / | | \ s
  239. // / | | | | | \ p
  240. // / | | | | | \ e
  241. // +-----+------------------------+---+--+---------------+----+ e
  242. // | BLOCK 1 | BLOCK 2 | d
  243. //
  244. // time ----->
  245. //
  246. // The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates
  247. // first block->accelerate_until step_events_completed, then keeps going at constant speed until
  248. // step_events_completed reaches block->decelerate_after after which it decelerates until the trapezoid generator is reset.
  249. // The slope of acceleration is calculated using v = u + at where t is the accumulated timer values of the steps so far.
  250. // "The Stepper Driver Interrupt" - This timer interrupt is the workhorse.
  251. // It pops blocks from the block_buffer and executes them by pulsing the stepper pins appropriately.
  252. ISR(TIMER1_COMPA_vect) {
  253. #ifdef DEBUG_STACK_MONITOR
  254. uint16_t sp = SPL + 256 * SPH;
  255. if (sp < SP_min) SP_min = sp;
  256. #endif //DEBUG_STACK_MONITOR
  257. #ifdef LIN_ADVANCE
  258. advance_isr_scheduler();
  259. #else
  260. isr();
  261. #endif
  262. // Don't run the ISR faster than possible
  263. // Is there a 8us time left before the next interrupt triggers?
  264. if (OCR1A < TCNT1 + 16) {
  265. #ifdef DEBUG_STEPPER_TIMER_MISSED
  266. // Verify whether the next planned timer interrupt has not been missed already.
  267. // This debugging test takes < 1.125us
  268. // This skews the profiling slightly as the fastest stepper timer
  269. // interrupt repeats at a 100us rate (10kHz).
  270. if (OCR1A + 40 < TCNT1) {
  271. // The interrupt was delayed by more than 20us (which is 1/5th of the 10kHz ISR repeat rate).
  272. // Give a warning.
  273. stepper_timer_overflow_state = true;
  274. stepper_timer_overflow_last = TCNT1 - OCR1A;
  275. // Beep, the beeper will be cleared at the stepper_timer_overflow() called from the main thread.
  276. WRITE(BEEPER, HIGH);
  277. }
  278. #endif
  279. // Fix the next interrupt to be executed after 8us from now.
  280. OCR1A = TCNT1 + 16;
  281. }
  282. }
  283. uint8_t last_dir_bits = 0;
  284. #ifdef BACKLASH_X
  285. uint8_t st_backlash_x = 0;
  286. #endif //BACKLASH_X
  287. #ifdef BACKLASH_Y
  288. uint8_t st_backlash_y = 0;
  289. #endif //BACKLASH_Y
  290. FORCE_INLINE void stepper_next_block()
  291. {
  292. // Anything in the buffer?
  293. //WRITE_NC(LOGIC_ANALYZER_CH2, true);
  294. current_block = plan_get_current_block();
  295. if (current_block != NULL) {
  296. #ifdef BACKLASH_X
  297. if (current_block->steps_x.wide)
  298. { //X-axis movement
  299. if ((current_block->direction_bits ^ last_dir_bits) & 1)
  300. {
  301. printf_P(PSTR("BL %d\n"), (current_block->direction_bits & 1)?st_backlash_x:-st_backlash_x);
  302. if (current_block->direction_bits & 1)
  303. WRITE_NC(X_DIR_PIN, INVERT_X_DIR);
  304. else
  305. WRITE_NC(X_DIR_PIN, !INVERT_X_DIR);
  306. delayMicroseconds(STEPPER_SET_DIR_DELAY);
  307. for (uint8_t i = 0; i < st_backlash_x; i++)
  308. {
  309. STEP_NC_HI(X_AXIS);
  310. STEPPER_MINIMUM_DELAY;
  311. STEP_NC_LO(X_AXIS);
  312. _delay_us(900); // hard-coded jerk! *bad*
  313. }
  314. }
  315. last_dir_bits &= ~1;
  316. last_dir_bits |= current_block->direction_bits & 1;
  317. }
  318. #endif
  319. #ifdef BACKLASH_Y
  320. if (current_block->steps_y.wide)
  321. { //Y-axis movement
  322. if ((current_block->direction_bits ^ last_dir_bits) & 2)
  323. {
  324. printf_P(PSTR("BL %d\n"), (current_block->direction_bits & 2)?st_backlash_y:-st_backlash_y);
  325. if (current_block->direction_bits & 2)
  326. WRITE_NC(Y_DIR_PIN, INVERT_Y_DIR);
  327. else
  328. WRITE_NC(Y_DIR_PIN, !INVERT_Y_DIR);
  329. delayMicroseconds(STEPPER_SET_DIR_DELAY);
  330. for (uint8_t i = 0; i < st_backlash_y; i++)
  331. {
  332. STEP_NC_HI(Y_AXIS);
  333. STEPPER_MINIMUM_DELAY;
  334. STEP_NC_LO(Y_AXIS);
  335. _delay_us(900); // hard-coded jerk! *bad*
  336. }
  337. }
  338. last_dir_bits &= ~2;
  339. last_dir_bits |= current_block->direction_bits & 2;
  340. }
  341. #endif
  342. // The busy flag is set by the plan_get_current_block() call.
  343. // current_block->busy = true;
  344. // Initializes the trapezoid generator from the current block. Called whenever a new
  345. // block begins.
  346. deceleration_time = 0;
  347. // Set the nominal step loops to zero to indicate, that the timer value is not known yet.
  348. // That means, delay the initialization of nominal step rate and step loops until the steady
  349. // state is reached.
  350. step_loops_nominal = 0;
  351. acc_step_rate = uint16_t(current_block->initial_rate);
  352. acceleration_time = calc_timer(acc_step_rate, step_loops);
  353. #ifdef LIN_ADVANCE
  354. if (current_block->use_advance_lead) {
  355. target_adv_steps = current_block->max_adv_steps;
  356. }
  357. e_steps = 0;
  358. nextAdvanceISR = ADV_NEVER;
  359. LA_phase = -1;
  360. #endif
  361. if (current_block->flag & BLOCK_FLAG_E_RESET) {
  362. count_position[E_AXIS] = 0;
  363. }
  364. if (current_block->flag & BLOCK_FLAG_DDA_LOWRES) {
  365. counter_x.lo = -(current_block->step_event_count.lo >> 1);
  366. counter_y.lo = counter_x.lo;
  367. counter_z.lo = counter_x.lo;
  368. counter_e.lo = counter_x.lo;
  369. #ifdef LIN_ADVANCE
  370. e_extruding = current_block->steps_e.lo != 0;
  371. #endif
  372. } else {
  373. counter_x.wide = -(current_block->step_event_count.wide >> 1);
  374. counter_y.wide = counter_x.wide;
  375. counter_z.wide = counter_x.wide;
  376. counter_e.wide = counter_x.wide;
  377. #ifdef LIN_ADVANCE
  378. e_extruding = current_block->steps_e.wide != 0;
  379. #endif
  380. }
  381. step_events_completed.wide = 0;
  382. // Set directions.
  383. out_bits = current_block->direction_bits;
  384. // Set the direction bits (X_AXIS=A_AXIS and Y_AXIS=B_AXIS for COREXY)
  385. if((out_bits & (1<<X_AXIS))!=0){
  386. WRITE_NC(X_DIR_PIN, INVERT_X_DIR);
  387. count_direction[X_AXIS]=-1;
  388. } else {
  389. WRITE_NC(X_DIR_PIN, !INVERT_X_DIR);
  390. count_direction[X_AXIS]=1;
  391. }
  392. if((out_bits & (1<<Y_AXIS))!=0){
  393. WRITE_NC(Y_DIR_PIN, INVERT_Y_DIR);
  394. count_direction[Y_AXIS]=-1;
  395. } else {
  396. WRITE_NC(Y_DIR_PIN, !INVERT_Y_DIR);
  397. count_direction[Y_AXIS]=1;
  398. }
  399. if ((out_bits & (1<<Z_AXIS)) != 0) { // -direction
  400. WRITE_NC(Z_DIR_PIN,INVERT_Z_DIR);
  401. count_direction[Z_AXIS]=-1;
  402. } else { // +direction
  403. WRITE_NC(Z_DIR_PIN,!INVERT_Z_DIR);
  404. count_direction[Z_AXIS]=1;
  405. }
  406. if ((out_bits & (1 << E_AXIS)) != 0) { // -direction
  407. #ifndef LIN_ADVANCE
  408. WRITE(E0_DIR_PIN,
  409. #ifdef SNMM
  410. (mmu_extruder == 0 || mmu_extruder == 2) ? !INVERT_E0_DIR :
  411. #endif // SNMM
  412. INVERT_E0_DIR);
  413. #endif /* LIN_ADVANCE */
  414. count_direction[E_AXIS] = -1;
  415. } else { // +direction
  416. #ifndef LIN_ADVANCE
  417. WRITE(E0_DIR_PIN,
  418. #ifdef SNMM
  419. (mmu_extruder == 0 || mmu_extruder == 2) ? INVERT_E0_DIR :
  420. #endif // SNMM
  421. !INVERT_E0_DIR);
  422. #endif /* LIN_ADVANCE */
  423. count_direction[E_AXIS] = 1;
  424. }
  425. #if defined(FILAMENT_SENSOR) && defined(PAT9125)
  426. fsensor_st_block_begin(count_direction[E_AXIS] < 0);
  427. #endif //FILAMENT_SENSOR
  428. }
  429. else {
  430. _NEXT_ISR(2000); // 1kHz.
  431. #ifdef LIN_ADVANCE
  432. // reset LA state when there's no block
  433. nextAdvanceISR = ADV_NEVER;
  434. e_steps = 0;
  435. // incrementally lose pressure to give a chance for
  436. // a new LA block to be scheduled and recover
  437. if(current_adv_steps)
  438. --current_adv_steps;
  439. #endif
  440. }
  441. //WRITE_NC(LOGIC_ANALYZER_CH2, false);
  442. }
  443. // Check limit switches.
  444. FORCE_INLINE void stepper_check_endstops()
  445. {
  446. if(check_endstops)
  447. {
  448. #ifndef COREXY
  449. if ((out_bits & (1<<X_AXIS)) != 0) // stepping along -X axis
  450. #else
  451. if ((((out_bits & (1<<X_AXIS)) != 0)&&(out_bits & (1<<Y_AXIS)) != 0)) //-X occurs for -A and -B
  452. #endif
  453. {
  454. #if ( (defined(X_MIN_PIN) && (X_MIN_PIN > -1)) || defined(TMC2130_SG_HOMING) ) && !defined(DEBUG_DISABLE_XMINLIMIT)
  455. #ifdef TMC2130_SG_HOMING
  456. // Stall guard homing turned on
  457. x_min_endstop = (READ(X_TMC2130_DIAG) != 0);
  458. #else
  459. // Normal homing
  460. x_min_endstop = (READ(X_MIN_PIN) != X_MIN_ENDSTOP_INVERTING);
  461. #endif
  462. if(x_min_endstop && old_x_min_endstop && (current_block->steps_x.wide > 0)) {
  463. endstops_trigsteps[X_AXIS] = count_position[X_AXIS];
  464. endstop_x_hit=true;
  465. step_events_completed.wide = current_block->step_event_count.wide;
  466. }
  467. old_x_min_endstop = x_min_endstop;
  468. #endif
  469. } else { // +direction
  470. #if ( (defined(X_MAX_PIN) && (X_MAX_PIN > -1)) || defined(TMC2130_SG_HOMING) ) && !defined(DEBUG_DISABLE_XMAXLIMIT)
  471. #ifdef TMC2130_SG_HOMING
  472. // Stall guard homing turned on
  473. x_max_endstop = (READ(X_TMC2130_DIAG) != 0);
  474. #else
  475. // Normal homing
  476. x_max_endstop = (READ(X_MAX_PIN) != X_MAX_ENDSTOP_INVERTING);
  477. #endif
  478. if(x_max_endstop && old_x_max_endstop && (current_block->steps_x.wide > 0)){
  479. endstops_trigsteps[X_AXIS] = count_position[X_AXIS];
  480. endstop_x_hit=true;
  481. step_events_completed.wide = current_block->step_event_count.wide;
  482. }
  483. old_x_max_endstop = x_max_endstop;
  484. #endif
  485. }
  486. #ifndef COREXY
  487. if ((out_bits & (1<<Y_AXIS)) != 0) // -direction
  488. #else
  489. if ((((out_bits & (1<<X_AXIS)) != 0)&&(out_bits & (1<<Y_AXIS)) == 0)) // -Y occurs for -A and +B
  490. #endif
  491. {
  492. #if ( (defined(Y_MIN_PIN) && (Y_MIN_PIN > -1)) || defined(TMC2130_SG_HOMING) ) && !defined(DEBUG_DISABLE_YMINLIMIT)
  493. #ifdef TMC2130_SG_HOMING
  494. // Stall guard homing turned on
  495. y_min_endstop = (READ(Y_TMC2130_DIAG) != 0);
  496. #else
  497. // Normal homing
  498. y_min_endstop = (READ(Y_MIN_PIN) != Y_MIN_ENDSTOP_INVERTING);
  499. #endif
  500. if(y_min_endstop && old_y_min_endstop && (current_block->steps_y.wide > 0)) {
  501. endstops_trigsteps[Y_AXIS] = count_position[Y_AXIS];
  502. endstop_y_hit=true;
  503. step_events_completed.wide = current_block->step_event_count.wide;
  504. }
  505. old_y_min_endstop = y_min_endstop;
  506. #endif
  507. } else { // +direction
  508. #if ( (defined(Y_MAX_PIN) && (Y_MAX_PIN > -1)) || defined(TMC2130_SG_HOMING) ) && !defined(DEBUG_DISABLE_YMAXLIMIT)
  509. #ifdef TMC2130_SG_HOMING
  510. // Stall guard homing turned on
  511. y_max_endstop = (READ(Y_TMC2130_DIAG) != 0);
  512. #else
  513. // Normal homing
  514. y_max_endstop = (READ(Y_MAX_PIN) != Y_MAX_ENDSTOP_INVERTING);
  515. #endif
  516. if(y_max_endstop && old_y_max_endstop && (current_block->steps_y.wide > 0)){
  517. endstops_trigsteps[Y_AXIS] = count_position[Y_AXIS];
  518. endstop_y_hit=true;
  519. step_events_completed.wide = current_block->step_event_count.wide;
  520. }
  521. old_y_max_endstop = y_max_endstop;
  522. #endif
  523. }
  524. if ((out_bits & (1<<Z_AXIS)) != 0) // -direction
  525. {
  526. #if defined(Z_MIN_PIN) && (Z_MIN_PIN > -1) && !defined(DEBUG_DISABLE_ZMINLIMIT)
  527. if (! check_z_endstop) {
  528. #ifdef TMC2130_SG_HOMING
  529. // Stall guard homing turned on
  530. #ifdef TMC2130_STEALTH_Z
  531. if ((tmc2130_mode == TMC2130_MODE_SILENT) && !(tmc2130_sg_homing_axes_mask & 0x04))
  532. z_min_endstop = (READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING);
  533. else
  534. #endif //TMC2130_STEALTH_Z
  535. z_min_endstop = (READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING) || (READ(Z_TMC2130_DIAG) != 0);
  536. #else
  537. z_min_endstop = (READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING);
  538. #endif //TMC2130_SG_HOMING
  539. if(z_min_endstop && old_z_min_endstop && (current_block->steps_z.wide > 0)) {
  540. endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS];
  541. endstop_z_hit=true;
  542. step_events_completed.wide = current_block->step_event_count.wide;
  543. }
  544. old_z_min_endstop = z_min_endstop;
  545. }
  546. #endif
  547. } else { // +direction
  548. #if defined(Z_MAX_PIN) && (Z_MAX_PIN > -1) && !defined(DEBUG_DISABLE_ZMAXLIMIT)
  549. #ifdef TMC2130_SG_HOMING
  550. // Stall guard homing turned on
  551. #ifdef TMC2130_STEALTH_Z
  552. if ((tmc2130_mode == TMC2130_MODE_SILENT) && !(tmc2130_sg_homing_axes_mask & 0x04))
  553. z_max_endstop = false;
  554. else
  555. #endif //TMC2130_STEALTH_Z
  556. z_max_endstop = (READ(Z_TMC2130_DIAG) != 0);
  557. #else
  558. z_max_endstop = (READ(Z_MAX_PIN) != Z_MAX_ENDSTOP_INVERTING);
  559. #endif //TMC2130_SG_HOMING
  560. if(z_max_endstop && old_z_max_endstop && (current_block->steps_z.wide > 0)) {
  561. endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS];
  562. endstop_z_hit=true;
  563. step_events_completed.wide = current_block->step_event_count.wide;
  564. }
  565. old_z_max_endstop = z_max_endstop;
  566. #endif
  567. }
  568. }
  569. // Supporting stopping on a trigger of the Z-stop induction sensor, not only for the Z-minus movements.
  570. #if defined(Z_MIN_PIN) && (Z_MIN_PIN > -1) && !defined(DEBUG_DISABLE_ZMINLIMIT)
  571. if (check_z_endstop) {
  572. // Check the Z min end-stop no matter what.
  573. // Good for searching for the center of an induction target.
  574. #ifdef TMC2130_SG_HOMING
  575. // Stall guard homing turned on
  576. #ifdef TMC2130_STEALTH_Z
  577. if ((tmc2130_mode == TMC2130_MODE_SILENT) && !(tmc2130_sg_homing_axes_mask & 0x04))
  578. z_min_endstop = (READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING);
  579. else
  580. #endif //TMC2130_STEALTH_Z
  581. z_min_endstop = (READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING) || (READ(Z_TMC2130_DIAG) != 0);
  582. #else
  583. z_min_endstop = (READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING);
  584. #endif //TMC2130_SG_HOMING
  585. if(z_min_endstop && old_z_min_endstop) {
  586. endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS];
  587. endstop_z_hit=true;
  588. step_events_completed.wide = current_block->step_event_count.wide;
  589. }
  590. old_z_min_endstop = z_min_endstop;
  591. }
  592. #endif
  593. }
  594. FORCE_INLINE void stepper_tick_lowres()
  595. {
  596. for (uint8_t i=0; i < step_loops; ++ i) { // Take multiple steps per interrupt (For high speed moves)
  597. MSerial.checkRx(); // Check for serial chars.
  598. // Step in X axis
  599. counter_x.lo += current_block->steps_x.lo;
  600. if (counter_x.lo > 0) {
  601. STEP_NC_HI(X_AXIS);
  602. #ifdef DEBUG_XSTEP_DUP_PIN
  603. STEP_NC_HI(X_DUP_AXIS);
  604. #endif //DEBUG_XSTEP_DUP_PIN
  605. counter_x.lo -= current_block->step_event_count.lo;
  606. count_position[X_AXIS]+=count_direction[X_AXIS];
  607. STEP_NC_LO(X_AXIS);
  608. #ifdef DEBUG_XSTEP_DUP_PIN
  609. STEP_NC_LO(X_DUP_AXIS);
  610. #endif //DEBUG_XSTEP_DUP_PIN
  611. }
  612. // Step in Y axis
  613. counter_y.lo += current_block->steps_y.lo;
  614. if (counter_y.lo > 0) {
  615. STEP_NC_HI(Y_AXIS);
  616. #ifdef DEBUG_YSTEP_DUP_PIN
  617. STEP_NC_HI(Y_DUP_AXIS);
  618. #endif //DEBUG_YSTEP_DUP_PIN
  619. counter_y.lo -= current_block->step_event_count.lo;
  620. count_position[Y_AXIS]+=count_direction[Y_AXIS];
  621. STEP_NC_LO(Y_AXIS);
  622. #ifdef DEBUG_YSTEP_DUP_PIN
  623. STEP_NC_LO(Y_DUP_AXIS);
  624. #endif //DEBUG_YSTEP_DUP_PIN
  625. }
  626. // Step in Z axis
  627. counter_z.lo += current_block->steps_z.lo;
  628. if (counter_z.lo > 0) {
  629. STEP_NC_HI(Z_AXIS);
  630. counter_z.lo -= current_block->step_event_count.lo;
  631. count_position[Z_AXIS]+=count_direction[Z_AXIS];
  632. STEP_NC_LO(Z_AXIS);
  633. }
  634. // Step in E axis
  635. counter_e.lo += current_block->steps_e.lo;
  636. if (counter_e.lo > 0) {
  637. #ifndef LIN_ADVANCE
  638. STEP_NC_HI(E_AXIS);
  639. #endif /* LIN_ADVANCE */
  640. counter_e.lo -= current_block->step_event_count.lo;
  641. count_position[E_AXIS] += count_direction[E_AXIS];
  642. #ifdef LIN_ADVANCE
  643. e_steps += count_direction[E_AXIS];
  644. #else
  645. #ifdef FILAMENT_SENSOR
  646. fsensor_counter += count_direction[E_AXIS];
  647. #endif //FILAMENT_SENSOR
  648. STEP_NC_LO(E_AXIS);
  649. #endif
  650. }
  651. if(++ step_events_completed.lo >= current_block->step_event_count.lo)
  652. break;
  653. }
  654. }
  655. FORCE_INLINE void stepper_tick_highres()
  656. {
  657. for (uint8_t i=0; i < step_loops; ++ i) { // Take multiple steps per interrupt (For high speed moves)
  658. MSerial.checkRx(); // Check for serial chars.
  659. // Step in X axis
  660. counter_x.wide += current_block->steps_x.wide;
  661. if (counter_x.wide > 0) {
  662. STEP_NC_HI(X_AXIS);
  663. #ifdef DEBUG_XSTEP_DUP_PIN
  664. STEP_NC_HI(X_DUP_AXIS);
  665. #endif //DEBUG_XSTEP_DUP_PIN
  666. counter_x.wide -= current_block->step_event_count.wide;
  667. count_position[X_AXIS]+=count_direction[X_AXIS];
  668. STEP_NC_LO(X_AXIS);
  669. #ifdef DEBUG_XSTEP_DUP_PIN
  670. STEP_NC_LO(X_DUP_AXIS);
  671. #endif //DEBUG_XSTEP_DUP_PIN
  672. }
  673. // Step in Y axis
  674. counter_y.wide += current_block->steps_y.wide;
  675. if (counter_y.wide > 0) {
  676. STEP_NC_HI(Y_AXIS);
  677. #ifdef DEBUG_YSTEP_DUP_PIN
  678. STEP_NC_HI(Y_DUP_AXIS);
  679. #endif //DEBUG_YSTEP_DUP_PIN
  680. counter_y.wide -= current_block->step_event_count.wide;
  681. count_position[Y_AXIS]+=count_direction[Y_AXIS];
  682. STEP_NC_LO(Y_AXIS);
  683. #ifdef DEBUG_YSTEP_DUP_PIN
  684. STEP_NC_LO(Y_DUP_AXIS);
  685. #endif //DEBUG_YSTEP_DUP_PIN
  686. }
  687. // Step in Z axis
  688. counter_z.wide += current_block->steps_z.wide;
  689. if (counter_z.wide > 0) {
  690. STEP_NC_HI(Z_AXIS);
  691. counter_z.wide -= current_block->step_event_count.wide;
  692. count_position[Z_AXIS]+=count_direction[Z_AXIS];
  693. STEP_NC_LO(Z_AXIS);
  694. }
  695. // Step in E axis
  696. counter_e.wide += current_block->steps_e.wide;
  697. if (counter_e.wide > 0) {
  698. #ifndef LIN_ADVANCE
  699. STEP_NC_HI(E_AXIS);
  700. #endif /* LIN_ADVANCE */
  701. counter_e.wide -= current_block->step_event_count.wide;
  702. count_position[E_AXIS]+=count_direction[E_AXIS];
  703. #ifdef LIN_ADVANCE
  704. e_steps += count_direction[E_AXIS];
  705. #else
  706. #ifdef FILAMENT_SENSOR
  707. fsensor_counter += count_direction[E_AXIS];
  708. #endif //FILAMENT_SENSOR
  709. STEP_NC_LO(E_AXIS);
  710. #endif
  711. }
  712. if(++ step_events_completed.wide >= current_block->step_event_count.wide)
  713. break;
  714. }
  715. }
  716. #ifdef LIN_ADVANCE
  717. // @wavexx: fast uint16_t division for small dividends<5
  718. // q/3 based on "Hacker's delight" formula
  719. FORCE_INLINE uint16_t fastdiv(uint16_t q, uint8_t d)
  720. {
  721. if(d != 3) return q >> (d / 2);
  722. else return ((uint32_t)0xAAAB * q) >> 17;
  723. }
  724. FORCE_INLINE void advance_spread(uint16_t timer)
  725. {
  726. eISR_Err += timer;
  727. uint8_t ticks = 0;
  728. while(eISR_Err >= current_block->advance_rate)
  729. {
  730. ++ticks;
  731. eISR_Err -= current_block->advance_rate;
  732. }
  733. if(!ticks)
  734. {
  735. eISR_Rate = timer;
  736. nextAdvanceISR = timer;
  737. return;
  738. }
  739. if (ticks <= 3)
  740. eISR_Rate = fastdiv(timer, ticks + 1);
  741. else
  742. {
  743. // >4 ticks are still possible on slow moves
  744. eISR_Rate = timer / (ticks + 1);
  745. }
  746. nextAdvanceISR = eISR_Rate;
  747. }
  748. #endif
  749. FORCE_INLINE void isr() {
  750. //WRITE_NC(LOGIC_ANALYZER_CH0, true);
  751. //if (UVLO) uvlo();
  752. // If there is no current block, attempt to pop one from the buffer
  753. if (current_block == NULL)
  754. stepper_next_block();
  755. if (current_block != NULL)
  756. {
  757. stepper_check_endstops();
  758. if (current_block->flag & BLOCK_FLAG_DDA_LOWRES)
  759. stepper_tick_lowres();
  760. else
  761. stepper_tick_highres();
  762. #ifdef LIN_ADVANCE
  763. if (e_steps) WRITE_NC(E0_DIR_PIN, e_steps < 0? INVERT_E0_DIR: !INVERT_E0_DIR);
  764. uint8_t la_state = 0;
  765. #endif
  766. // Calculate new timer value
  767. // 13.38-14.63us for steady state,
  768. // 25.12us for acceleration / deceleration.
  769. {
  770. //WRITE_NC(LOGIC_ANALYZER_CH1, true);
  771. if (step_events_completed.wide <= current_block->accelerate_until) {
  772. // v = t * a -> acc_step_rate = acceleration_time * current_block->acceleration_rate
  773. MultiU24X24toH16(acc_step_rate, acceleration_time, current_block->acceleration_rate);
  774. acc_step_rate += uint16_t(current_block->initial_rate);
  775. // upper limit
  776. if(acc_step_rate > uint16_t(current_block->nominal_rate))
  777. acc_step_rate = current_block->nominal_rate;
  778. // step_rate to timer interval
  779. uint16_t timer = calc_timer(acc_step_rate, step_loops);
  780. _NEXT_ISR(timer);
  781. acceleration_time += timer;
  782. #ifdef LIN_ADVANCE
  783. if (current_block->use_advance_lead) {
  784. if (step_events_completed.wide <= (unsigned long int)step_loops) {
  785. la_state = ADV_INIT | ADV_ACC_VARY;
  786. if (e_extruding && current_adv_steps > target_adv_steps)
  787. target_adv_steps = current_adv_steps;
  788. }
  789. }
  790. #endif
  791. }
  792. else if (step_events_completed.wide > current_block->decelerate_after) {
  793. uint16_t step_rate;
  794. MultiU24X24toH16(step_rate, deceleration_time, current_block->acceleration_rate);
  795. if (step_rate > acc_step_rate) { // Check step_rate stays positive
  796. step_rate = uint16_t(current_block->final_rate);
  797. }
  798. else {
  799. step_rate = acc_step_rate - step_rate; // Decelerate from acceleration end point.
  800. // lower limit
  801. if (step_rate < current_block->final_rate)
  802. step_rate = uint16_t(current_block->final_rate);
  803. }
  804. // Step_rate to timer interval.
  805. uint16_t timer = calc_timer(step_rate, step_loops);
  806. _NEXT_ISR(timer);
  807. deceleration_time += timer;
  808. #ifdef LIN_ADVANCE
  809. if (current_block->use_advance_lead) {
  810. if (step_events_completed.wide <= current_block->decelerate_after + step_loops) {
  811. target_adv_steps = current_block->final_adv_steps;
  812. la_state = ADV_INIT | ADV_ACC_VARY;
  813. if (e_extruding && current_adv_steps < target_adv_steps)
  814. target_adv_steps = current_adv_steps;
  815. }
  816. }
  817. #endif
  818. }
  819. else {
  820. if (! step_loops_nominal) {
  821. // Calculation of the steady state timer rate has been delayed to the 1st tick of the steady state to lower
  822. // the initial interrupt blocking.
  823. OCR1A_nominal = calc_timer(uint16_t(current_block->nominal_rate), step_loops);
  824. step_loops_nominal = step_loops;
  825. #ifdef LIN_ADVANCE
  826. if(current_block->use_advance_lead) {
  827. // Due to E-jerk, there can be discontinuities in pressure state where an
  828. // acceleration or deceleration can be skipped or joined with the previous block.
  829. // If LA was not previously active, re-check the pressure level
  830. la_state = ADV_INIT;
  831. if (e_extruding)
  832. target_adv_steps = current_adv_steps;
  833. }
  834. #endif
  835. }
  836. _NEXT_ISR(OCR1A_nominal);
  837. }
  838. //WRITE_NC(LOGIC_ANALYZER_CH1, false);
  839. }
  840. #ifdef LIN_ADVANCE
  841. // avoid multiple instances or function calls to advance_spread
  842. if (la_state & ADV_INIT) {
  843. LA_phase = -1;
  844. if (current_adv_steps == target_adv_steps) {
  845. // nothing to be done in this phase, cancel any pending eisr
  846. la_state = 0;
  847. nextAdvanceISR = ADV_NEVER;
  848. }
  849. else {
  850. // reset error and iterations per loop for this phase
  851. eISR_Err = current_block->advance_rate;
  852. e_step_loops = current_block->advance_step_loops;
  853. if ((la_state & ADV_ACC_VARY) && e_extruding && (current_adv_steps > target_adv_steps)) {
  854. // LA could reverse the direction of extrusion in this phase
  855. eISR_Err += current_block->advance_rate;
  856. LA_phase = 0;
  857. }
  858. }
  859. }
  860. if (la_state & ADV_INIT || nextAdvanceISR != ADV_NEVER) {
  861. // update timers & phase for the next iteration
  862. advance_spread(main_Rate);
  863. if (LA_phase >= 0) {
  864. if (step_loops == e_step_loops)
  865. LA_phase = (current_block->advance_rate < main_Rate);
  866. else {
  867. // avoid overflow through division. warning: we need to _guarantee_ step_loops
  868. // and e_step_loops are <= 4 due to fastdiv's limit
  869. auto adv_rate_n = fastdiv(current_block->advance_rate, step_loops);
  870. auto main_rate_n = fastdiv(main_Rate, e_step_loops);
  871. LA_phase = (adv_rate_n < main_rate_n);
  872. }
  873. }
  874. }
  875. // Check for serial chars. This executes roughtly inbetween 50-60% of the total runtime of the
  876. // entire isr, making this spot a much better choice than checking during esteps
  877. MSerial.checkRx();
  878. #endif
  879. // If current block is finished, reset pointer
  880. if (step_events_completed.wide >= current_block->step_event_count.wide) {
  881. #if !defined(LIN_ADVANCE) && defined(FILAMENT_SENSOR)
  882. fsensor_st_block_chunk(fsensor_counter);
  883. fsensor_counter = 0;
  884. #endif //FILAMENT_SENSOR
  885. current_block = NULL;
  886. plan_discard_current_block();
  887. }
  888. #if !defined(LIN_ADVANCE) && defined(FILAMENT_SENSOR)
  889. else if ((abs(fsensor_counter) >= fsensor_chunk_len))
  890. {
  891. fsensor_st_block_chunk(fsensor_counter);
  892. fsensor_counter = 0;
  893. }
  894. #endif //FILAMENT_SENSOR
  895. }
  896. #ifdef TMC2130
  897. tmc2130_st_isr();
  898. #endif //TMC2130
  899. //WRITE_NC(LOGIC_ANALYZER_CH0, false);
  900. }
  901. #ifdef LIN_ADVANCE
  902. // Timer interrupt for E. e_steps is set in the main routine.
  903. FORCE_INLINE void advance_isr() {
  904. if (current_adv_steps > target_adv_steps) {
  905. // decompression
  906. if (e_step_loops != 1) {
  907. uint16_t d_steps = current_adv_steps - target_adv_steps;
  908. if (d_steps < e_step_loops)
  909. e_step_loops = d_steps;
  910. }
  911. e_steps -= e_step_loops;
  912. if (e_steps) WRITE_NC(E0_DIR_PIN, e_steps < 0? INVERT_E0_DIR: !INVERT_E0_DIR);
  913. current_adv_steps -= e_step_loops;
  914. }
  915. else if (current_adv_steps < target_adv_steps) {
  916. // compression
  917. if (e_step_loops != 1) {
  918. uint16_t d_steps = target_adv_steps - current_adv_steps;
  919. if (d_steps < e_step_loops)
  920. e_step_loops = d_steps;
  921. }
  922. e_steps += e_step_loops;
  923. if (e_steps) WRITE_NC(E0_DIR_PIN, e_steps < 0? INVERT_E0_DIR: !INVERT_E0_DIR);
  924. current_adv_steps += e_step_loops;
  925. }
  926. if (current_adv_steps == target_adv_steps) {
  927. // advance steps completed
  928. nextAdvanceISR = ADV_NEVER;
  929. }
  930. else {
  931. // schedule another tick
  932. nextAdvanceISR = eISR_Rate;
  933. }
  934. }
  935. FORCE_INLINE void advance_isr_scheduler() {
  936. // Integrate the final timer value, accounting for scheduling adjustments
  937. if(nextAdvanceISR && nextAdvanceISR != ADV_NEVER)
  938. {
  939. if(nextAdvanceISR > OCR1A)
  940. nextAdvanceISR -= OCR1A;
  941. else
  942. nextAdvanceISR = 0;
  943. }
  944. if(nextMainISR > OCR1A)
  945. nextMainISR -= OCR1A;
  946. else
  947. nextMainISR = 0;
  948. // Run main stepping ISR if flagged
  949. if (!nextMainISR)
  950. {
  951. #ifdef LA_DEBUG_LOGIC
  952. WRITE_NC(LOGIC_ANALYZER_CH0, true);
  953. #endif
  954. isr();
  955. #ifdef LA_DEBUG_LOGIC
  956. WRITE_NC(LOGIC_ANALYZER_CH0, false);
  957. #endif
  958. }
  959. // Run the next advance isr if triggered
  960. bool eisr = !nextAdvanceISR;
  961. if (eisr)
  962. {
  963. #ifdef LA_DEBUG_LOGIC
  964. WRITE_NC(LOGIC_ANALYZER_CH1, true);
  965. #endif
  966. advance_isr();
  967. #ifdef LA_DEBUG_LOGIC
  968. WRITE_NC(LOGIC_ANALYZER_CH1, false);
  969. #endif
  970. }
  971. // Tick E steps if any
  972. if (e_steps && (LA_phase < 0 || LA_phase == eisr)) {
  973. uint8_t max_ticks = (eisr? e_step_loops: step_loops);
  974. max_ticks = min(abs(e_steps), max_ticks);
  975. bool rev = (e_steps < 0);
  976. do
  977. {
  978. STEP_NC_HI(E_AXIS);
  979. e_steps += (rev? 1: -1);
  980. STEP_NC_LO(E_AXIS);
  981. #if defined(FILAMENT_SENSOR) && defined(PAT9125)
  982. fsensor_counter += (rev? -1: 1);
  983. #endif
  984. }
  985. while(--max_ticks);
  986. #if defined(FILAMENT_SENSOR) && defined(PAT9125)
  987. if (abs(fsensor_counter) >= fsensor_chunk_len)
  988. {
  989. fsensor_st_block_chunk(fsensor_counter);
  990. fsensor_counter = 0;
  991. }
  992. #endif
  993. }
  994. // Schedule the next closest tick, ignoring advance if scheduled too
  995. // soon in order to avoid skewing the regular stepper acceleration
  996. if (nextAdvanceISR != ADV_NEVER && (nextAdvanceISR + 40) < nextMainISR)
  997. OCR1A = nextAdvanceISR;
  998. else
  999. OCR1A = nextMainISR;
  1000. }
  1001. #endif // LIN_ADVANCE
  1002. void st_init()
  1003. {
  1004. #ifdef TMC2130
  1005. tmc2130_init(TMCInitParams(false, FarmOrUserECool()));
  1006. #endif //TMC2130
  1007. st_current_init(); //Initialize Digipot Motor Current
  1008. microstep_init(); //Initialize Microstepping Pins
  1009. //Initialize Dir Pins
  1010. #if defined(X_DIR_PIN) && X_DIR_PIN > -1
  1011. SET_OUTPUT(X_DIR_PIN);
  1012. #endif
  1013. #if defined(X2_DIR_PIN) && X2_DIR_PIN > -1
  1014. SET_OUTPUT(X2_DIR_PIN);
  1015. #endif
  1016. #if defined(Y_DIR_PIN) && Y_DIR_PIN > -1
  1017. SET_OUTPUT(Y_DIR_PIN);
  1018. #if defined(Y_DUAL_STEPPER_DRIVERS) && defined(Y2_DIR_PIN) && (Y2_DIR_PIN > -1)
  1019. SET_OUTPUT(Y2_DIR_PIN);
  1020. #endif
  1021. #endif
  1022. #if defined(Z_DIR_PIN) && Z_DIR_PIN > -1
  1023. SET_OUTPUT(Z_DIR_PIN);
  1024. #if defined(Z_DUAL_STEPPER_DRIVERS) && defined(Z2_DIR_PIN) && (Z2_DIR_PIN > -1)
  1025. SET_OUTPUT(Z2_DIR_PIN);
  1026. #endif
  1027. #endif
  1028. #if defined(E0_DIR_PIN) && E0_DIR_PIN > -1
  1029. SET_OUTPUT(E0_DIR_PIN);
  1030. #endif
  1031. #if defined(E1_DIR_PIN) && (E1_DIR_PIN > -1)
  1032. SET_OUTPUT(E1_DIR_PIN);
  1033. #endif
  1034. #if defined(E2_DIR_PIN) && (E2_DIR_PIN > -1)
  1035. SET_OUTPUT(E2_DIR_PIN);
  1036. #endif
  1037. //Initialize Enable Pins - steppers default to disabled.
  1038. #if defined(X_ENABLE_PIN) && X_ENABLE_PIN > -1
  1039. SET_OUTPUT(X_ENABLE_PIN);
  1040. if(!X_ENABLE_ON) WRITE(X_ENABLE_PIN,HIGH);
  1041. #endif
  1042. #if defined(X2_ENABLE_PIN) && X2_ENABLE_PIN > -1
  1043. SET_OUTPUT(X2_ENABLE_PIN);
  1044. if(!X_ENABLE_ON) WRITE(X2_ENABLE_PIN,HIGH);
  1045. #endif
  1046. #if defined(Y_ENABLE_PIN) && Y_ENABLE_PIN > -1
  1047. SET_OUTPUT(Y_ENABLE_PIN);
  1048. if(!Y_ENABLE_ON) WRITE(Y_ENABLE_PIN,HIGH);
  1049. #if defined(Y_DUAL_STEPPER_DRIVERS) && defined(Y2_ENABLE_PIN) && (Y2_ENABLE_PIN > -1)
  1050. SET_OUTPUT(Y2_ENABLE_PIN);
  1051. if(!Y_ENABLE_ON) WRITE(Y2_ENABLE_PIN,HIGH);
  1052. #endif
  1053. #endif
  1054. #if defined(Z_ENABLE_PIN) && Z_ENABLE_PIN > -1
  1055. SET_OUTPUT(Z_ENABLE_PIN);
  1056. if(!Z_ENABLE_ON) WRITE(Z_ENABLE_PIN,HIGH);
  1057. #if defined(Z_DUAL_STEPPER_DRIVERS) && defined(Z2_ENABLE_PIN) && (Z2_ENABLE_PIN > -1)
  1058. SET_OUTPUT(Z2_ENABLE_PIN);
  1059. if(!Z_ENABLE_ON) WRITE(Z2_ENABLE_PIN,HIGH);
  1060. #endif
  1061. #endif
  1062. #if defined(E0_ENABLE_PIN) && (E0_ENABLE_PIN > -1)
  1063. SET_OUTPUT(E0_ENABLE_PIN);
  1064. if(!E_ENABLE_ON) WRITE(E0_ENABLE_PIN,HIGH);
  1065. #endif
  1066. #if defined(E1_ENABLE_PIN) && (E1_ENABLE_PIN > -1)
  1067. SET_OUTPUT(E1_ENABLE_PIN);
  1068. if(!E_ENABLE_ON) WRITE(E1_ENABLE_PIN,HIGH);
  1069. #endif
  1070. #if defined(E2_ENABLE_PIN) && (E2_ENABLE_PIN > -1)
  1071. SET_OUTPUT(E2_ENABLE_PIN);
  1072. if(!E_ENABLE_ON) WRITE(E2_ENABLE_PIN,HIGH);
  1073. #endif
  1074. //endstops and pullups
  1075. #ifdef TMC2130_SG_HOMING
  1076. SET_INPUT(X_TMC2130_DIAG);
  1077. WRITE(X_TMC2130_DIAG,HIGH);
  1078. SET_INPUT(Y_TMC2130_DIAG);
  1079. WRITE(Y_TMC2130_DIAG,HIGH);
  1080. SET_INPUT(Z_TMC2130_DIAG);
  1081. WRITE(Z_TMC2130_DIAG,HIGH);
  1082. SET_INPUT(E0_TMC2130_DIAG);
  1083. WRITE(E0_TMC2130_DIAG,HIGH);
  1084. #endif
  1085. #if defined(X_MIN_PIN) && X_MIN_PIN > -1
  1086. SET_INPUT(X_MIN_PIN);
  1087. #ifdef ENDSTOPPULLUP_XMIN
  1088. WRITE(X_MIN_PIN,HIGH);
  1089. #endif
  1090. #endif
  1091. #if defined(Y_MIN_PIN) && Y_MIN_PIN > -1
  1092. SET_INPUT(Y_MIN_PIN);
  1093. #ifdef ENDSTOPPULLUP_YMIN
  1094. WRITE(Y_MIN_PIN,HIGH);
  1095. #endif
  1096. #endif
  1097. #if defined(Z_MIN_PIN) && Z_MIN_PIN > -1
  1098. SET_INPUT(Z_MIN_PIN);
  1099. #ifdef ENDSTOPPULLUP_ZMIN
  1100. WRITE(Z_MIN_PIN,HIGH);
  1101. #endif
  1102. #endif
  1103. #if defined(X_MAX_PIN) && X_MAX_PIN > -1
  1104. SET_INPUT(X_MAX_PIN);
  1105. #ifdef ENDSTOPPULLUP_XMAX
  1106. WRITE(X_MAX_PIN,HIGH);
  1107. #endif
  1108. #endif
  1109. #if defined(Y_MAX_PIN) && Y_MAX_PIN > -1
  1110. SET_INPUT(Y_MAX_PIN);
  1111. #ifdef ENDSTOPPULLUP_YMAX
  1112. WRITE(Y_MAX_PIN,HIGH);
  1113. #endif
  1114. #endif
  1115. #if defined(Z_MAX_PIN) && Z_MAX_PIN > -1
  1116. SET_INPUT(Z_MAX_PIN);
  1117. #ifdef ENDSTOPPULLUP_ZMAX
  1118. WRITE(Z_MAX_PIN,HIGH);
  1119. #endif
  1120. #endif
  1121. #if (defined(FANCHECK) && defined(TACH_0) && (TACH_0 > -1))
  1122. SET_INPUT(TACH_0);
  1123. #ifdef TACH0PULLUP
  1124. WRITE(TACH_0, HIGH);
  1125. #endif
  1126. #endif
  1127. //Initialize Step Pins
  1128. #if defined(X_STEP_PIN) && (X_STEP_PIN > -1)
  1129. SET_OUTPUT(X_STEP_PIN);
  1130. WRITE(X_STEP_PIN,INVERT_X_STEP_PIN);
  1131. #ifdef DEBUG_XSTEP_DUP_PIN
  1132. SET_OUTPUT(DEBUG_XSTEP_DUP_PIN);
  1133. WRITE(DEBUG_XSTEP_DUP_PIN,INVERT_X_STEP_PIN);
  1134. #endif //DEBUG_XSTEP_DUP_PIN
  1135. disable_x();
  1136. #endif
  1137. #if defined(X2_STEP_PIN) && (X2_STEP_PIN > -1)
  1138. SET_OUTPUT(X2_STEP_PIN);
  1139. WRITE(X2_STEP_PIN,INVERT_X_STEP_PIN);
  1140. disable_x();
  1141. #endif
  1142. #if defined(Y_STEP_PIN) && (Y_STEP_PIN > -1)
  1143. SET_OUTPUT(Y_STEP_PIN);
  1144. WRITE(Y_STEP_PIN,INVERT_Y_STEP_PIN);
  1145. #ifdef DEBUG_YSTEP_DUP_PIN
  1146. SET_OUTPUT(DEBUG_YSTEP_DUP_PIN);
  1147. WRITE(DEBUG_YSTEP_DUP_PIN,INVERT_Y_STEP_PIN);
  1148. #endif //DEBUG_YSTEP_DUP_PIN
  1149. #if defined(Y_DUAL_STEPPER_DRIVERS) && defined(Y2_STEP_PIN) && (Y2_STEP_PIN > -1)
  1150. SET_OUTPUT(Y2_STEP_PIN);
  1151. WRITE(Y2_STEP_PIN,INVERT_Y_STEP_PIN);
  1152. #endif
  1153. disable_y();
  1154. #endif
  1155. #if defined(Z_STEP_PIN) && (Z_STEP_PIN > -1)
  1156. SET_OUTPUT(Z_STEP_PIN);
  1157. WRITE(Z_STEP_PIN,INVERT_Z_STEP_PIN);
  1158. #if defined(Z_DUAL_STEPPER_DRIVERS) && defined(Z2_STEP_PIN) && (Z2_STEP_PIN > -1)
  1159. SET_OUTPUT(Z2_STEP_PIN);
  1160. WRITE(Z2_STEP_PIN,INVERT_Z_STEP_PIN);
  1161. #endif
  1162. #ifdef PSU_Delta
  1163. init_force_z();
  1164. #endif // PSU_Delta
  1165. disable_z();
  1166. #endif
  1167. #if defined(E0_STEP_PIN) && (E0_STEP_PIN > -1)
  1168. SET_OUTPUT(E0_STEP_PIN);
  1169. WRITE(E0_STEP_PIN,INVERT_E_STEP_PIN);
  1170. disable_e0();
  1171. #endif
  1172. #if defined(E1_STEP_PIN) && (E1_STEP_PIN > -1)
  1173. SET_OUTPUT(E1_STEP_PIN);
  1174. WRITE(E1_STEP_PIN,INVERT_E_STEP_PIN);
  1175. disable_e1();
  1176. #endif
  1177. #if defined(E2_STEP_PIN) && (E2_STEP_PIN > -1)
  1178. SET_OUTPUT(E2_STEP_PIN);
  1179. WRITE(E2_STEP_PIN,INVERT_E_STEP_PIN);
  1180. disable_e2();
  1181. #endif
  1182. // waveform generation = 0100 = CTC
  1183. TCCR1B &= ~(1<<WGM13);
  1184. TCCR1B |= (1<<WGM12);
  1185. TCCR1A &= ~(1<<WGM11);
  1186. TCCR1A &= ~(1<<WGM10);
  1187. // output mode = 00 (disconnected)
  1188. TCCR1A &= ~(3<<COM1A0);
  1189. TCCR1A &= ~(3<<COM1B0);
  1190. // Set the timer pre-scaler
  1191. // Generally we use a divider of 8, resulting in a 2MHz timer
  1192. // frequency on a 16MHz MCU. If you are going to change this, be
  1193. // sure to regenerate speed_lookuptable.h with
  1194. // create_speed_lookuptable.py
  1195. TCCR1B = (TCCR1B & ~(0x07<<CS10)) | (2<<CS10);
  1196. // Plan the first interrupt after 8ms from now.
  1197. OCR1A = 0x4000;
  1198. TCNT1 = 0;
  1199. #ifdef LIN_ADVANCE
  1200. #ifdef LA_DEBUG_LOGIC
  1201. LOGIC_ANALYZER_CH0_ENABLE;
  1202. LOGIC_ANALYZER_CH1_ENABLE;
  1203. WRITE_NC(LOGIC_ANALYZER_CH0, false);
  1204. WRITE_NC(LOGIC_ANALYZER_CH1, false);
  1205. #endif
  1206. // Initialize state for the linear advance scheduler
  1207. nextMainISR = 0;
  1208. nextAdvanceISR = ADV_NEVER;
  1209. main_Rate = ADV_NEVER;
  1210. current_adv_steps = 0;
  1211. #endif
  1212. enable_endstops(true); // Start with endstops active. After homing they can be disabled
  1213. ENABLE_STEPPER_DRIVER_INTERRUPT();
  1214. sei();
  1215. }
  1216. void st_reset_timer()
  1217. {
  1218. // Clear a possible pending interrupt on OCR1A overflow.
  1219. TIFR1 |= 1 << OCF1A;
  1220. // Reset the counter.
  1221. TCNT1 = 0;
  1222. // Wake up after 1ms from now.
  1223. OCR1A = 2000;
  1224. #ifdef LIN_ADVANCE
  1225. nextMainISR = 0;
  1226. if(nextAdvanceISR && nextAdvanceISR != ADV_NEVER)
  1227. nextAdvanceISR = 0;
  1228. #endif
  1229. }
  1230. // Block until all buffered steps are executed
  1231. void st_synchronize()
  1232. {
  1233. while(blocks_queued())
  1234. {
  1235. #ifdef TMC2130
  1236. manage_heater();
  1237. // Vojtech: Don't disable motors inside the planner!
  1238. if (!tmc2130_update_sg())
  1239. {
  1240. manage_inactivity(true);
  1241. lcd_update(0);
  1242. }
  1243. #else //TMC2130
  1244. manage_heater();
  1245. // Vojtech: Don't disable motors inside the planner!
  1246. manage_inactivity(true);
  1247. lcd_update(0);
  1248. #endif //TMC2130
  1249. }
  1250. }
  1251. void st_set_position(const long &x, const long &y, const long &z, const long &e)
  1252. {
  1253. CRITICAL_SECTION_START;
  1254. // Copy 4x4B.
  1255. // This block locks the interrupts globally for 4.56 us,
  1256. // which corresponds to a maximum repeat frequency of 219.18 kHz.
  1257. // This blocking is safe in the context of a 10kHz stepper driver interrupt
  1258. // or a 115200 Bd serial line receive interrupt, which will not trigger faster than 12kHz.
  1259. count_position[X_AXIS] = x;
  1260. count_position[Y_AXIS] = y;
  1261. count_position[Z_AXIS] = z;
  1262. count_position[E_AXIS] = e;
  1263. CRITICAL_SECTION_END;
  1264. }
  1265. void st_set_e_position(const long &e)
  1266. {
  1267. CRITICAL_SECTION_START;
  1268. count_position[E_AXIS] = e;
  1269. CRITICAL_SECTION_END;
  1270. }
  1271. long st_get_position(uint8_t axis)
  1272. {
  1273. long count_pos;
  1274. CRITICAL_SECTION_START;
  1275. count_pos = count_position[axis];
  1276. CRITICAL_SECTION_END;
  1277. return count_pos;
  1278. }
  1279. void st_get_position_xy(long &x, long &y)
  1280. {
  1281. CRITICAL_SECTION_START;
  1282. x = count_position[X_AXIS];
  1283. y = count_position[Y_AXIS];
  1284. CRITICAL_SECTION_END;
  1285. }
  1286. float st_get_position_mm(uint8_t axis)
  1287. {
  1288. float steper_position_in_steps = st_get_position(axis);
  1289. return steper_position_in_steps / cs.axis_steps_per_unit[axis];
  1290. }
  1291. void quickStop()
  1292. {
  1293. DISABLE_STEPPER_DRIVER_INTERRUPT();
  1294. while (blocks_queued()) plan_discard_current_block();
  1295. current_block = NULL;
  1296. #ifdef LIN_ADVANCE
  1297. nextAdvanceISR = ADV_NEVER;
  1298. current_adv_steps = 0;
  1299. #endif
  1300. st_reset_timer();
  1301. ENABLE_STEPPER_DRIVER_INTERRUPT();
  1302. }
  1303. #ifdef BABYSTEPPING
  1304. void babystep(const uint8_t axis,const bool direction)
  1305. {
  1306. // MUST ONLY BE CALLED BY A ISR as stepper pins are manipulated directly.
  1307. // note: when switching direction no delay is inserted at the end when the
  1308. // original is restored. We assume enough time passes as the function
  1309. // returns and the stepper is manipulated again (to avoid dead times)
  1310. switch(axis)
  1311. {
  1312. case X_AXIS:
  1313. {
  1314. enable_x();
  1315. uint8_t old_x_dir_pin = READ(X_DIR_PIN); //if dualzstepper, both point to same direction.
  1316. uint8_t new_x_dir_pin = (INVERT_X_DIR)^direction;
  1317. //setup new step
  1318. if (new_x_dir_pin != old_x_dir_pin) {
  1319. WRITE_NC(X_DIR_PIN, new_x_dir_pin);
  1320. delayMicroseconds(STEPPER_SET_DIR_DELAY);
  1321. }
  1322. //perform step
  1323. STEP_NC_HI(X_AXIS);
  1324. #ifdef DEBUG_XSTEP_DUP_PIN
  1325. STEP_NC_HI(X_DUP_AXIS);
  1326. #endif
  1327. STEPPER_MINIMUM_DELAY;
  1328. STEP_NC_LO(X_AXIS);
  1329. #ifdef DEBUG_XSTEP_DUP_PIN
  1330. STEP_NC_LO(X_DUP_AXIS);
  1331. #endif
  1332. //get old pin state back.
  1333. WRITE_NC(X_DIR_PIN, old_x_dir_pin);
  1334. }
  1335. break;
  1336. case Y_AXIS:
  1337. {
  1338. enable_y();
  1339. uint8_t old_y_dir_pin = READ(Y_DIR_PIN); //if dualzstepper, both point to same direction.
  1340. uint8_t new_y_dir_pin = (INVERT_Y_DIR)^direction;
  1341. //setup new step
  1342. if (new_y_dir_pin != old_y_dir_pin) {
  1343. WRITE_NC(Y_DIR_PIN, new_y_dir_pin);
  1344. delayMicroseconds(STEPPER_SET_DIR_DELAY);
  1345. }
  1346. //perform step
  1347. STEP_NC_HI(Y_AXIS);
  1348. #ifdef DEBUG_YSTEP_DUP_PIN
  1349. STEP_NC_HI(Y_DUP_AXIS);
  1350. #endif
  1351. STEPPER_MINIMUM_DELAY;
  1352. STEP_NC_LO(Y_AXIS);
  1353. #ifdef DEBUG_YSTEP_DUP_PIN
  1354. STEP_NC_LO(Y_DUP_AXIS);
  1355. #endif
  1356. //get old pin state back.
  1357. WRITE_NC(Y_DIR_PIN, old_y_dir_pin);
  1358. }
  1359. break;
  1360. case Z_AXIS:
  1361. {
  1362. enable_z();
  1363. uint8_t old_z_dir_pin = READ(Z_DIR_PIN); //if dualzstepper, both point to same direction.
  1364. uint8_t new_z_dir_pin = (INVERT_Z_DIR)^direction^BABYSTEP_INVERT_Z;
  1365. //setup new step
  1366. if (new_z_dir_pin != old_z_dir_pin) {
  1367. WRITE_NC(Z_DIR_PIN, new_z_dir_pin);
  1368. #ifdef Z_DUAL_STEPPER_DRIVERS
  1369. WRITE_NC(Z2_DIR_PIN, new_z_dir_pin);
  1370. #endif
  1371. delayMicroseconds(STEPPER_SET_DIR_DELAY);
  1372. }
  1373. //perform step
  1374. STEP_NC_HI(Z_AXIS);
  1375. #ifdef Z_DUAL_STEPPER_DRIVERS
  1376. STEP_NC_HI(Z2_AXIS);
  1377. #endif
  1378. STEPPER_MINIMUM_DELAY;
  1379. STEP_NC_LO(Z_AXIS);
  1380. #ifdef Z_DUAL_STEPPER_DRIVERS
  1381. STEP_NC_LO(Z2_AXIS);
  1382. #endif
  1383. //get old pin state back.
  1384. if (new_z_dir_pin != old_z_dir_pin) {
  1385. WRITE_NC(Z_DIR_PIN, old_z_dir_pin);
  1386. #ifdef Z_DUAL_STEPPER_DRIVERS
  1387. WRITE_NC(Z2_DIR_PIN, old_z_dir_pin);
  1388. #endif
  1389. }
  1390. }
  1391. break;
  1392. default: break;
  1393. }
  1394. }
  1395. #endif //BABYSTEPPING
  1396. #if defined(DIGIPOTSS_PIN) && DIGIPOTSS_PIN > -1
  1397. void digitalPotWrite(int address, int value) // From Arduino DigitalPotControl example
  1398. {
  1399. digitalWrite(DIGIPOTSS_PIN,LOW); // take the SS pin low to select the chip
  1400. SPI.transfer(address); // send in the address and value via SPI:
  1401. SPI.transfer(value);
  1402. digitalWrite(DIGIPOTSS_PIN,HIGH); // take the SS pin high to de-select the chip:
  1403. //_delay(10);
  1404. }
  1405. #endif
  1406. void st_current_init() //Initialize Digipot Motor Current
  1407. {
  1408. #ifdef MOTOR_CURRENT_PWM_XY_PIN
  1409. uint8_t SilentMode = eeprom_read_byte((uint8_t*)EEPROM_SILENT);
  1410. SilentModeMenu = SilentMode;
  1411. pinMode(MOTOR_CURRENT_PWM_XY_PIN, OUTPUT);
  1412. pinMode(MOTOR_CURRENT_PWM_Z_PIN, OUTPUT);
  1413. pinMode(MOTOR_CURRENT_PWM_E_PIN, OUTPUT);
  1414. if((SilentMode == SILENT_MODE_OFF) || (farm_mode) ){
  1415. motor_current_setting[0] = motor_current_setting_loud[0];
  1416. motor_current_setting[1] = motor_current_setting_loud[1];
  1417. motor_current_setting[2] = motor_current_setting_loud[2];
  1418. }else{
  1419. motor_current_setting[0] = motor_current_setting_silent[0];
  1420. motor_current_setting[1] = motor_current_setting_silent[1];
  1421. motor_current_setting[2] = motor_current_setting_silent[2];
  1422. }
  1423. st_current_set(0, motor_current_setting[0]);
  1424. st_current_set(1, motor_current_setting[1]);
  1425. st_current_set(2, motor_current_setting[2]);
  1426. //Set timer5 to 31khz so the PWM of the motor power is as constant as possible. (removes a buzzing noise)
  1427. TCCR5B = (TCCR5B & ~(_BV(CS50) | _BV(CS51) | _BV(CS52))) | _BV(CS50);
  1428. #endif
  1429. }
  1430. #ifdef MOTOR_CURRENT_PWM_XY_PIN
  1431. void st_current_set(uint8_t driver, int current)
  1432. {
  1433. if (driver == 0) analogWrite(MOTOR_CURRENT_PWM_XY_PIN, (long)current * 255L / (long)MOTOR_CURRENT_PWM_RANGE);
  1434. if (driver == 1) analogWrite(MOTOR_CURRENT_PWM_Z_PIN, (long)current * 255L / (long)MOTOR_CURRENT_PWM_RANGE);
  1435. if (driver == 2) analogWrite(MOTOR_CURRENT_PWM_E_PIN, (long)current * 255L / (long)MOTOR_CURRENT_PWM_RANGE);
  1436. }
  1437. #else //MOTOR_CURRENT_PWM_XY_PIN
  1438. void st_current_set(uint8_t, int ){}
  1439. #endif //MOTOR_CURRENT_PWM_XY_PIN
  1440. void microstep_init()
  1441. {
  1442. #if defined(E1_MS1_PIN) && E1_MS1_PIN > -1
  1443. pinMode(E1_MS1_PIN,OUTPUT);
  1444. pinMode(E1_MS2_PIN,OUTPUT);
  1445. #endif
  1446. #if defined(X_MS1_PIN) && X_MS1_PIN > -1
  1447. const uint8_t microstep_modes[] = MICROSTEP_MODES;
  1448. pinMode(X_MS1_PIN,OUTPUT);
  1449. pinMode(X_MS2_PIN,OUTPUT);
  1450. pinMode(Y_MS1_PIN,OUTPUT);
  1451. pinMode(Y_MS2_PIN,OUTPUT);
  1452. pinMode(Z_MS1_PIN,OUTPUT);
  1453. pinMode(Z_MS2_PIN,OUTPUT);
  1454. pinMode(E0_MS1_PIN,OUTPUT);
  1455. pinMode(E0_MS2_PIN,OUTPUT);
  1456. for(int i=0;i<=4;i++) microstep_mode(i,microstep_modes[i]);
  1457. #endif
  1458. }
  1459. #ifndef TMC2130
  1460. void microstep_ms(uint8_t driver, int8_t ms1, int8_t ms2)
  1461. {
  1462. if(ms1 > -1) switch(driver)
  1463. {
  1464. case 0: digitalWrite( X_MS1_PIN,ms1); break;
  1465. case 1: digitalWrite( Y_MS1_PIN,ms1); break;
  1466. case 2: digitalWrite( Z_MS1_PIN,ms1); break;
  1467. case 3: digitalWrite(E0_MS1_PIN,ms1); break;
  1468. #if defined(E1_MS1_PIN) && E1_MS1_PIN > -1
  1469. case 4: digitalWrite(E1_MS1_PIN,ms1); break;
  1470. #endif
  1471. }
  1472. if(ms2 > -1) switch(driver)
  1473. {
  1474. case 0: digitalWrite( X_MS2_PIN,ms2); break;
  1475. case 1: digitalWrite( Y_MS2_PIN,ms2); break;
  1476. case 2: digitalWrite( Z_MS2_PIN,ms2); break;
  1477. case 3: digitalWrite(E0_MS2_PIN,ms2); break;
  1478. #if defined(E1_MS2_PIN) && E1_MS2_PIN > -1
  1479. case 4: digitalWrite(E1_MS2_PIN,ms2); break;
  1480. #endif
  1481. }
  1482. }
  1483. void microstep_mode(uint8_t driver, uint8_t stepping_mode)
  1484. {
  1485. switch(stepping_mode)
  1486. {
  1487. case 1: microstep_ms(driver,MICROSTEP1); break;
  1488. case 2: microstep_ms(driver,MICROSTEP2); break;
  1489. case 4: microstep_ms(driver,MICROSTEP4); break;
  1490. case 8: microstep_ms(driver,MICROSTEP8); break;
  1491. case 16: microstep_ms(driver,MICROSTEP16); break;
  1492. }
  1493. }
  1494. void microstep_readings()
  1495. {
  1496. SERIAL_PROTOCOLPGM("MS1,MS2 Pins\n");
  1497. SERIAL_PROTOCOLPGM("X: ");
  1498. SERIAL_PROTOCOL( digitalRead(X_MS1_PIN));
  1499. SERIAL_PROTOCOLLN( digitalRead(X_MS2_PIN));
  1500. SERIAL_PROTOCOLPGM("Y: ");
  1501. SERIAL_PROTOCOL( digitalRead(Y_MS1_PIN));
  1502. SERIAL_PROTOCOLLN( digitalRead(Y_MS2_PIN));
  1503. SERIAL_PROTOCOLPGM("Z: ");
  1504. SERIAL_PROTOCOL( digitalRead(Z_MS1_PIN));
  1505. SERIAL_PROTOCOLLN( digitalRead(Z_MS2_PIN));
  1506. SERIAL_PROTOCOLPGM("E0: ");
  1507. SERIAL_PROTOCOL( digitalRead(E0_MS1_PIN));
  1508. SERIAL_PROTOCOLLN( digitalRead(E0_MS2_PIN));
  1509. #if defined(E1_MS1_PIN) && E1_MS1_PIN > -1
  1510. SERIAL_PROTOCOLPGM("E1: ");
  1511. SERIAL_PROTOCOL( digitalRead(E1_MS1_PIN));
  1512. SERIAL_PROTOCOLLN( digitalRead(E1_MS2_PIN));
  1513. #endif
  1514. }
  1515. #endif //TMC2130
  1516. #if defined(FILAMENT_SENSOR) && defined(PAT9125)
  1517. void st_reset_fsensor()
  1518. {
  1519. CRITICAL_SECTION_START;
  1520. fsensor_counter = 0;
  1521. CRITICAL_SECTION_END;
  1522. }
  1523. #endif //FILAMENT_SENSOR