stepper.cpp 35 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826827828829830831832833834835836837838839840841842843844845846847848849850851852853854855856857858859860861862863864865866867868869870871872873874875876877878879880881882883884885886887888889890891892893894895896897898899900901902903904905906907908909910911912913914915916917918919920921922923924925926927928929930931932933934935936937938939940941942943944945946947948949950951952953954955956957958959960961962963964965966967968969970971972973974975976977978979980981982983984985986987988989990991992993994995996997998999100010011002100310041005100610071008100910101011101210131014101510161017101810191020102110221023102410251026102710281029103010311032103310341035103610371038103910401041104210431044104510461047104810491050105110521053105410551056105710581059106010611062106310641065106610671068106910701071107210731074107510761077107810791080108110821083108410851086108710881089109010911092109310941095109610971098109911001101110211031104110511061107110811091110111111121113111411151116111711181119112011211122112311241125112611271128112911301131113211331134113511361137113811391140114111421143114411451146114711481149115011511152115311541155115611571158115911601161116211631164116511661167116811691170117111721173117411751176117711781179118011811182118311841185118611871188118911901191119211931194119511961197119811991200120112021203120412051206120712081209121012111212121312141215121612171218121912201221
  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 HAVE_TMC2130_DRIVERS
  30. #include "tmc2130.h"
  31. #endif //HAVE_TMC2130_DRIVERS
  32. //===========================================================================
  33. //=============================public variables ============================
  34. //===========================================================================
  35. block_t *current_block; // A pointer to the block currently being traced
  36. //===========================================================================
  37. //=============================private variables ============================
  38. //===========================================================================
  39. //static makes it inpossible to be called from outside of this file by extern.!
  40. // Variables used by The Stepper Driver Interrupt
  41. static unsigned char out_bits; // The next stepping-bits to be output
  42. static int32_t counter_x, // Counter variables for the bresenham line tracer
  43. counter_y,
  44. counter_z,
  45. counter_e;
  46. volatile static uint32_t step_events_completed; // The number of step events executed in the current block
  47. static int32_t acceleration_time, deceleration_time;
  48. //static unsigned long accelerate_until, decelerate_after, acceleration_rate, initial_rate, final_rate, nominal_rate;
  49. static uint16_t acc_step_rate; // needed for deccelaration start point
  50. static uint8_t step_loops;
  51. static uint16_t OCR1A_nominal;
  52. static uint8_t step_loops_nominal;
  53. volatile long endstops_trigsteps[3]={0,0,0};
  54. volatile long endstops_stepsTotal,endstops_stepsDone;
  55. static volatile bool endstop_x_hit=false;
  56. static volatile bool endstop_y_hit=false;
  57. static volatile bool endstop_z_hit=false;
  58. #ifdef ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED
  59. bool abort_on_endstop_hit = false;
  60. #endif
  61. #ifdef MOTOR_CURRENT_PWM_XY_PIN
  62. int motor_current_setting[3] = DEFAULT_PWM_MOTOR_CURRENT;
  63. int motor_current_setting_silent[3] = DEFAULT_PWM_MOTOR_CURRENT;
  64. int motor_current_setting_loud[3] = DEFAULT_PWM_MOTOR_CURRENT_LOUD;
  65. #endif
  66. static bool old_x_min_endstop=false;
  67. static bool old_x_max_endstop=false;
  68. static bool old_y_min_endstop=false;
  69. static bool old_y_max_endstop=false;
  70. static bool old_z_min_endstop=false;
  71. static bool old_z_max_endstop=false;
  72. #ifdef TMC2130_SG_HOMING_SW
  73. static bool check_endstops = false;
  74. #else
  75. static bool check_endstops = true;
  76. #endif
  77. static bool check_z_endstop = false;
  78. int8_t SilentMode;
  79. volatile long count_position[NUM_AXIS] = { 0, 0, 0, 0};
  80. volatile signed char count_direction[NUM_AXIS] = { 1, 1, 1, 1};
  81. //===========================================================================
  82. //=============================functions ============================
  83. //===========================================================================
  84. #define CHECK_ENDSTOPS if(check_endstops)
  85. // intRes = intIn1 * intIn2 >> 16
  86. // uses:
  87. // r26 to store 0
  88. // r27 to store the byte 1 of the 24 bit result
  89. #define MultiU16X8toH16(intRes, charIn1, intIn2) \
  90. asm volatile ( \
  91. "clr r26 \n\t" \
  92. "mul %A1, %B2 \n\t" \
  93. "movw %A0, r0 \n\t" \
  94. "mul %A1, %A2 \n\t" \
  95. "add %A0, r1 \n\t" \
  96. "adc %B0, r26 \n\t" \
  97. "lsr r0 \n\t" \
  98. "adc %A0, r26 \n\t" \
  99. "adc %B0, r26 \n\t" \
  100. "clr r1 \n\t" \
  101. : \
  102. "=&r" (intRes) \
  103. : \
  104. "d" (charIn1), \
  105. "d" (intIn2) \
  106. : \
  107. "r26" \
  108. )
  109. // intRes = longIn1 * longIn2 >> 24
  110. // uses:
  111. // r26 to store 0
  112. // r27 to store the byte 1 of the 48bit result
  113. #define MultiU24X24toH16(intRes, longIn1, longIn2) \
  114. asm volatile ( \
  115. "clr r26 \n\t" \
  116. "mul %A1, %B2 \n\t" \
  117. "mov r27, r1 \n\t" \
  118. "mul %B1, %C2 \n\t" \
  119. "movw %A0, r0 \n\t" \
  120. "mul %C1, %C2 \n\t" \
  121. "add %B0, r0 \n\t" \
  122. "mul %C1, %B2 \n\t" \
  123. "add %A0, r0 \n\t" \
  124. "adc %B0, r1 \n\t" \
  125. "mul %A1, %C2 \n\t" \
  126. "add r27, r0 \n\t" \
  127. "adc %A0, r1 \n\t" \
  128. "adc %B0, r26 \n\t" \
  129. "mul %B1, %B2 \n\t" \
  130. "add r27, r0 \n\t" \
  131. "adc %A0, r1 \n\t" \
  132. "adc %B0, r26 \n\t" \
  133. "mul %C1, %A2 \n\t" \
  134. "add r27, r0 \n\t" \
  135. "adc %A0, r1 \n\t" \
  136. "adc %B0, r26 \n\t" \
  137. "mul %B1, %A2 \n\t" \
  138. "add r27, r1 \n\t" \
  139. "adc %A0, r26 \n\t" \
  140. "adc %B0, r26 \n\t" \
  141. "lsr r27 \n\t" \
  142. "adc %A0, r26 \n\t" \
  143. "adc %B0, r26 \n\t" \
  144. "clr r1 \n\t" \
  145. : \
  146. "=&r" (intRes) \
  147. : \
  148. "d" (longIn1), \
  149. "d" (longIn2) \
  150. : \
  151. "r26" , "r27" \
  152. )
  153. // Some useful constants
  154. #define ENABLE_STEPPER_DRIVER_INTERRUPT() TIMSK1 |= (1<<OCIE1A)
  155. #define DISABLE_STEPPER_DRIVER_INTERRUPT() TIMSK1 &= ~(1<<OCIE1A)
  156. void checkHitEndstops()
  157. {
  158. if( endstop_x_hit || endstop_y_hit || endstop_z_hit) {
  159. SERIAL_ECHO_START;
  160. SERIAL_ECHORPGM(MSG_ENDSTOPS_HIT);
  161. if(endstop_x_hit) {
  162. SERIAL_ECHOPAIR(" X:",(float)endstops_trigsteps[X_AXIS]/axis_steps_per_unit[X_AXIS]);
  163. LCD_MESSAGERPGM(CAT2(MSG_ENDSTOPS_HIT, PSTR("X")));
  164. }
  165. if(endstop_y_hit) {
  166. SERIAL_ECHOPAIR(" Y:",(float)endstops_trigsteps[Y_AXIS]/axis_steps_per_unit[Y_AXIS]);
  167. LCD_MESSAGERPGM(CAT2(MSG_ENDSTOPS_HIT, PSTR("Y")));
  168. }
  169. if(endstop_z_hit) {
  170. SERIAL_ECHOPAIR(" Z:",(float)endstops_trigsteps[Z_AXIS]/axis_steps_per_unit[Z_AXIS]);
  171. LCD_MESSAGERPGM(CAT2(MSG_ENDSTOPS_HIT,PSTR("Z")));
  172. }
  173. SERIAL_ECHOLN("");
  174. endstop_x_hit=false;
  175. endstop_y_hit=false;
  176. endstop_z_hit=false;
  177. #if defined(ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED) && defined(SDSUPPORT)
  178. if (abort_on_endstop_hit)
  179. {
  180. card.sdprinting = false;
  181. card.closefile();
  182. quickStop();
  183. setTargetHotend0(0);
  184. setTargetHotend1(0);
  185. setTargetHotend2(0);
  186. }
  187. #endif
  188. }
  189. }
  190. bool endstops_hit_on_purpose()
  191. {
  192. bool hit = endstop_x_hit || endstop_y_hit || endstop_z_hit;
  193. endstop_x_hit=false;
  194. endstop_y_hit=false;
  195. endstop_z_hit=false;
  196. return hit;
  197. }
  198. bool endstop_z_hit_on_purpose()
  199. {
  200. bool hit = endstop_z_hit;
  201. endstop_z_hit=false;
  202. return hit;
  203. }
  204. bool enable_endstops(bool check)
  205. {
  206. bool old = check_endstops;
  207. check_endstops = check;
  208. return old;
  209. }
  210. bool enable_z_endstop(bool check)
  211. {
  212. bool old = check_z_endstop;
  213. check_z_endstop = check;
  214. endstop_z_hit=false;
  215. return old;
  216. }
  217. // __________________________
  218. // /| |\ _________________ ^
  219. // / | | \ /| |\ |
  220. // / | | \ / | | \ s
  221. // / | | | | | \ p
  222. // / | | | | | \ e
  223. // +-----+------------------------+---+--+---------------+----+ e
  224. // | BLOCK 1 | BLOCK 2 | d
  225. //
  226. // time ----->
  227. //
  228. // The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates
  229. // first block->accelerate_until step_events_completed, then keeps going at constant speed until
  230. // step_events_completed reaches block->decelerate_after after which it decelerates until the trapezoid generator is reset.
  231. // The slope of acceleration is calculated with the leib ramp alghorithm.
  232. void st_wake_up() {
  233. // TCNT1 = 0;
  234. ENABLE_STEPPER_DRIVER_INTERRUPT();
  235. }
  236. void step_wait(){
  237. for(int8_t i=0; i < 6; i++){
  238. }
  239. }
  240. FORCE_INLINE unsigned short calc_timer(unsigned short step_rate) {
  241. unsigned short timer;
  242. if(step_rate > MAX_STEP_FREQUENCY) step_rate = MAX_STEP_FREQUENCY;
  243. if(step_rate > 20000) { // If steprate > 20kHz >> step 4 times
  244. step_rate = (step_rate >> 2)&0x3fff;
  245. step_loops = 4;
  246. }
  247. else if(step_rate > 10000) { // If steprate > 10kHz >> step 2 times
  248. step_rate = (step_rate >> 1)&0x7fff;
  249. step_loops = 2;
  250. }
  251. else {
  252. step_loops = 1;
  253. }
  254. // step_loops = 1;
  255. if(step_rate < (F_CPU/500000)) step_rate = (F_CPU/500000);
  256. step_rate -= (F_CPU/500000); // Correct for minimal speed
  257. if(step_rate >= (8*256)){ // higher step rate
  258. unsigned short table_address = (unsigned short)&speed_lookuptable_fast[(unsigned char)(step_rate>>8)][0];
  259. unsigned char tmp_step_rate = (step_rate & 0x00ff);
  260. unsigned short gain = (unsigned short)pgm_read_word_near(table_address+2);
  261. MultiU16X8toH16(timer, tmp_step_rate, gain);
  262. timer = (unsigned short)pgm_read_word_near(table_address) - timer;
  263. }
  264. else { // lower step rates
  265. unsigned short table_address = (unsigned short)&speed_lookuptable_slow[0][0];
  266. table_address += ((step_rate)>>1) & 0xfffc;
  267. timer = (unsigned short)pgm_read_word_near(table_address);
  268. timer -= (((unsigned short)pgm_read_word_near(table_address+2) * (unsigned char)(step_rate & 0x0007))>>3);
  269. }
  270. if(timer < 100) { timer = 100; MYSERIAL.print(MSG_STEPPER_TOO_HIGH); MYSERIAL.println(step_rate); }//(20kHz this should never happen)
  271. return timer;
  272. }
  273. // Initializes the trapezoid generator from the current block. Called whenever a new
  274. // block begins.
  275. FORCE_INLINE void trapezoid_generator_reset() {
  276. deceleration_time = 0;
  277. // step_rate to timer interval
  278. OCR1A_nominal = calc_timer(current_block->nominal_rate);
  279. // make a note of the number of step loops required at nominal speed
  280. step_loops_nominal = step_loops;
  281. acc_step_rate = current_block->initial_rate;
  282. acceleration_time = calc_timer(acc_step_rate);
  283. OCR1A = acceleration_time;
  284. // SERIAL_ECHO_START;
  285. // SERIAL_ECHOPGM("advance :");
  286. // SERIAL_ECHO(current_block->advance/256.0);
  287. // SERIAL_ECHOPGM("advance rate :");
  288. // SERIAL_ECHO(current_block->advance_rate/256.0);
  289. // SERIAL_ECHOPGM("initial advance :");
  290. // SERIAL_ECHO(current_block->initial_advance/256.0);
  291. // SERIAL_ECHOPGM("final advance :");
  292. // SERIAL_ECHOLN(current_block->final_advance/256.0);
  293. }
  294. // "The Stepper Driver Interrupt" - This timer interrupt is the workhorse.
  295. // It pops blocks from the block_buffer and executes them by pulsing the stepper pins appropriately.
  296. ISR(TIMER1_COMPA_vect)
  297. {
  298. // If there is no current block, attempt to pop one from the buffer
  299. if (current_block == NULL) {
  300. // Anything in the buffer?
  301. current_block = plan_get_current_block();
  302. if (current_block != NULL) {
  303. // The busy flag is set by the plan_get_current_block() call.
  304. // current_block->busy = true;
  305. trapezoid_generator_reset();
  306. counter_x = -(current_block->step_event_count >> 1);
  307. counter_y = counter_x;
  308. counter_z = counter_x;
  309. counter_e = counter_x;
  310. step_events_completed = 0;
  311. #ifdef Z_LATE_ENABLE
  312. if(current_block->steps_z > 0) {
  313. enable_z();
  314. OCR1A = 2000; //1ms wait
  315. return;
  316. }
  317. #endif
  318. }
  319. else {
  320. OCR1A=2000; // 1kHz.
  321. }
  322. }
  323. if (current_block != NULL) {
  324. // Set directions TO DO This should be done once during init of trapezoid. Endstops -> interrupt
  325. out_bits = current_block->direction_bits;
  326. // Set the direction bits (X_AXIS=A_AXIS and Y_AXIS=B_AXIS for COREXY)
  327. if((out_bits & (1<<X_AXIS))!=0){
  328. WRITE(X_DIR_PIN, INVERT_X_DIR);
  329. count_direction[X_AXIS]=-1;
  330. }
  331. else{
  332. WRITE(X_DIR_PIN, !INVERT_X_DIR);
  333. count_direction[X_AXIS]=1;
  334. }
  335. if((out_bits & (1<<Y_AXIS))!=0){
  336. WRITE(Y_DIR_PIN, INVERT_Y_DIR);
  337. #ifdef Y_DUAL_STEPPER_DRIVERS
  338. WRITE(Y2_DIR_PIN, !(INVERT_Y_DIR == INVERT_Y2_VS_Y_DIR));
  339. #endif
  340. count_direction[Y_AXIS]=-1;
  341. }
  342. else{
  343. WRITE(Y_DIR_PIN, !INVERT_Y_DIR);
  344. #ifdef Y_DUAL_STEPPER_DRIVERS
  345. WRITE(Y2_DIR_PIN, (INVERT_Y_DIR == INVERT_Y2_VS_Y_DIR));
  346. #endif
  347. count_direction[Y_AXIS]=1;
  348. }
  349. // Set direction en check limit switches
  350. #ifndef COREXY
  351. if ((out_bits & (1<<X_AXIS)) != 0) { // stepping along -X axis
  352. #else
  353. if ((((out_bits & (1<<X_AXIS)) != 0)&&(out_bits & (1<<Y_AXIS)) != 0)) { //-X occurs for -A and -B
  354. #endif
  355. CHECK_ENDSTOPS
  356. {
  357. {
  358. #if defined(X_MIN_PIN) && X_MIN_PIN > -1
  359. #ifndef TMC2130_SG_HOMING_SW
  360. bool x_min_endstop = (READ(X_MIN_PIN) != X_MIN_ENDSTOP_INVERTING);
  361. #else //TMC2130_SG_HOMING_SW
  362. bool x_min_endstop = tmc2130_axis_stalled[X_AXIS];
  363. #endif //TMC2130_SG_HOMING_SW
  364. if(x_min_endstop && old_x_min_endstop && (current_block->steps_x > 0)) {
  365. endstops_trigsteps[X_AXIS] = count_position[X_AXIS];
  366. endstop_x_hit=true;
  367. step_events_completed = current_block->step_event_count;
  368. }
  369. old_x_min_endstop = x_min_endstop;
  370. #endif
  371. }
  372. }
  373. }
  374. else { // +direction
  375. CHECK_ENDSTOPS
  376. {
  377. {
  378. #if defined(X_MAX_PIN) && X_MAX_PIN > -1
  379. #ifndef TMC2130_SG_HOMING_SW
  380. bool x_max_endstop = (READ(X_MAX_PIN) != X_MAX_ENDSTOP_INVERTING);
  381. #else //TMC2130_SG_HOMING_SW
  382. bool x_max_endstop = tmc2130_axis_stalled[X_AXIS];
  383. #endif //TMC2130_SG_HOMING_SW
  384. if(x_max_endstop && old_x_max_endstop && (current_block->steps_x > 0)){
  385. endstops_trigsteps[X_AXIS] = count_position[X_AXIS];
  386. endstop_x_hit=true;
  387. step_events_completed = current_block->step_event_count;
  388. }
  389. old_x_max_endstop = x_max_endstop;
  390. #endif
  391. }
  392. }
  393. }
  394. #ifndef COREXY
  395. if ((out_bits & (1<<Y_AXIS)) != 0) { // -direction
  396. #else
  397. if ((((out_bits & (1<<X_AXIS)) != 0)&&(out_bits & (1<<Y_AXIS)) == 0)) { // -Y occurs for -A and +B
  398. #endif
  399. CHECK_ENDSTOPS
  400. {
  401. #if defined(Y_MIN_PIN) && Y_MIN_PIN > -1
  402. #ifndef TMC2130_SG_HOMING_SW
  403. bool y_min_endstop=(READ(Y_MIN_PIN) != Y_MIN_ENDSTOP_INVERTING);
  404. #else //TMC2130_SG_HOMING_SW
  405. bool y_min_endstop = tmc2130_axis_stalled[Y_AXIS];
  406. #endif //TMC2130_SG_HOMING_SW
  407. if(y_min_endstop && old_y_min_endstop && (current_block->steps_y > 0)) {
  408. endstops_trigsteps[Y_AXIS] = count_position[Y_AXIS];
  409. endstop_y_hit=true;
  410. step_events_completed = current_block->step_event_count;
  411. }
  412. old_y_min_endstop = y_min_endstop;
  413. #endif
  414. }
  415. }
  416. else { // +direction
  417. CHECK_ENDSTOPS
  418. {
  419. #if defined(Y_MAX_PIN) && Y_MAX_PIN > -1
  420. #ifndef TMC2130_SG_HOMING_SW
  421. bool y_max_endstop=(READ(Y_MAX_PIN) != Y_MAX_ENDSTOP_INVERTING);
  422. #else //TMC2130_SG_HOMING_SW
  423. bool y_max_endstop = tmc2130_axis_stalled[Y_AXIS];
  424. #endif //TMC2130_SG_HOMING_SW
  425. if(y_max_endstop && old_y_max_endstop && (current_block->steps_y > 0)){
  426. endstops_trigsteps[Y_AXIS] = count_position[Y_AXIS];
  427. endstop_y_hit=true;
  428. step_events_completed = current_block->step_event_count;
  429. }
  430. old_y_max_endstop = y_max_endstop;
  431. #endif
  432. }
  433. }
  434. if ((out_bits & (1<<Z_AXIS)) != 0) { // -direction
  435. WRITE(Z_DIR_PIN,INVERT_Z_DIR);
  436. #ifdef Z_DUAL_STEPPER_DRIVERS
  437. WRITE(Z2_DIR_PIN,INVERT_Z_DIR);
  438. #endif
  439. count_direction[Z_AXIS]=-1;
  440. if(check_endstops && ! check_z_endstop)
  441. {
  442. #if defined(Z_MIN_PIN) && Z_MIN_PIN > -1
  443. bool z_min_endstop=(READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING);
  444. if(z_min_endstop && old_z_min_endstop && (current_block->steps_z > 0)) {
  445. endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS];
  446. endstop_z_hit=true;
  447. step_events_completed = current_block->step_event_count;
  448. }
  449. old_z_min_endstop = z_min_endstop;
  450. #endif
  451. }
  452. }
  453. else { // +direction
  454. WRITE(Z_DIR_PIN,!INVERT_Z_DIR);
  455. #ifdef Z_DUAL_STEPPER_DRIVERS
  456. WRITE(Z2_DIR_PIN,!INVERT_Z_DIR);
  457. #endif
  458. count_direction[Z_AXIS]=1;
  459. CHECK_ENDSTOPS
  460. {
  461. #if defined(Z_MAX_PIN) && Z_MAX_PIN > -1
  462. bool z_max_endstop=(READ(Z_MAX_PIN) != Z_MAX_ENDSTOP_INVERTING);
  463. if(z_max_endstop && old_z_max_endstop && (current_block->steps_z > 0)) {
  464. endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS];
  465. endstop_z_hit=true;
  466. step_events_completed = current_block->step_event_count;
  467. }
  468. old_z_max_endstop = z_max_endstop;
  469. #endif
  470. }
  471. }
  472. // Supporting stopping on a trigger of the Z-stop induction sensor, not only for the Z-minus movements.
  473. #if defined(Z_MIN_PIN) && Z_MIN_PIN > -1
  474. if(check_z_endstop) {
  475. // Check the Z min end-stop no matter what.
  476. // Good for searching for the center of an induction target.
  477. bool z_min_endstop=(READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING);
  478. if(z_min_endstop && old_z_min_endstop) {
  479. endstops_trigsteps[Z_AXIS] = count_position[Z_AXIS];
  480. endstop_z_hit=true;
  481. step_events_completed = current_block->step_event_count;
  482. }
  483. old_z_min_endstop = z_min_endstop;
  484. }
  485. #endif
  486. if ((out_bits & (1 << E_AXIS)) != 0)
  487. { // -direction
  488. //AKU
  489. #ifdef SNMM
  490. if (snmm_extruder == 0 || snmm_extruder == 2)
  491. {
  492. NORM_E_DIR();
  493. }
  494. else
  495. {
  496. REV_E_DIR();
  497. }
  498. #else
  499. REV_E_DIR();
  500. #endif // SNMM
  501. count_direction[E_AXIS] = -1;
  502. }
  503. else
  504. { // +direction
  505. #ifdef SNMM
  506. if (snmm_extruder == 0 || snmm_extruder == 2)
  507. {
  508. REV_E_DIR();
  509. }
  510. else
  511. {
  512. NORM_E_DIR();
  513. }
  514. #else
  515. NORM_E_DIR();
  516. #endif // SNMM
  517. count_direction[E_AXIS] = 1;
  518. }
  519. for(uint8_t i=0; i < step_loops; i++) { // Take multiple steps per interrupt (For high speed moves)
  520. #ifndef AT90USB
  521. MSerial.checkRx(); // Check for serial chars.
  522. #endif
  523. counter_x += current_block->steps_x;
  524. if (counter_x > 0) {
  525. WRITE(X_STEP_PIN, !INVERT_X_STEP_PIN);
  526. counter_x -= current_block->step_event_count;
  527. count_position[X_AXIS]+=count_direction[X_AXIS];
  528. WRITE(X_STEP_PIN, INVERT_X_STEP_PIN);
  529. }
  530. counter_y += current_block->steps_y;
  531. if (counter_y > 0) {
  532. WRITE(Y_STEP_PIN, !INVERT_Y_STEP_PIN);
  533. #ifdef Y_DUAL_STEPPER_DRIVERS
  534. WRITE(Y2_STEP_PIN, !INVERT_Y_STEP_PIN);
  535. #endif
  536. counter_y -= current_block->step_event_count;
  537. count_position[Y_AXIS]+=count_direction[Y_AXIS];
  538. WRITE(Y_STEP_PIN, INVERT_Y_STEP_PIN);
  539. #ifdef Y_DUAL_STEPPER_DRIVERS
  540. WRITE(Y2_STEP_PIN, INVERT_Y_STEP_PIN);
  541. #endif
  542. }
  543. counter_z += current_block->steps_z;
  544. if (counter_z > 0) {
  545. WRITE(Z_STEP_PIN, !INVERT_Z_STEP_PIN);
  546. #ifdef Z_DUAL_STEPPER_DRIVERS
  547. WRITE(Z2_STEP_PIN, !INVERT_Z_STEP_PIN);
  548. #endif
  549. counter_z -= current_block->step_event_count;
  550. count_position[Z_AXIS]+=count_direction[Z_AXIS];
  551. WRITE(Z_STEP_PIN, INVERT_Z_STEP_PIN);
  552. #ifdef Z_DUAL_STEPPER_DRIVERS
  553. WRITE(Z2_STEP_PIN, INVERT_Z_STEP_PIN);
  554. #endif
  555. }
  556. counter_e += current_block->steps_e;
  557. if (counter_e > 0) {
  558. WRITE_E_STEP(!INVERT_E_STEP_PIN);
  559. counter_e -= current_block->step_event_count;
  560. count_position[E_AXIS]+=count_direction[E_AXIS];
  561. WRITE_E_STEP(INVERT_E_STEP_PIN);
  562. }
  563. step_events_completed += 1;
  564. if(step_events_completed >= current_block->step_event_count) break;
  565. }
  566. // Calculare new timer value
  567. unsigned short timer;
  568. unsigned short step_rate;
  569. if (step_events_completed <= (unsigned long int)current_block->accelerate_until) {
  570. // v = t * a -> acc_step_rate = acceleration_time * current_block->acceleration_rate
  571. MultiU24X24toH16(acc_step_rate, acceleration_time, current_block->acceleration_rate);
  572. acc_step_rate += current_block->initial_rate;
  573. // upper limit
  574. if(acc_step_rate > current_block->nominal_rate)
  575. acc_step_rate = current_block->nominal_rate;
  576. // step_rate to timer interval
  577. timer = calc_timer(acc_step_rate);
  578. OCR1A = timer;
  579. acceleration_time += timer;
  580. }
  581. else if (step_events_completed > (unsigned long int)current_block->decelerate_after) {
  582. MultiU24X24toH16(step_rate, deceleration_time, current_block->acceleration_rate);
  583. if(step_rate > acc_step_rate) { // Check step_rate stays positive
  584. step_rate = current_block->final_rate;
  585. }
  586. else {
  587. step_rate = acc_step_rate - step_rate; // Decelerate from aceleration end point.
  588. }
  589. // lower limit
  590. if(step_rate < current_block->final_rate)
  591. step_rate = current_block->final_rate;
  592. // step_rate to timer interval
  593. timer = calc_timer(step_rate);
  594. OCR1A = timer;
  595. deceleration_time += timer;
  596. }
  597. else {
  598. OCR1A = OCR1A_nominal;
  599. // ensure we're running at the correct step rate, even if we just came off an acceleration
  600. step_loops = step_loops_nominal;
  601. }
  602. // If current block is finished, reset pointer
  603. if (step_events_completed >= current_block->step_event_count) {
  604. current_block = NULL;
  605. plan_discard_current_block();
  606. }
  607. }
  608. check_fans();
  609. }
  610. void st_init()
  611. {
  612. #ifdef HAVE_TMC2130_DRIVERS
  613. tmc2130_init();
  614. #endif //HAVE_TMC2130_DRIVERS
  615. digipot_init(); //Initialize Digipot Motor Current
  616. microstep_init(); //Initialize Microstepping Pins
  617. //Initialize Dir Pins
  618. #if defined(X_DIR_PIN) && X_DIR_PIN > -1
  619. SET_OUTPUT(X_DIR_PIN);
  620. #endif
  621. #if defined(X2_DIR_PIN) && X2_DIR_PIN > -1
  622. SET_OUTPUT(X2_DIR_PIN);
  623. #endif
  624. #if defined(Y_DIR_PIN) && Y_DIR_PIN > -1
  625. SET_OUTPUT(Y_DIR_PIN);
  626. #if defined(Y_DUAL_STEPPER_DRIVERS) && defined(Y2_DIR_PIN) && (Y2_DIR_PIN > -1)
  627. SET_OUTPUT(Y2_DIR_PIN);
  628. #endif
  629. #endif
  630. #if defined(Z_DIR_PIN) && Z_DIR_PIN > -1
  631. SET_OUTPUT(Z_DIR_PIN);
  632. #if defined(Z_DUAL_STEPPER_DRIVERS) && defined(Z2_DIR_PIN) && (Z2_DIR_PIN > -1)
  633. SET_OUTPUT(Z2_DIR_PIN);
  634. #endif
  635. #endif
  636. #if defined(E0_DIR_PIN) && E0_DIR_PIN > -1
  637. SET_OUTPUT(E0_DIR_PIN);
  638. #endif
  639. #if defined(E1_DIR_PIN) && (E1_DIR_PIN > -1)
  640. SET_OUTPUT(E1_DIR_PIN);
  641. #endif
  642. #if defined(E2_DIR_PIN) && (E2_DIR_PIN > -1)
  643. SET_OUTPUT(E2_DIR_PIN);
  644. #endif
  645. //Initialize Enable Pins - steppers default to disabled.
  646. #if defined(X_ENABLE_PIN) && X_ENABLE_PIN > -1
  647. SET_OUTPUT(X_ENABLE_PIN);
  648. if(!X_ENABLE_ON) WRITE(X_ENABLE_PIN,HIGH);
  649. #endif
  650. #if defined(X2_ENABLE_PIN) && X2_ENABLE_PIN > -1
  651. SET_OUTPUT(X2_ENABLE_PIN);
  652. if(!X_ENABLE_ON) WRITE(X2_ENABLE_PIN,HIGH);
  653. #endif
  654. #if defined(Y_ENABLE_PIN) && Y_ENABLE_PIN > -1
  655. SET_OUTPUT(Y_ENABLE_PIN);
  656. if(!Y_ENABLE_ON) WRITE(Y_ENABLE_PIN,HIGH);
  657. #if defined(Y_DUAL_STEPPER_DRIVERS) && defined(Y2_ENABLE_PIN) && (Y2_ENABLE_PIN > -1)
  658. SET_OUTPUT(Y2_ENABLE_PIN);
  659. if(!Y_ENABLE_ON) WRITE(Y2_ENABLE_PIN,HIGH);
  660. #endif
  661. #endif
  662. #if defined(Z_ENABLE_PIN) && Z_ENABLE_PIN > -1
  663. SET_OUTPUT(Z_ENABLE_PIN);
  664. if(!Z_ENABLE_ON) WRITE(Z_ENABLE_PIN,HIGH);
  665. #if defined(Z_DUAL_STEPPER_DRIVERS) && defined(Z2_ENABLE_PIN) && (Z2_ENABLE_PIN > -1)
  666. SET_OUTPUT(Z2_ENABLE_PIN);
  667. if(!Z_ENABLE_ON) WRITE(Z2_ENABLE_PIN,HIGH);
  668. #endif
  669. #endif
  670. #if defined(E0_ENABLE_PIN) && (E0_ENABLE_PIN > -1)
  671. SET_OUTPUT(E0_ENABLE_PIN);
  672. if(!E_ENABLE_ON) WRITE(E0_ENABLE_PIN,HIGH);
  673. #endif
  674. #if defined(E1_ENABLE_PIN) && (E1_ENABLE_PIN > -1)
  675. SET_OUTPUT(E1_ENABLE_PIN);
  676. if(!E_ENABLE_ON) WRITE(E1_ENABLE_PIN,HIGH);
  677. #endif
  678. #if defined(E2_ENABLE_PIN) && (E2_ENABLE_PIN > -1)
  679. SET_OUTPUT(E2_ENABLE_PIN);
  680. if(!E_ENABLE_ON) WRITE(E2_ENABLE_PIN,HIGH);
  681. #endif
  682. //endstops and pullups
  683. #if defined(X_MIN_PIN) && X_MIN_PIN > -1
  684. SET_INPUT(X_MIN_PIN);
  685. #ifdef ENDSTOPPULLUP_XMIN
  686. WRITE(X_MIN_PIN,HIGH);
  687. #endif
  688. #endif
  689. #if defined(Y_MIN_PIN) && Y_MIN_PIN > -1
  690. SET_INPUT(Y_MIN_PIN);
  691. #ifdef ENDSTOPPULLUP_YMIN
  692. WRITE(Y_MIN_PIN,HIGH);
  693. #endif
  694. #endif
  695. #if defined(Z_MIN_PIN) && Z_MIN_PIN > -1
  696. SET_INPUT(Z_MIN_PIN);
  697. #ifdef ENDSTOPPULLUP_ZMIN
  698. WRITE(Z_MIN_PIN,HIGH);
  699. #endif
  700. #endif
  701. #if defined(X_MAX_PIN) && X_MAX_PIN > -1
  702. SET_INPUT(X_MAX_PIN);
  703. #ifdef ENDSTOPPULLUP_XMAX
  704. WRITE(X_MAX_PIN,HIGH);
  705. #endif
  706. #endif
  707. #if defined(Y_MAX_PIN) && Y_MAX_PIN > -1
  708. SET_INPUT(Y_MAX_PIN);
  709. #ifdef ENDSTOPPULLUP_YMAX
  710. WRITE(Y_MAX_PIN,HIGH);
  711. #endif
  712. #endif
  713. #if defined(Z_MAX_PIN) && Z_MAX_PIN > -1
  714. SET_INPUT(Z_MAX_PIN);
  715. #ifdef ENDSTOPPULLUP_ZMAX
  716. WRITE(Z_MAX_PIN,HIGH);
  717. #endif
  718. #endif
  719. //Initialize Step Pins
  720. #if defined(X_STEP_PIN) && (X_STEP_PIN > -1)
  721. SET_OUTPUT(X_STEP_PIN);
  722. WRITE(X_STEP_PIN,INVERT_X_STEP_PIN);
  723. disable_x();
  724. #endif
  725. #if defined(X2_STEP_PIN) && (X2_STEP_PIN > -1)
  726. SET_OUTPUT(X2_STEP_PIN);
  727. WRITE(X2_STEP_PIN,INVERT_X_STEP_PIN);
  728. disable_x();
  729. #endif
  730. #if defined(Y_STEP_PIN) && (Y_STEP_PIN > -1)
  731. SET_OUTPUT(Y_STEP_PIN);
  732. WRITE(Y_STEP_PIN,INVERT_Y_STEP_PIN);
  733. #if defined(Y_DUAL_STEPPER_DRIVERS) && defined(Y2_STEP_PIN) && (Y2_STEP_PIN > -1)
  734. SET_OUTPUT(Y2_STEP_PIN);
  735. WRITE(Y2_STEP_PIN,INVERT_Y_STEP_PIN);
  736. #endif
  737. disable_y();
  738. #endif
  739. #if defined(Z_STEP_PIN) && (Z_STEP_PIN > -1)
  740. SET_OUTPUT(Z_STEP_PIN);
  741. WRITE(Z_STEP_PIN,INVERT_Z_STEP_PIN);
  742. #if defined(Z_DUAL_STEPPER_DRIVERS) && defined(Z2_STEP_PIN) && (Z2_STEP_PIN > -1)
  743. SET_OUTPUT(Z2_STEP_PIN);
  744. WRITE(Z2_STEP_PIN,INVERT_Z_STEP_PIN);
  745. #endif
  746. disable_z();
  747. #endif
  748. #if defined(E0_STEP_PIN) && (E0_STEP_PIN > -1)
  749. SET_OUTPUT(E0_STEP_PIN);
  750. WRITE(E0_STEP_PIN,INVERT_E_STEP_PIN);
  751. disable_e0();
  752. #endif
  753. #if defined(E1_STEP_PIN) && (E1_STEP_PIN > -1)
  754. SET_OUTPUT(E1_STEP_PIN);
  755. WRITE(E1_STEP_PIN,INVERT_E_STEP_PIN);
  756. disable_e1();
  757. #endif
  758. #if defined(E2_STEP_PIN) && (E2_STEP_PIN > -1)
  759. SET_OUTPUT(E2_STEP_PIN);
  760. WRITE(E2_STEP_PIN,INVERT_E_STEP_PIN);
  761. disable_e2();
  762. #endif
  763. // waveform generation = 0100 = CTC
  764. TCCR1B &= ~(1<<WGM13);
  765. TCCR1B |= (1<<WGM12);
  766. TCCR1A &= ~(1<<WGM11);
  767. TCCR1A &= ~(1<<WGM10);
  768. // output mode = 00 (disconnected)
  769. TCCR1A &= ~(3<<COM1A0);
  770. TCCR1A &= ~(3<<COM1B0);
  771. // Set the timer pre-scaler
  772. // Generally we use a divider of 8, resulting in a 2MHz timer
  773. // frequency on a 16MHz MCU. If you are going to change this, be
  774. // sure to regenerate speed_lookuptable.h with
  775. // create_speed_lookuptable.py
  776. TCCR1B = (TCCR1B & ~(0x07<<CS10)) | (2<<CS10);
  777. OCR1A = 0x4000;
  778. TCNT1 = 0;
  779. ENABLE_STEPPER_DRIVER_INTERRUPT();
  780. enable_endstops(true); // Start with endstops active. After homing they can be disabled
  781. sei();
  782. }
  783. // Block until all buffered steps are executed
  784. void st_synchronize()
  785. {
  786. while(blocks_queued())
  787. {
  788. #ifdef HAVE_TMC2130_DRIVERS
  789. manage_heater();
  790. // Vojtech: Don't disable motors inside the planner!
  791. if (!tmc2130_update_sg())
  792. {
  793. manage_inactivity(true);
  794. lcd_update();
  795. }
  796. #else //HAVE_TMC2130_DRIVERS
  797. manage_heater();
  798. // Vojtech: Don't disable motors inside the planner!
  799. manage_inactivity(true);
  800. lcd_update();
  801. #endif //HAVE_TMC2130_DRIVERS
  802. }
  803. }
  804. void st_set_position(const long &x, const long &y, const long &z, const long &e)
  805. {
  806. CRITICAL_SECTION_START;
  807. count_position[X_AXIS] = x;
  808. count_position[Y_AXIS] = y;
  809. count_position[Z_AXIS] = z;
  810. count_position[E_AXIS] = e;
  811. CRITICAL_SECTION_END;
  812. }
  813. void st_set_e_position(const long &e)
  814. {
  815. CRITICAL_SECTION_START;
  816. count_position[E_AXIS] = e;
  817. CRITICAL_SECTION_END;
  818. }
  819. long st_get_position(uint8_t axis)
  820. {
  821. long count_pos;
  822. CRITICAL_SECTION_START;
  823. count_pos = count_position[axis];
  824. CRITICAL_SECTION_END;
  825. return count_pos;
  826. }
  827. void st_get_position_xy(long &x, long &y)
  828. {
  829. CRITICAL_SECTION_START;
  830. x = count_position[X_AXIS];
  831. y = count_position[Y_AXIS];
  832. CRITICAL_SECTION_END;
  833. }
  834. float st_get_position_mm(uint8_t axis)
  835. {
  836. float steper_position_in_steps = st_get_position(axis);
  837. return steper_position_in_steps / axis_steps_per_unit[axis];
  838. }
  839. void finishAndDisableSteppers()
  840. {
  841. st_synchronize();
  842. disable_x();
  843. disable_y();
  844. disable_z();
  845. disable_e0();
  846. disable_e1();
  847. disable_e2();
  848. }
  849. void quickStop()
  850. {
  851. DISABLE_STEPPER_DRIVER_INTERRUPT();
  852. while (blocks_queued()) plan_discard_current_block();
  853. current_block = NULL;
  854. ENABLE_STEPPER_DRIVER_INTERRUPT();
  855. }
  856. #ifdef BABYSTEPPING
  857. void babystep(const uint8_t axis,const bool direction)
  858. {
  859. //MUST ONLY BE CALLED BY A ISR, it depends on that no other ISR interrupts this
  860. //store initial pin states
  861. switch(axis)
  862. {
  863. case X_AXIS:
  864. {
  865. enable_x();
  866. uint8_t old_x_dir_pin= READ(X_DIR_PIN); //if dualzstepper, both point to same direction.
  867. //setup new step
  868. WRITE(X_DIR_PIN,(INVERT_X_DIR)^direction);
  869. //perform step
  870. WRITE(X_STEP_PIN, !INVERT_X_STEP_PIN);
  871. {
  872. volatile float x=1./float(axis+1)/float(axis+2); //wait a tiny bit
  873. }
  874. WRITE(X_STEP_PIN, INVERT_X_STEP_PIN);
  875. //get old pin state back.
  876. WRITE(X_DIR_PIN,old_x_dir_pin);
  877. }
  878. break;
  879. case Y_AXIS:
  880. {
  881. enable_y();
  882. uint8_t old_y_dir_pin= READ(Y_DIR_PIN); //if dualzstepper, both point to same direction.
  883. //setup new step
  884. WRITE(Y_DIR_PIN,(INVERT_Y_DIR)^direction);
  885. //perform step
  886. WRITE(Y_STEP_PIN, !INVERT_Y_STEP_PIN);
  887. {
  888. volatile float x=1./float(axis+1)/float(axis+2); //wait a tiny bit
  889. }
  890. WRITE(Y_STEP_PIN, INVERT_Y_STEP_PIN);
  891. //get old pin state back.
  892. WRITE(Y_DIR_PIN,old_y_dir_pin);
  893. }
  894. break;
  895. case Z_AXIS:
  896. {
  897. enable_z();
  898. uint8_t old_z_dir_pin= READ(Z_DIR_PIN); //if dualzstepper, both point to same direction.
  899. //setup new step
  900. WRITE(Z_DIR_PIN,(INVERT_Z_DIR)^direction^BABYSTEP_INVERT_Z);
  901. #ifdef Z_DUAL_STEPPER_DRIVERS
  902. WRITE(Z2_DIR_PIN,(INVERT_Z_DIR)^direction^BABYSTEP_INVERT_Z);
  903. #endif
  904. //perform step
  905. WRITE(Z_STEP_PIN, !INVERT_Z_STEP_PIN);
  906. #ifdef Z_DUAL_STEPPER_DRIVERS
  907. WRITE(Z2_STEP_PIN, !INVERT_Z_STEP_PIN);
  908. #endif
  909. //wait a tiny bit
  910. {
  911. volatile float x=1./float(axis+1); //absolutely useless
  912. }
  913. WRITE(Z_STEP_PIN, INVERT_Z_STEP_PIN);
  914. #ifdef Z_DUAL_STEPPER_DRIVERS
  915. WRITE(Z2_STEP_PIN, INVERT_Z_STEP_PIN);
  916. #endif
  917. //get old pin state back.
  918. WRITE(Z_DIR_PIN,old_z_dir_pin);
  919. #ifdef Z_DUAL_STEPPER_DRIVERS
  920. WRITE(Z2_DIR_PIN,old_z_dir_pin);
  921. #endif
  922. }
  923. break;
  924. default: break;
  925. }
  926. }
  927. #endif //BABYSTEPPING
  928. void digitalPotWrite(int address, int value) // From Arduino DigitalPotControl example
  929. {
  930. #if defined(DIGIPOTSS_PIN) && DIGIPOTSS_PIN > -1
  931. digitalWrite(DIGIPOTSS_PIN,LOW); // take the SS pin low to select the chip
  932. SPI.transfer(address); // send in the address and value via SPI:
  933. SPI.transfer(value);
  934. digitalWrite(DIGIPOTSS_PIN,HIGH); // take the SS pin high to de-select the chip:
  935. //delay(10);
  936. #endif
  937. }
  938. void EEPROM_read_st(int pos, uint8_t* value, uint8_t size)
  939. {
  940. do
  941. {
  942. *value = eeprom_read_byte((unsigned char*)pos);
  943. pos++;
  944. value++;
  945. }while(--size);
  946. }
  947. void digipot_init() //Initialize Digipot Motor Current
  948. {
  949. EEPROM_read_st(EEPROM_SILENT,(uint8_t*)&SilentMode,sizeof(SilentMode));
  950. #if defined(DIGIPOTSS_PIN) && DIGIPOTSS_PIN > -1
  951. if(SilentMode == 0){
  952. const uint8_t digipot_motor_current[] = DIGIPOT_MOTOR_CURRENT_LOUD;
  953. }else{
  954. const uint8_t digipot_motor_current[] = DIGIPOT_MOTOR_CURRENT;
  955. }
  956. SPI.begin();
  957. pinMode(DIGIPOTSS_PIN, OUTPUT);
  958. for(int i=0;i<=4;i++)
  959. //digitalPotWrite(digipot_ch[i], digipot_motor_current[i]);
  960. digipot_current(i,digipot_motor_current[i]);
  961. #endif
  962. #ifdef MOTOR_CURRENT_PWM_XY_PIN
  963. pinMode(MOTOR_CURRENT_PWM_XY_PIN, OUTPUT);
  964. pinMode(MOTOR_CURRENT_PWM_Z_PIN, OUTPUT);
  965. pinMode(MOTOR_CURRENT_PWM_E_PIN, OUTPUT);
  966. if((SilentMode == 0) || (farm_mode) ){
  967. motor_current_setting[0] = motor_current_setting_loud[0];
  968. motor_current_setting[1] = motor_current_setting_loud[1];
  969. motor_current_setting[2] = motor_current_setting_loud[2];
  970. }else{
  971. motor_current_setting[0] = motor_current_setting_silent[0];
  972. motor_current_setting[1] = motor_current_setting_silent[1];
  973. motor_current_setting[2] = motor_current_setting_silent[2];
  974. }
  975. digipot_current(0, motor_current_setting[0]);
  976. digipot_current(1, motor_current_setting[1]);
  977. digipot_current(2, motor_current_setting[2]);
  978. //Set timer5 to 31khz so the PWM of the motor power is as constant as possible. (removes a buzzing noise)
  979. TCCR5B = (TCCR5B & ~(_BV(CS50) | _BV(CS51) | _BV(CS52))) | _BV(CS50);
  980. #endif
  981. }
  982. void digipot_current(uint8_t driver, int current)
  983. {
  984. #if defined(DIGIPOTSS_PIN) && DIGIPOTSS_PIN > -1
  985. const uint8_t digipot_ch[] = DIGIPOT_CHANNELS;
  986. digitalPotWrite(digipot_ch[driver], current);
  987. #endif
  988. #ifdef MOTOR_CURRENT_PWM_XY_PIN
  989. if (driver == 0) analogWrite(MOTOR_CURRENT_PWM_XY_PIN, (long)current * 255L / (long)MOTOR_CURRENT_PWM_RANGE);
  990. if (driver == 1) analogWrite(MOTOR_CURRENT_PWM_Z_PIN, (long)current * 255L / (long)MOTOR_CURRENT_PWM_RANGE);
  991. if (driver == 2) analogWrite(MOTOR_CURRENT_PWM_E_PIN, (long)current * 255L / (long)MOTOR_CURRENT_PWM_RANGE);
  992. #endif
  993. }
  994. void microstep_init()
  995. {
  996. const uint8_t microstep_modes[] = MICROSTEP_MODES;
  997. #if defined(E1_MS1_PIN) && E1_MS1_PIN > -1
  998. pinMode(E1_MS1_PIN,OUTPUT);
  999. pinMode(E1_MS2_PIN,OUTPUT);
  1000. #endif
  1001. #if defined(X_MS1_PIN) && X_MS1_PIN > -1
  1002. pinMode(X_MS1_PIN,OUTPUT);
  1003. pinMode(X_MS2_PIN,OUTPUT);
  1004. pinMode(Y_MS1_PIN,OUTPUT);
  1005. pinMode(Y_MS2_PIN,OUTPUT);
  1006. pinMode(Z_MS1_PIN,OUTPUT);
  1007. pinMode(Z_MS2_PIN,OUTPUT);
  1008. pinMode(E0_MS1_PIN,OUTPUT);
  1009. pinMode(E0_MS2_PIN,OUTPUT);
  1010. for(int i=0;i<=4;i++) microstep_mode(i,microstep_modes[i]);
  1011. #endif
  1012. }
  1013. void microstep_ms(uint8_t driver, int8_t ms1, int8_t ms2)
  1014. {
  1015. if(ms1 > -1) switch(driver)
  1016. {
  1017. case 0: digitalWrite( X_MS1_PIN,ms1); break;
  1018. case 1: digitalWrite( Y_MS1_PIN,ms1); break;
  1019. case 2: digitalWrite( Z_MS1_PIN,ms1); break;
  1020. case 3: digitalWrite(E0_MS1_PIN,ms1); break;
  1021. #if defined(E1_MS1_PIN) && E1_MS1_PIN > -1
  1022. case 4: digitalWrite(E1_MS1_PIN,ms1); break;
  1023. #endif
  1024. }
  1025. if(ms2 > -1) switch(driver)
  1026. {
  1027. case 0: digitalWrite( X_MS2_PIN,ms2); break;
  1028. case 1: digitalWrite( Y_MS2_PIN,ms2); break;
  1029. case 2: digitalWrite( Z_MS2_PIN,ms2); break;
  1030. case 3: digitalWrite(E0_MS2_PIN,ms2); break;
  1031. #if defined(E1_MS2_PIN) && E1_MS2_PIN > -1
  1032. case 4: digitalWrite(E1_MS2_PIN,ms2); break;
  1033. #endif
  1034. }
  1035. }
  1036. void microstep_mode(uint8_t driver, uint8_t stepping_mode)
  1037. {
  1038. switch(stepping_mode)
  1039. {
  1040. case 1: microstep_ms(driver,MICROSTEP1); break;
  1041. case 2: microstep_ms(driver,MICROSTEP2); break;
  1042. case 4: microstep_ms(driver,MICROSTEP4); break;
  1043. case 8: microstep_ms(driver,MICROSTEP8); break;
  1044. case 16: microstep_ms(driver,MICROSTEP16); break;
  1045. }
  1046. }
  1047. void microstep_readings()
  1048. {
  1049. SERIAL_PROTOCOLPGM("MS1,MS2 Pins\n");
  1050. SERIAL_PROTOCOLPGM("X: ");
  1051. SERIAL_PROTOCOL( digitalRead(X_MS1_PIN));
  1052. SERIAL_PROTOCOLLN( digitalRead(X_MS2_PIN));
  1053. SERIAL_PROTOCOLPGM("Y: ");
  1054. SERIAL_PROTOCOL( digitalRead(Y_MS1_PIN));
  1055. SERIAL_PROTOCOLLN( digitalRead(Y_MS2_PIN));
  1056. SERIAL_PROTOCOLPGM("Z: ");
  1057. SERIAL_PROTOCOL( digitalRead(Z_MS1_PIN));
  1058. SERIAL_PROTOCOLLN( digitalRead(Z_MS2_PIN));
  1059. SERIAL_PROTOCOLPGM("E0: ");
  1060. SERIAL_PROTOCOL( digitalRead(E0_MS1_PIN));
  1061. SERIAL_PROTOCOLLN( digitalRead(E0_MS2_PIN));
  1062. #if defined(E1_MS1_PIN) && E1_MS1_PIN > -1
  1063. SERIAL_PROTOCOLPGM("E1: ");
  1064. SERIAL_PROTOCOL( digitalRead(E1_MS1_PIN));
  1065. SERIAL_PROTOCOLLN( digitalRead(E1_MS2_PIN));
  1066. #endif
  1067. }
  1068. static void check_fans() {
  1069. if (READ(TACH_0) != fan_state[0]) {
  1070. fan_edge_counter[0] ++;
  1071. fan_state[0] = READ(TACH_0);
  1072. }
  1073. if (READ(TACH_1) != fan_state[1]) {
  1074. fan_edge_counter[1] ++;
  1075. fan_state[1] = READ(TACH_1);
  1076. }
  1077. }