stepper.cpp 51 KB

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