My Marlin configs for Fabrikator Mini and CTC i3 Pro B
Nevar pievienot vairāk kā 25 tēmas Tēmai ir jāsākas ar burtu vai ciparu, tā var saturēt domu zīmes ('-') un var būt līdz 35 simboliem gara.

stepper.cpp 40KB

12345678910111213141516171819202122232425262728293031323334353637383940414243444546474849505152535455565758596061626364656667686970717273747576777879808182838485868788899091929394959697989910010110210310410510610710810911011111211311411511611711811912012112212312412512612712812913013113213313413513613713813914014114214314414514614714814915015115215315415515615715815916016116216316416516616716816917017117217317417517617717817918018118218318418518618718818919019119219319419519619719819920020120220320420520620720820921021121221321421521621721821922022122222322422522622722822923023123223323423523623723823924024124224324424524624724824925025125225325425525625725825926026126226326426526626726826927027127227327427527627727827928028128228328428528628728828929029129229329429529629729829930030130230330430530630730830931031131231331431531631731831932032132232332432532632732832933033133233333433533633733833934034134234334434534634734834935035135235335435535635735835936036136236336436536636736836937037137237337437537637737837938038138238338438538638738838939039139239339439539639739839940040140240340440540640740840941041141241341441541641741841942042142242342442542642742842943043143243343443543643743843944044144244344444544644744844945045145245345445545645745845946046146246346446546646746846947047147247347447547647747847948048148248348448548648748848949049149249349449549649749849950050150250350450550650750850951051151251351451551651751851952052152252352452552652752852953053153253353453553653753853954054154254354454554654754854955055155255355455555655755855956056156256356456556656756856957057157257357457557657757857958058158258358458558658758858959059159259359459559659759859960060160260360460560660760860961061161261361461561661761861962062162262362462562662762862963063163263363463563663763863964064164264364464564664764864965065165265365465565665765865966066166266366466566666766866967067167267367467567667767867968068168268368468568668768868969069169269369469569669769869970070170270370470570670770870971071171271371471571671771871972072172272372472572672772872973073173273373473573673773873974074174274374474574674774874975075175275375475575675775875976076176276376476576676776876977077177277377477577677777877978078178278378478578678778878979079179279379479579679779879980080180280380480580680780880981081181281381481581681781881982082182282382482582682782882983083183283383483583683783883984084184284384484584684784884985085185285385485585685785885986086186286386486586686786886987087187287387487587687787887988088188288388488588688788888989089189289389489589689789889990090190290390490590690790890991091191291391491591691791891992092192292392492592692792892993093193293393493593693793893994094194294394494594694794894995095195295395495595695795895996096196296396496596696796896997097197297397497597697797897998098198298398498598698798898999099199299399499599699799899910001001100210031004100510061007100810091010101110121013101410151016101710181019102010211022102310241025102610271028102910301031103210331034103510361037103810391040104110421043104410451046104710481049105010511052105310541055105610571058105910601061106210631064106510661067106810691070107110721073107410751076107710781079108010811082108310841085108610871088108910901091109210931094109510961097109810991100110111021103110411051106110711081109111011111112111311141115111611171118111911201121112211231124112511261127112811291130113111321133113411351136113711381139114011411142114311441145114611471148114911501151115211531154115511561157115811591160116111621163116411651166116711681169117011711172117311741175117611771178117911801181118211831184118511861187118811891190119111921193119411951196119711981199120012011202120312041205120612071208120912101211121212131214121512161217121812191220122112221223122412251226122712281229123012311232123312341235123612371238123912401241124212431244124512461247124812491250125112521253125412551256125712581259126012611262126312641265126612671268126912701271127212731274127512761277127812791280128112821283128412851286128712881289129012911292129312941295129612971298129913001301130213031304130513061307130813091310131113121313131413151316131713181319132013211322132313241325132613271328132913301331133213331334133513361337133813391340134113421343134413451346134713481349135013511352135313541355135613571358135913601361
  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. #ifdef CONFIG_STEPPERS_TOSHIBA
  505. /* The toshiba stepper controller require much longer pulses
  506. * tjerfore we 'stage' decompose the pulses between high, and
  507. * low instead of doing each in turn. The extra tests add enough
  508. * lag to allow it work with without needing NOPs */
  509. if (counter_x > 0) {
  510. WRITE(X_STEP_PIN, HIGH);
  511. }
  512. counter_y += current_block->steps_y;
  513. if (counter_y > 0) {
  514. WRITE(Y_STEP_PIN, HIGH);
  515. }
  516. counter_z += current_block->steps_z;
  517. if (counter_z > 0) {
  518. WRITE(Z_STEP_PIN, HIGH);
  519. }
  520. #ifndef ADVANCE
  521. counter_e += current_block->steps_e;
  522. if (counter_e > 0) {
  523. WRITE_E_STEP(HIGH);
  524. }
  525. #endif //!ADVANCE
  526. if (counter_x > 0) {
  527. counter_x -= current_block->step_event_count;
  528. count_position[X_AXIS]+=count_direction[X_AXIS];
  529. WRITE(X_STEP_PIN, LOW);
  530. }
  531. if (counter_y > 0) {
  532. counter_y -= current_block->step_event_count;
  533. count_position[Y_AXIS]+=count_direction[Y_AXIS];
  534. WRITE(Y_STEP_PIN, LOW);
  535. }
  536. if (counter_z > 0) {
  537. counter_z -= current_block->step_event_count;
  538. count_position[Z_AXIS]+=count_direction[Z_AXIS];
  539. WRITE(Z_STEP_PIN, LOW);
  540. }
  541. #ifndef ADVANCE
  542. if (counter_e > 0) {
  543. counter_e -= current_block->step_event_count;
  544. count_position[E_AXIS]+=count_direction[E_AXIS];
  545. WRITE_E_STEP(LOW);
  546. }
  547. #endif //!ADVANCE
  548. #else
  549. if (counter_x > 0) {
  550. #ifdef DUAL_X_CARRIAGE
  551. if (extruder_duplication_enabled){
  552. WRITE(X_STEP_PIN, !INVERT_X_STEP_PIN);
  553. WRITE(X2_STEP_PIN, !INVERT_X_STEP_PIN);
  554. }
  555. else {
  556. if (current_block->active_extruder != 0)
  557. WRITE(X2_STEP_PIN, !INVERT_X_STEP_PIN);
  558. else
  559. WRITE(X_STEP_PIN, !INVERT_X_STEP_PIN);
  560. }
  561. #else
  562. WRITE(X_STEP_PIN, !INVERT_X_STEP_PIN);
  563. #endif
  564. counter_x -= current_block->step_event_count;
  565. count_position[X_AXIS]+=count_direction[X_AXIS];
  566. #ifdef DUAL_X_CARRIAGE
  567. if (extruder_duplication_enabled){
  568. WRITE(X_STEP_PIN, INVERT_X_STEP_PIN);
  569. WRITE(X2_STEP_PIN, INVERT_X_STEP_PIN);
  570. }
  571. else {
  572. if (current_block->active_extruder != 0)
  573. WRITE(X2_STEP_PIN, INVERT_X_STEP_PIN);
  574. else
  575. WRITE(X_STEP_PIN, INVERT_X_STEP_PIN);
  576. }
  577. #else
  578. WRITE(X_STEP_PIN, INVERT_X_STEP_PIN);
  579. #endif
  580. }
  581. counter_y += current_block->steps_y;
  582. if (counter_y > 0) {
  583. WRITE(Y_STEP_PIN, !INVERT_Y_STEP_PIN);
  584. #ifdef Y_DUAL_STEPPER_DRIVERS
  585. WRITE(Y2_STEP_PIN, !INVERT_Y_STEP_PIN);
  586. #endif
  587. counter_y -= current_block->step_event_count;
  588. count_position[Y_AXIS]+=count_direction[Y_AXIS];
  589. WRITE(Y_STEP_PIN, INVERT_Y_STEP_PIN);
  590. #ifdef Y_DUAL_STEPPER_DRIVERS
  591. WRITE(Y2_STEP_PIN, INVERT_Y_STEP_PIN);
  592. #endif
  593. }
  594. counter_z += current_block->steps_z;
  595. if (counter_z > 0) {
  596. WRITE(Z_STEP_PIN, !INVERT_Z_STEP_PIN);
  597. #ifdef Z_DUAL_STEPPER_DRIVERS
  598. WRITE(Z2_STEP_PIN, !INVERT_Z_STEP_PIN);
  599. #endif
  600. counter_z -= current_block->step_event_count;
  601. count_position[Z_AXIS]+=count_direction[Z_AXIS];
  602. WRITE(Z_STEP_PIN, INVERT_Z_STEP_PIN);
  603. #ifdef Z_DUAL_STEPPER_DRIVERS
  604. WRITE(Z2_STEP_PIN, INVERT_Z_STEP_PIN);
  605. #endif
  606. }
  607. #ifndef ADVANCE
  608. counter_e += current_block->steps_e;
  609. if (counter_e > 0) {
  610. WRITE_E_STEP(!INVERT_E_STEP_PIN);
  611. counter_e -= current_block->step_event_count;
  612. count_position[E_AXIS]+=count_direction[E_AXIS];
  613. WRITE_E_STEP(INVERT_E_STEP_PIN);
  614. }
  615. #endif //!ADVANCE
  616. #endif
  617. step_events_completed += 1;
  618. if(step_events_completed >= current_block->step_event_count) break;
  619. }
  620. // Calculare new timer value
  621. unsigned short timer;
  622. unsigned short step_rate;
  623. if (step_events_completed <= (unsigned long int)current_block->accelerate_until) {
  624. MultiU24X24toH16(acc_step_rate, acceleration_time, current_block->acceleration_rate);
  625. acc_step_rate += current_block->initial_rate;
  626. // upper limit
  627. if(acc_step_rate > current_block->nominal_rate)
  628. acc_step_rate = current_block->nominal_rate;
  629. // step_rate to timer interval
  630. timer = calc_timer(acc_step_rate);
  631. OCR1A = timer;
  632. acceleration_time += timer;
  633. #ifdef ADVANCE
  634. for(int8_t i=0; i < step_loops; i++) {
  635. advance += advance_rate;
  636. }
  637. //if(advance > current_block->advance) advance = current_block->advance;
  638. // Do E steps + advance steps
  639. e_steps[current_block->active_extruder] += ((advance >>8) - old_advance);
  640. old_advance = advance >>8;
  641. #endif
  642. }
  643. else if (step_events_completed > (unsigned long int)current_block->decelerate_after) {
  644. MultiU24X24toH16(step_rate, deceleration_time, current_block->acceleration_rate);
  645. if(step_rate > acc_step_rate) { // Check step_rate stays positive
  646. step_rate = current_block->final_rate;
  647. }
  648. else {
  649. step_rate = acc_step_rate - step_rate; // Decelerate from aceleration end point.
  650. }
  651. // lower limit
  652. if(step_rate < current_block->final_rate)
  653. step_rate = current_block->final_rate;
  654. // step_rate to timer interval
  655. timer = calc_timer(step_rate);
  656. OCR1A = timer;
  657. deceleration_time += timer;
  658. #ifdef ADVANCE
  659. for(int8_t i=0; i < step_loops; i++) {
  660. advance -= advance_rate;
  661. }
  662. if(advance < final_advance) advance = final_advance;
  663. // Do E steps + advance steps
  664. e_steps[current_block->active_extruder] += ((advance >>8) - old_advance);
  665. old_advance = advance >>8;
  666. #endif //ADVANCE
  667. }
  668. else {
  669. OCR1A = OCR1A_nominal;
  670. // ensure we're running at the correct step rate, even if we just came off an acceleration
  671. step_loops = step_loops_nominal;
  672. }
  673. // If current block is finished, reset pointer
  674. if (step_events_completed >= current_block->step_event_count) {
  675. current_block = NULL;
  676. plan_discard_current_block();
  677. }
  678. }
  679. }
  680. #ifdef ADVANCE
  681. unsigned char old_OCR0A;
  682. // Timer interrupt for E. e_steps is set in the main routine;
  683. // Timer 0 is shared with millies
  684. ISR(TIMER0_COMPA_vect)
  685. {
  686. old_OCR0A += 52; // ~10kHz interrupt (250000 / 26 = 9615kHz)
  687. OCR0A = old_OCR0A;
  688. // Set E direction (Depends on E direction + advance)
  689. for(unsigned char i=0; i<4;i++) {
  690. if (e_steps[0] != 0) {
  691. WRITE(E0_STEP_PIN, INVERT_E_STEP_PIN);
  692. if (e_steps[0] < 0) {
  693. WRITE(E0_DIR_PIN, INVERT_E0_DIR);
  694. e_steps[0]++;
  695. WRITE(E0_STEP_PIN, !INVERT_E_STEP_PIN);
  696. }
  697. else if (e_steps[0] > 0) {
  698. WRITE(E0_DIR_PIN, !INVERT_E0_DIR);
  699. e_steps[0]--;
  700. WRITE(E0_STEP_PIN, !INVERT_E_STEP_PIN);
  701. }
  702. }
  703. #if EXTRUDERS > 1
  704. if (e_steps[1] != 0) {
  705. WRITE(E1_STEP_PIN, INVERT_E_STEP_PIN);
  706. if (e_steps[1] < 0) {
  707. WRITE(E1_DIR_PIN, INVERT_E1_DIR);
  708. e_steps[1]++;
  709. WRITE(E1_STEP_PIN, !INVERT_E_STEP_PIN);
  710. }
  711. else if (e_steps[1] > 0) {
  712. WRITE(E1_DIR_PIN, !INVERT_E1_DIR);
  713. e_steps[1]--;
  714. WRITE(E1_STEP_PIN, !INVERT_E_STEP_PIN);
  715. }
  716. }
  717. #endif
  718. #if EXTRUDERS > 2
  719. if (e_steps[2] != 0) {
  720. WRITE(E2_STEP_PIN, INVERT_E_STEP_PIN);
  721. if (e_steps[2] < 0) {
  722. WRITE(E2_DIR_PIN, INVERT_E2_DIR);
  723. e_steps[2]++;
  724. WRITE(E2_STEP_PIN, !INVERT_E_STEP_PIN);
  725. }
  726. else if (e_steps[2] > 0) {
  727. WRITE(E2_DIR_PIN, !INVERT_E2_DIR);
  728. e_steps[2]--;
  729. WRITE(E2_STEP_PIN, !INVERT_E_STEP_PIN);
  730. }
  731. }
  732. #endif
  733. }
  734. }
  735. #endif // ADVANCE
  736. void st_init()
  737. {
  738. digipot_init(); //Initialize Digipot Motor Current
  739. microstep_init(); //Initialize Microstepping Pins
  740. //Initialize Dir Pins
  741. #if defined(X_DIR_PIN) && X_DIR_PIN > -1
  742. SET_OUTPUT(X_DIR_PIN);
  743. #endif
  744. #if defined(X2_DIR_PIN) && X2_DIR_PIN > -1
  745. SET_OUTPUT(X2_DIR_PIN);
  746. #endif
  747. #if defined(Y_DIR_PIN) && Y_DIR_PIN > -1
  748. SET_OUTPUT(Y_DIR_PIN);
  749. #if defined(Y_DUAL_STEPPER_DRIVERS) && defined(Y2_DIR_PIN) && (Y2_DIR_PIN > -1)
  750. SET_OUTPUT(Y2_DIR_PIN);
  751. #endif
  752. #endif
  753. #if defined(Z_DIR_PIN) && Z_DIR_PIN > -1
  754. SET_OUTPUT(Z_DIR_PIN);
  755. #if defined(Z_DUAL_STEPPER_DRIVERS) && defined(Z2_DIR_PIN) && (Z2_DIR_PIN > -1)
  756. SET_OUTPUT(Z2_DIR_PIN);
  757. #endif
  758. #endif
  759. #if defined(E0_DIR_PIN) && E0_DIR_PIN > -1
  760. SET_OUTPUT(E0_DIR_PIN);
  761. #endif
  762. #if defined(E1_DIR_PIN) && (E1_DIR_PIN > -1)
  763. SET_OUTPUT(E1_DIR_PIN);
  764. #endif
  765. #if defined(E2_DIR_PIN) && (E2_DIR_PIN > -1)
  766. SET_OUTPUT(E2_DIR_PIN);
  767. #endif
  768. //Initialize Enable Pins - steppers default to disabled.
  769. #if defined(X_ENABLE_PIN) && X_ENABLE_PIN > -1
  770. SET_OUTPUT(X_ENABLE_PIN);
  771. if(!X_ENABLE_ON) WRITE(X_ENABLE_PIN,HIGH);
  772. #endif
  773. #if defined(X2_ENABLE_PIN) && X2_ENABLE_PIN > -1
  774. SET_OUTPUT(X2_ENABLE_PIN);
  775. if(!X_ENABLE_ON) WRITE(X2_ENABLE_PIN,HIGH);
  776. #endif
  777. #if defined(Y_ENABLE_PIN) && Y_ENABLE_PIN > -1
  778. SET_OUTPUT(Y_ENABLE_PIN);
  779. if(!Y_ENABLE_ON) WRITE(Y_ENABLE_PIN,HIGH);
  780. #if defined(Y_DUAL_STEPPER_DRIVERS) && defined(Y2_ENABLE_PIN) && (Y2_ENABLE_PIN > -1)
  781. SET_OUTPUT(Y2_ENABLE_PIN);
  782. if(!Y_ENABLE_ON) WRITE(Y2_ENABLE_PIN,HIGH);
  783. #endif
  784. #endif
  785. #if defined(Z_ENABLE_PIN) && Z_ENABLE_PIN > -1
  786. SET_OUTPUT(Z_ENABLE_PIN);
  787. if(!Z_ENABLE_ON) WRITE(Z_ENABLE_PIN,HIGH);
  788. #if defined(Z_DUAL_STEPPER_DRIVERS) && defined(Z2_ENABLE_PIN) && (Z2_ENABLE_PIN > -1)
  789. SET_OUTPUT(Z2_ENABLE_PIN);
  790. if(!Z_ENABLE_ON) WRITE(Z2_ENABLE_PIN,HIGH);
  791. #endif
  792. #endif
  793. #if defined(E0_ENABLE_PIN) && (E0_ENABLE_PIN > -1)
  794. SET_OUTPUT(E0_ENABLE_PIN);
  795. if(!E_ENABLE_ON) WRITE(E0_ENABLE_PIN,HIGH);
  796. #endif
  797. #if defined(E1_ENABLE_PIN) && (E1_ENABLE_PIN > -1)
  798. SET_OUTPUT(E1_ENABLE_PIN);
  799. if(!E_ENABLE_ON) WRITE(E1_ENABLE_PIN,HIGH);
  800. #endif
  801. #if defined(E2_ENABLE_PIN) && (E2_ENABLE_PIN > -1)
  802. SET_OUTPUT(E2_ENABLE_PIN);
  803. if(!E_ENABLE_ON) WRITE(E2_ENABLE_PIN,HIGH);
  804. #endif
  805. //endstops and pullups
  806. #if defined(X_MIN_PIN) && X_MIN_PIN > -1
  807. SET_INPUT(X_MIN_PIN);
  808. #ifdef ENDSTOPPULLUP_XMIN
  809. WRITE(X_MIN_PIN,HIGH);
  810. #endif
  811. #endif
  812. #if defined(Y_MIN_PIN) && Y_MIN_PIN > -1
  813. SET_INPUT(Y_MIN_PIN);
  814. #ifdef ENDSTOPPULLUP_YMIN
  815. WRITE(Y_MIN_PIN,HIGH);
  816. #endif
  817. #endif
  818. #if defined(Z_MIN_PIN) && Z_MIN_PIN > -1
  819. SET_INPUT(Z_MIN_PIN);
  820. #ifdef ENDSTOPPULLUP_ZMIN
  821. WRITE(Z_MIN_PIN,HIGH);
  822. #endif
  823. #endif
  824. #if defined(X_MAX_PIN) && X_MAX_PIN > -1
  825. SET_INPUT(X_MAX_PIN);
  826. #ifdef ENDSTOPPULLUP_XMAX
  827. WRITE(X_MAX_PIN,HIGH);
  828. #endif
  829. #endif
  830. #if defined(Y_MAX_PIN) && Y_MAX_PIN > -1
  831. SET_INPUT(Y_MAX_PIN);
  832. #ifdef ENDSTOPPULLUP_YMAX
  833. WRITE(Y_MAX_PIN,HIGH);
  834. #endif
  835. #endif
  836. #if defined(Z_MAX_PIN) && Z_MAX_PIN > -1
  837. SET_INPUT(Z_MAX_PIN);
  838. #ifdef ENDSTOPPULLUP_ZMAX
  839. WRITE(Z_MAX_PIN,HIGH);
  840. #endif
  841. #endif
  842. //Initialize Step Pins
  843. #if defined(X_STEP_PIN) && (X_STEP_PIN > -1)
  844. SET_OUTPUT(X_STEP_PIN);
  845. WRITE(X_STEP_PIN,INVERT_X_STEP_PIN);
  846. disable_x();
  847. #endif
  848. #if defined(X2_STEP_PIN) && (X2_STEP_PIN > -1)
  849. SET_OUTPUT(X2_STEP_PIN);
  850. WRITE(X2_STEP_PIN,INVERT_X_STEP_PIN);
  851. disable_x();
  852. #endif
  853. #if defined(Y_STEP_PIN) && (Y_STEP_PIN > -1)
  854. SET_OUTPUT(Y_STEP_PIN);
  855. WRITE(Y_STEP_PIN,INVERT_Y_STEP_PIN);
  856. #if defined(Y_DUAL_STEPPER_DRIVERS) && defined(Y2_STEP_PIN) && (Y2_STEP_PIN > -1)
  857. SET_OUTPUT(Y2_STEP_PIN);
  858. WRITE(Y2_STEP_PIN,INVERT_Y_STEP_PIN);
  859. #endif
  860. disable_y();
  861. #endif
  862. #if defined(Z_STEP_PIN) && (Z_STEP_PIN > -1)
  863. SET_OUTPUT(Z_STEP_PIN);
  864. WRITE(Z_STEP_PIN,INVERT_Z_STEP_PIN);
  865. #if defined(Z_DUAL_STEPPER_DRIVERS) && defined(Z2_STEP_PIN) && (Z2_STEP_PIN > -1)
  866. SET_OUTPUT(Z2_STEP_PIN);
  867. WRITE(Z2_STEP_PIN,INVERT_Z_STEP_PIN);
  868. #endif
  869. disable_z();
  870. #endif
  871. #if defined(E0_STEP_PIN) && (E0_STEP_PIN > -1)
  872. SET_OUTPUT(E0_STEP_PIN);
  873. WRITE(E0_STEP_PIN,INVERT_E_STEP_PIN);
  874. disable_e0();
  875. #endif
  876. #if defined(E1_STEP_PIN) && (E1_STEP_PIN > -1)
  877. SET_OUTPUT(E1_STEP_PIN);
  878. WRITE(E1_STEP_PIN,INVERT_E_STEP_PIN);
  879. disable_e1();
  880. #endif
  881. #if defined(E2_STEP_PIN) && (E2_STEP_PIN > -1)
  882. SET_OUTPUT(E2_STEP_PIN);
  883. WRITE(E2_STEP_PIN,INVERT_E_STEP_PIN);
  884. disable_e2();
  885. #endif
  886. // waveform generation = 0100 = CTC
  887. TCCR1B &= ~(1<<WGM13);
  888. TCCR1B |= (1<<WGM12);
  889. TCCR1A &= ~(1<<WGM11);
  890. TCCR1A &= ~(1<<WGM10);
  891. // output mode = 00 (disconnected)
  892. TCCR1A &= ~(3<<COM1A0);
  893. TCCR1A &= ~(3<<COM1B0);
  894. // Set the timer pre-scaler
  895. // Generally we use a divider of 8, resulting in a 2MHz timer
  896. // frequency on a 16MHz MCU. If you are going to change this, be
  897. // sure to regenerate speed_lookuptable.h with
  898. // create_speed_lookuptable.py
  899. TCCR1B = (TCCR1B & ~(0x07<<CS10)) | (2<<CS10);
  900. OCR1A = 0x4000;
  901. TCNT1 = 0;
  902. ENABLE_STEPPER_DRIVER_INTERRUPT();
  903. #ifdef ADVANCE
  904. #if defined(TCCR0A) && defined(WGM01)
  905. TCCR0A &= ~(1<<WGM01);
  906. TCCR0A &= ~(1<<WGM00);
  907. #endif
  908. e_steps[0] = 0;
  909. e_steps[1] = 0;
  910. e_steps[2] = 0;
  911. TIMSK0 |= (1<<OCIE0A);
  912. #endif //ADVANCE
  913. enable_endstops(true); // Start with endstops active. After homing they can be disabled
  914. sei();
  915. }
  916. // Block until all buffered steps are executed
  917. void st_synchronize()
  918. {
  919. while( blocks_queued()) {
  920. manage_heater();
  921. manage_inactivity();
  922. lcd_update();
  923. }
  924. }
  925. void st_set_position(const long &x, const long &y, const long &z, const long &e)
  926. {
  927. CRITICAL_SECTION_START;
  928. count_position[X_AXIS] = x;
  929. count_position[Y_AXIS] = y;
  930. count_position[Z_AXIS] = z;
  931. count_position[E_AXIS] = e;
  932. CRITICAL_SECTION_END;
  933. }
  934. void st_set_e_position(const long &e)
  935. {
  936. CRITICAL_SECTION_START;
  937. count_position[E_AXIS] = e;
  938. CRITICAL_SECTION_END;
  939. }
  940. long st_get_position(uint8_t axis)
  941. {
  942. long count_pos;
  943. CRITICAL_SECTION_START;
  944. count_pos = count_position[axis];
  945. CRITICAL_SECTION_END;
  946. return count_pos;
  947. }
  948. #ifdef ENABLE_AUTO_BED_LEVELING
  949. float st_get_position_mm(uint8_t axis)
  950. {
  951. float steper_position_in_steps = st_get_position(axis);
  952. return steper_position_in_steps / axis_steps_per_unit[axis];
  953. }
  954. #endif // ENABLE_AUTO_BED_LEVELING
  955. void finishAndDisableSteppers()
  956. {
  957. st_synchronize();
  958. disable_x();
  959. disable_y();
  960. disable_z();
  961. disable_e0();
  962. disable_e1();
  963. disable_e2();
  964. }
  965. void quickStop()
  966. {
  967. DISABLE_STEPPER_DRIVER_INTERRUPT();
  968. while(blocks_queued())
  969. plan_discard_current_block();
  970. current_block = NULL;
  971. ENABLE_STEPPER_DRIVER_INTERRUPT();
  972. }
  973. #ifdef BABYSTEPPING
  974. void babystep(const uint8_t axis,const bool direction)
  975. {
  976. //MUST ONLY BE CALLED BY A ISR, it depends on that no other ISR interrupts this
  977. //store initial pin states
  978. switch(axis)
  979. {
  980. case X_AXIS:
  981. {
  982. enable_x();
  983. uint8_t old_x_dir_pin= READ(X_DIR_PIN); //if dualzstepper, both point to same direction.
  984. //setup new step
  985. WRITE(X_DIR_PIN,(INVERT_X_DIR)^direction);
  986. #ifdef DUAL_X_CARRIAGE
  987. WRITE(X2_DIR_PIN,(INVERT_X_DIR)^direction);
  988. #endif
  989. //perform step
  990. WRITE(X_STEP_PIN, !INVERT_X_STEP_PIN);
  991. #ifdef DUAL_X_CARRIAGE
  992. WRITE(X2_STEP_PIN, !INVERT_X_STEP_PIN);
  993. #endif
  994. {
  995. float x=1./float(axis+1)/float(axis+2); //wait a tiny bit
  996. }
  997. WRITE(X_STEP_PIN, INVERT_X_STEP_PIN);
  998. #ifdef DUAL_X_CARRIAGE
  999. WRITE(X2_STEP_PIN, INVERT_X_STEP_PIN);
  1000. #endif
  1001. //get old pin state back.
  1002. WRITE(X_DIR_PIN,old_x_dir_pin);
  1003. #ifdef DUAL_X_CARRIAGE
  1004. WRITE(X2_DIR_PIN,old_x_dir_pin);
  1005. #endif
  1006. }
  1007. break;
  1008. case Y_AXIS:
  1009. {
  1010. enable_y();
  1011. uint8_t old_y_dir_pin= READ(Y_DIR_PIN); //if dualzstepper, both point to same direction.
  1012. //setup new step
  1013. WRITE(Y_DIR_PIN,(INVERT_Y_DIR)^direction);
  1014. #ifdef DUAL_Y_CARRIAGE
  1015. WRITE(Y2_DIR_PIN,(INVERT_Y_DIR)^direction);
  1016. #endif
  1017. //perform step
  1018. WRITE(Y_STEP_PIN, !INVERT_Y_STEP_PIN);
  1019. #ifdef DUAL_Y_CARRIAGE
  1020. WRITE(Y2_STEP_PIN, !INVERT_Y_STEP_PIN);
  1021. #endif
  1022. {
  1023. float x=1./float(axis+1)/float(axis+2); //wait a tiny bit
  1024. }
  1025. WRITE(Y_STEP_PIN, INVERT_Y_STEP_PIN);
  1026. #ifdef DUAL_Y_CARRIAGE
  1027. WRITE(Y2_STEP_PIN, INVERT_Y_STEP_PIN);
  1028. #endif
  1029. //get old pin state back.
  1030. WRITE(Y_DIR_PIN,old_y_dir_pin);
  1031. #ifdef DUAL_Y_CARRIAGE
  1032. WRITE(Y2_DIR_PIN,old_y_dir_pin);
  1033. #endif
  1034. }
  1035. break;
  1036. #ifndef DELTA
  1037. case Z_AXIS:
  1038. {
  1039. enable_z();
  1040. uint8_t old_z_dir_pin= READ(Z_DIR_PIN); //if dualzstepper, both point to same direction.
  1041. //setup new step
  1042. WRITE(Z_DIR_PIN,(INVERT_Z_DIR)^direction^BABYSTEP_INVERT_Z);
  1043. #ifdef Z_DUAL_STEPPER_DRIVERS
  1044. WRITE(Z2_DIR_PIN,(INVERT_Z_DIR)^direction^BABYSTEP_INVERT_Z);
  1045. #endif
  1046. //perform step
  1047. WRITE(Z_STEP_PIN, !INVERT_Z_STEP_PIN);
  1048. #ifdef Z_DUAL_STEPPER_DRIVERS
  1049. WRITE(Z2_STEP_PIN, !INVERT_Z_STEP_PIN);
  1050. #endif
  1051. //wait a tiny bit
  1052. {
  1053. float x=1./float(axis+1); //absolutely useless
  1054. }
  1055. WRITE(Z_STEP_PIN, INVERT_Z_STEP_PIN);
  1056. #ifdef Z_DUAL_STEPPER_DRIVERS
  1057. WRITE(Z2_STEP_PIN, INVERT_Z_STEP_PIN);
  1058. #endif
  1059. //get old pin state back.
  1060. WRITE(Z_DIR_PIN,old_z_dir_pin);
  1061. #ifdef Z_DUAL_STEPPER_DRIVERS
  1062. WRITE(Z2_DIR_PIN,old_z_dir_pin);
  1063. #endif
  1064. }
  1065. break;
  1066. #else //DELTA
  1067. case Z_AXIS:
  1068. {
  1069. enable_x();
  1070. enable_y();
  1071. enable_z();
  1072. uint8_t old_x_dir_pin= READ(X_DIR_PIN);
  1073. uint8_t old_y_dir_pin= READ(Y_DIR_PIN);
  1074. uint8_t old_z_dir_pin= READ(Z_DIR_PIN);
  1075. //setup new step
  1076. WRITE(X_DIR_PIN,(INVERT_X_DIR)^direction^BABYSTEP_INVERT_Z);
  1077. WRITE(Y_DIR_PIN,(INVERT_Y_DIR)^direction^BABYSTEP_INVERT_Z);
  1078. WRITE(Z_DIR_PIN,(INVERT_Z_DIR)^direction^BABYSTEP_INVERT_Z);
  1079. //perform step
  1080. WRITE(X_STEP_PIN, !INVERT_X_STEP_PIN);
  1081. WRITE(Y_STEP_PIN, !INVERT_Y_STEP_PIN);
  1082. WRITE(Z_STEP_PIN, !INVERT_Z_STEP_PIN);
  1083. //wait a tiny bit
  1084. {
  1085. float x=1./float(axis+1); //absolutely useless
  1086. }
  1087. WRITE(X_STEP_PIN, INVERT_X_STEP_PIN);
  1088. WRITE(Y_STEP_PIN, INVERT_Y_STEP_PIN);
  1089. WRITE(Z_STEP_PIN, INVERT_Z_STEP_PIN);
  1090. //get old pin state back.
  1091. WRITE(X_DIR_PIN,old_x_dir_pin);
  1092. WRITE(Y_DIR_PIN,old_y_dir_pin);
  1093. WRITE(Z_DIR_PIN,old_z_dir_pin);
  1094. }
  1095. break;
  1096. #endif
  1097. default: break;
  1098. }
  1099. }
  1100. #endif //BABYSTEPPING
  1101. void digitalPotWrite(int address, int value) // From Arduino DigitalPotControl example
  1102. {
  1103. #if defined(DIGIPOTSS_PIN) && DIGIPOTSS_PIN > -1
  1104. digitalWrite(DIGIPOTSS_PIN,LOW); // take the SS pin low to select the chip
  1105. SPI.transfer(address); // send in the address and value via SPI:
  1106. SPI.transfer(value);
  1107. digitalWrite(DIGIPOTSS_PIN,HIGH); // take the SS pin high to de-select the chip:
  1108. //delay(10);
  1109. #endif
  1110. }
  1111. void digipot_init() //Initialize Digipot Motor Current
  1112. {
  1113. #if defined(DIGIPOTSS_PIN) && DIGIPOTSS_PIN > -1
  1114. const uint8_t digipot_motor_current[] = DIGIPOT_MOTOR_CURRENT;
  1115. SPI.begin();
  1116. pinMode(DIGIPOTSS_PIN, OUTPUT);
  1117. for(int i=0;i<=4;i++)
  1118. //digitalPotWrite(digipot_ch[i], digipot_motor_current[i]);
  1119. digipot_current(i,digipot_motor_current[i]);
  1120. #endif
  1121. #ifdef MOTOR_CURRENT_PWM_XY_PIN
  1122. pinMode(MOTOR_CURRENT_PWM_XY_PIN, OUTPUT);
  1123. pinMode(MOTOR_CURRENT_PWM_Z_PIN, OUTPUT);
  1124. pinMode(MOTOR_CURRENT_PWM_E_PIN, OUTPUT);
  1125. digipot_current(0, motor_current_setting[0]);
  1126. digipot_current(1, motor_current_setting[1]);
  1127. digipot_current(2, motor_current_setting[2]);
  1128. //Set timer5 to 31khz so the PWM of the motor power is as constant as possible. (removes a buzzing noise)
  1129. TCCR5B = (TCCR5B & ~(_BV(CS50) | _BV(CS51) | _BV(CS52))) | _BV(CS50);
  1130. #endif
  1131. }
  1132. void digipot_current(uint8_t driver, int current)
  1133. {
  1134. #if defined(DIGIPOTSS_PIN) && DIGIPOTSS_PIN > -1
  1135. const uint8_t digipot_ch[] = DIGIPOT_CHANNELS;
  1136. digitalPotWrite(digipot_ch[driver], current);
  1137. #endif
  1138. #ifdef MOTOR_CURRENT_PWM_XY_PIN
  1139. if (driver == 0) analogWrite(MOTOR_CURRENT_PWM_XY_PIN, (long)current * 255L / (long)MOTOR_CURRENT_PWM_RANGE);
  1140. if (driver == 1) analogWrite(MOTOR_CURRENT_PWM_Z_PIN, (long)current * 255L / (long)MOTOR_CURRENT_PWM_RANGE);
  1141. if (driver == 2) analogWrite(MOTOR_CURRENT_PWM_E_PIN, (long)current * 255L / (long)MOTOR_CURRENT_PWM_RANGE);
  1142. #endif
  1143. }
  1144. void microstep_init()
  1145. {
  1146. const uint8_t microstep_modes[] = MICROSTEP_MODES;
  1147. #if defined(E1_MS1_PIN) && E1_MS1_PIN > -1
  1148. pinMode(E1_MS1_PIN,OUTPUT);
  1149. pinMode(E1_MS2_PIN,OUTPUT);
  1150. #endif
  1151. #if defined(X_MS1_PIN) && X_MS1_PIN > -1
  1152. pinMode(X_MS1_PIN,OUTPUT);
  1153. pinMode(X_MS2_PIN,OUTPUT);
  1154. pinMode(Y_MS1_PIN,OUTPUT);
  1155. pinMode(Y_MS2_PIN,OUTPUT);
  1156. pinMode(Z_MS1_PIN,OUTPUT);
  1157. pinMode(Z_MS2_PIN,OUTPUT);
  1158. pinMode(E0_MS1_PIN,OUTPUT);
  1159. pinMode(E0_MS2_PIN,OUTPUT);
  1160. for(int i=0;i<=4;i++) microstep_mode(i,microstep_modes[i]);
  1161. #endif
  1162. }
  1163. void microstep_ms(uint8_t driver, int8_t ms1, int8_t ms2)
  1164. {
  1165. if(ms1 > -1) switch(driver)
  1166. {
  1167. case 0: digitalWrite( X_MS1_PIN,ms1); break;
  1168. case 1: digitalWrite( Y_MS1_PIN,ms1); break;
  1169. case 2: digitalWrite( Z_MS1_PIN,ms1); break;
  1170. case 3: digitalWrite(E0_MS1_PIN,ms1); break;
  1171. #if defined(E1_MS1_PIN) && E1_MS1_PIN > -1
  1172. case 4: digitalWrite(E1_MS1_PIN,ms1); break;
  1173. #endif
  1174. }
  1175. if(ms2 > -1) switch(driver)
  1176. {
  1177. case 0: digitalWrite( X_MS2_PIN,ms2); break;
  1178. case 1: digitalWrite( Y_MS2_PIN,ms2); break;
  1179. case 2: digitalWrite( Z_MS2_PIN,ms2); break;
  1180. case 3: digitalWrite(E0_MS2_PIN,ms2); break;
  1181. #if defined(E1_MS2_PIN) && E1_MS2_PIN > -1
  1182. case 4: digitalWrite(E1_MS2_PIN,ms2); break;
  1183. #endif
  1184. }
  1185. }
  1186. void microstep_mode(uint8_t driver, uint8_t stepping_mode)
  1187. {
  1188. switch(stepping_mode)
  1189. {
  1190. case 1: microstep_ms(driver,MICROSTEP1); break;
  1191. case 2: microstep_ms(driver,MICROSTEP2); break;
  1192. case 4: microstep_ms(driver,MICROSTEP4); break;
  1193. case 8: microstep_ms(driver,MICROSTEP8); break;
  1194. case 16: microstep_ms(driver,MICROSTEP16); break;
  1195. }
  1196. }
  1197. void microstep_readings()
  1198. {
  1199. SERIAL_PROTOCOLPGM("MS1,MS2 Pins\n");
  1200. SERIAL_PROTOCOLPGM("X: ");
  1201. SERIAL_PROTOCOL( digitalRead(X_MS1_PIN));
  1202. SERIAL_PROTOCOLLN( digitalRead(X_MS2_PIN));
  1203. SERIAL_PROTOCOLPGM("Y: ");
  1204. SERIAL_PROTOCOL( digitalRead(Y_MS1_PIN));
  1205. SERIAL_PROTOCOLLN( digitalRead(Y_MS2_PIN));
  1206. SERIAL_PROTOCOLPGM("Z: ");
  1207. SERIAL_PROTOCOL( digitalRead(Z_MS1_PIN));
  1208. SERIAL_PROTOCOLLN( digitalRead(Z_MS2_PIN));
  1209. SERIAL_PROTOCOLPGM("E0: ");
  1210. SERIAL_PROTOCOL( digitalRead(E0_MS1_PIN));
  1211. SERIAL_PROTOCOLLN( digitalRead(E0_MS2_PIN));
  1212. #if defined(E1_MS1_PIN) && E1_MS1_PIN > -1
  1213. SERIAL_PROTOCOLPGM("E1: ");
  1214. SERIAL_PROTOCOL( digitalRead(E1_MS1_PIN));
  1215. SERIAL_PROTOCOLLN( digitalRead(E1_MS2_PIN));
  1216. #endif
  1217. }