sm4.c 4.9 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194
  1. //sm4.c - simple 4-axis stepper control
  2. #include "sm4.h"
  3. #include <avr/io.h>
  4. #include <avr/pgmspace.h>
  5. #include "boards.h"
  6. #define bool int8_t
  7. #define false 0
  8. #define true 1
  9. #include "Configuration_prusa.h"
  10. #ifdef NEW_XYZCAL
  11. // Signal pinouts
  12. // direction signal - MiniRambo
  13. //#define X_DIR_PIN 48 //PL1 (-)
  14. //#define Y_DIR_PIN 49 //PL0 (-)
  15. //#define Z_DIR_PIN 47 //PL2 (-)
  16. //#define E0_DIR_PIN 43 //PL6 (+)
  17. //direction signal - EinsyRambo
  18. //#define X_DIR_PIN 49 //PL0 (+)
  19. //#define Y_DIR_PIN 48 //PL1 (-)
  20. //#define Z_DIR_PIN 47 //PL2 (+)
  21. //#define E0_DIR_PIN 43 //PL6 (-)
  22. //step signal pinout - common for all rambo boards
  23. //#define X_STEP_PIN 37 //PC0 (+)
  24. //#define Y_STEP_PIN 36 //PC1 (+)
  25. //#define Z_STEP_PIN 35 //PC2 (+)
  26. //#define E0_STEP_PIN 34 //PC3 (+)
  27. sm4_stop_cb_t sm4_stop_cb = 0;
  28. sm4_update_pos_cb_t sm4_update_pos_cb = 0;
  29. sm4_calc_delay_cb_t sm4_calc_delay_cb = 0;
  30. uint16_t sm4_cpu_time = 0;
  31. uint8_t sm4_get_dir(uint8_t axis)
  32. {
  33. switch (axis)
  34. {
  35. #if ((MOTHERBOARD == BOARD_RAMBO_MINI_1_0) || (MOTHERBOARD == BOARD_RAMBO_MINI_1_3))
  36. case 0: return (PORTL & 2)?0:1;
  37. case 1: return (PORTL & 1)?0:1;
  38. case 2: return (PORTL & 4)?0:1;
  39. case 3: return (PORTL & 64)?1:0;
  40. #elif ((MOTHERBOARD == BOARD_EINSY_1_0a))
  41. case 0: return (PORTL & 1)?1:0;
  42. case 1: return (PORTL & 2)?0:1;
  43. case 2: return (PORTL & 4)?1:0;
  44. case 3: return (PORTL & 64)?0:1;
  45. #endif
  46. }
  47. return 0;
  48. }
  49. void sm4_set_dir(uint8_t axis, uint8_t dir)
  50. {
  51. switch (axis)
  52. {
  53. #if ((MOTHERBOARD == BOARD_RAMBO_MINI_1_0) || (MOTHERBOARD == BOARD_RAMBO_MINI_1_3))
  54. case 0: if (!dir) PORTL |= 2; else PORTL &= ~2; break;
  55. case 1: if (!dir) PORTL |= 1; else PORTL &= ~1; break;
  56. case 2: if (!dir) PORTL |= 4; else PORTL &= ~4; break;
  57. case 3: if (dir) PORTL |= 64; else PORTL &= ~64; break;
  58. #elif ((MOTHERBOARD == BOARD_EINSY_1_0a))
  59. case 0: if (dir) PORTL |= 1; else PORTL &= ~1; break;
  60. case 1: if (!dir) PORTL |= 2; else PORTL &= ~2; break;
  61. case 2: if (dir) PORTL |= 4; else PORTL &= ~4; break;
  62. case 3: if (!dir) PORTL |= 64; else PORTL &= ~64; break;
  63. #endif
  64. }
  65. asm("nop");
  66. }
  67. uint8_t sm4_get_dir_bits(void)
  68. {
  69. uint8_t register dir_bits = 0;
  70. uint8_t register portL = PORTL;
  71. //TODO -optimize in asm
  72. #if ((MOTHERBOARD == BOARD_RAMBO_MINI_1_0) || (MOTHERBOARD == BOARD_RAMBO_MINI_1_3))
  73. if (portL & 2) dir_bits |= 1;
  74. if (portL & 1) dir_bits |= 2;
  75. if (portL & 4) dir_bits |= 4;
  76. if (portL & 64) dir_bits |= 8;
  77. dir_bits ^= 0x07; //invert XYZ, do not invert E
  78. #elif ((MOTHERBOARD == BOARD_EINSY_1_0a))
  79. if (portL & 1) dir_bits |= 1;
  80. if (portL & 2) dir_bits |= 2;
  81. if (portL & 4) dir_bits |= 4;
  82. if (portL & 64) dir_bits |= 8;
  83. dir_bits ^= 0x0a; //invert YE, do not invert XZ
  84. #endif
  85. return dir_bits;
  86. }
  87. void sm4_set_dir_bits(uint8_t dir_bits)
  88. {
  89. uint8_t register portL = PORTL;
  90. portL &= 0xb8; //set direction bits to zero
  91. //TODO -optimize in asm
  92. #if ((MOTHERBOARD == BOARD_RAMBO_MINI_1_0) || (MOTHERBOARD == BOARD_RAMBO_MINI_1_3))
  93. dir_bits ^= 0x07; //invert XYZ, do not invert E
  94. if (dir_bits & 1) portL |= 2; //set X direction bit
  95. if (dir_bits & 2) portL |= 1; //set Y direction bit
  96. if (dir_bits & 4) portL |= 4; //set Z direction bit
  97. if (dir_bits & 8) portL |= 64; //set E direction bit
  98. #elif ((MOTHERBOARD == BOARD_EINSY_1_0a))
  99. dir_bits ^= 0x0a; //invert YE, do not invert XZ
  100. if (dir_bits & 1) portL |= 1; //set X direction bit
  101. if (dir_bits & 2) portL |= 2; //set Y direction bit
  102. if (dir_bits & 4) portL |= 4; //set Z direction bit
  103. if (dir_bits & 8) portL |= 64; //set E direction bit
  104. #endif
  105. PORTL = portL;
  106. asm("nop");
  107. }
  108. void sm4_do_step(uint8_t axes_mask)
  109. {
  110. #if ((MOTHERBOARD == BOARD_RAMBO_MINI_1_0) || (MOTHERBOARD == BOARD_RAMBO_MINI_1_3) || (MOTHERBOARD == BOARD_EINSY_1_0a))
  111. uint8_t register portC = PORTC & 0xf0;
  112. PORTC = portC | (axes_mask & 0x0f); //set step signals by mask
  113. asm("nop");
  114. PORTC = portC; //set step signals to zero
  115. asm("nop");
  116. #endif //((MOTHERBOARD == BOARD_RAMBO_MINI_1_0) || (MOTHERBOARD == BOARD_RAMBO_MINI_1_3) || (MOTHERBOARD == BOARD_EINSY_1_0a))
  117. }
  118. uint16_t sm4_line_xyze_ui(uint16_t dx, uint16_t dy, uint16_t dz, uint16_t de)
  119. {
  120. uint16_t dd = (uint16_t)(sqrt((float)(((uint32_t)dx)*dx + ((uint32_t)dy*dy) + ((uint32_t)dz*dz) + ((uint32_t)de*de))) + 0.5);
  121. uint16_t nd = dd;
  122. uint16_t cx = dd;
  123. uint16_t cy = dd;
  124. uint16_t cz = dd;
  125. uint16_t ce = dd;
  126. uint16_t x = 0;
  127. uint16_t y = 0;
  128. uint16_t z = 0;
  129. uint16_t e = 0;
  130. while (nd)
  131. {
  132. if (sm4_stop_cb && (*sm4_stop_cb)()) break;
  133. uint8_t sm = 0; //step mask
  134. if (cx <= dx)
  135. {
  136. sm |= 1;
  137. cx += dd;
  138. x++;
  139. }
  140. if (cy <= dy)
  141. {
  142. sm |= 2;
  143. cy += dd;
  144. y++;
  145. }
  146. if (cz <= dz)
  147. {
  148. sm |= 4;
  149. cz += dd;
  150. z++;
  151. }
  152. if (ce <= de)
  153. {
  154. sm |= 4;
  155. ce += dd;
  156. e++;
  157. }
  158. cx -= dx;
  159. cy -= dy;
  160. cz -= dz;
  161. ce -= de;
  162. sm4_do_step(sm);
  163. uint16_t delay = SM4_DEFDELAY;
  164. if (sm4_calc_delay_cb) delay = (*sm4_calc_delay_cb)(nd, dd);
  165. if (delay) delayMicroseconds(delay);
  166. nd--;
  167. }
  168. if (sm4_update_pos_cb) (*sm4_update_pos_cb)(x, y, z, e);
  169. return nd;
  170. }
  171. #endif //NEW_XYZCAL