My Marlin configs for Fabrikator Mini and CTC i3 Pro B
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

stepper.cpp 39KB

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