sm4.c 6.1 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246
  1. //sm4.c - simple 4-axis stepper control
  2. #include "sm4.h"
  3. #include <avr/io.h>
  4. #include <avr/pgmspace.h>
  5. #include <math.h>
  6. #include "Arduino.h"
  7. #include "boards.h"
  8. #define false 0
  9. #define true 1
  10. #include "Configuration_var.h"
  11. #ifdef NEW_XYZCAL
  12. // Signal pinouts
  13. // direction signal - MiniRambo
  14. //#define X_DIR_PIN 48 //PL1 (-)
  15. //#define Y_DIR_PIN 49 //PL0 (-)
  16. //#define Z_DIR_PIN 47 //PL2 (-)
  17. //#define E0_DIR_PIN 43 //PL6 (+)
  18. //direction signal - EinsyRambo
  19. //#define X_DIR_PIN 49 //PL0 (+)
  20. //#define Y_DIR_PIN 48 //PL1 (-)
  21. //#define Z_DIR_PIN 47 //PL2 (+)
  22. //#define E0_DIR_PIN 43 //PL6 (-)
  23. //step signal pinout - common for all rambo boards
  24. //#define X_STEP_PIN 37 //PC0 (+)
  25. //#define Y_STEP_PIN 36 //PC1 (+)
  26. //#define Z_STEP_PIN 35 //PC2 (+)
  27. //#define E0_STEP_PIN 34 //PC3 (+)
  28. #define XDIR INVERT_X_DIR:!INVERT_X_DIR
  29. #define YDIR INVERT_Y_DIR:!INVERT_Y_DIR
  30. #define ZDIR INVERT_Z_DIR:!INVERT_Z_DIR
  31. #define EDIR INVERT_E0_DIR:!INVERT_E0_DIR
  32. uint8_t dir_mask = 0x0F^(INVERT_X_DIR | (INVERT_Y_DIR << 1) | (INVERT_Z_DIR << 2) | (INVERT_E0_DIR << 3));
  33. sm4_stop_cb_t sm4_stop_cb = 0;
  34. sm4_update_pos_cb_t sm4_update_pos_cb = 0;
  35. sm4_calc_delay_cb_t sm4_calc_delay_cb = 0;
  36. uint16_t sm4_cpu_time = 0;
  37. uint8_t sm4_get_dir(uint8_t axis)
  38. {
  39. switch (axis)
  40. {
  41. #if ((MOTHERBOARD == BOARD_RAMBO_MINI_1_0) || (MOTHERBOARD == BOARD_RAMBO_MINI_1_3))
  42. case 0: return (PORTL & 2)?XDIR;
  43. case 1: return (PORTL & 1)?YDIR;
  44. case 2: return (PORTL & 4)?ZDIR;
  45. case 3: return (PORTL & 64)?EDIR;
  46. #elif ((MOTHERBOARD == BOARD_EINSY_1_0a))
  47. case 0: return (PORTL & 1)?XDIR;
  48. case 1: return (PORTL & 2)?YDIR;
  49. case 2: return (PORTL & 4)?ZDIR;
  50. case 3: return (PORTL & 64)?EDIR;
  51. #endif
  52. }
  53. return 0;
  54. }
  55. void sm4_set_dir(uint8_t axis, uint8_t dir)
  56. {
  57. switch (axis)
  58. {
  59. #if ((MOTHERBOARD == BOARD_RAMBO_MINI_1_0) || (MOTHERBOARD == BOARD_RAMBO_MINI_1_3))
  60. case 0: if (dir == INVERT_X_DIR) PORTL |= 2; else PORTL &= ~2; break;
  61. case 1: if (dir == INVERT_Y_DIR) PORTL |= 1; else PORTL &= ~1; break;
  62. case 2: if (dir == INVERT_Z_DIR) PORTL |= 4; else PORTL &= ~4; break;
  63. case 3: if (dir == INVERT_E0_DIR) PORTL |= 64; else PORTL &= ~64; break;
  64. #elif ((MOTHERBOARD == BOARD_EINSY_1_0a))
  65. case 0: if (dir == INVERT_X_DIR) PORTL |= 1; else PORTL &= ~1; break;
  66. case 1: if (dir == INVERT_Y_DIR) PORTL |= 2; else PORTL &= ~2; break;
  67. case 2: if (dir == INVERT_Z_DIR) PORTL |= 4; else PORTL &= ~4; break;
  68. case 3: if (dir == INVERT_E0_DIR) PORTL |= 64; else PORTL &= ~64; break;
  69. #endif
  70. }
  71. asm("nop");
  72. }
  73. uint8_t sm4_get_dir_bits(void)
  74. {
  75. register uint8_t dir_bits = 0;
  76. register uint8_t portL = PORTL;
  77. //TODO -optimize in asm
  78. #if ((MOTHERBOARD == BOARD_RAMBO_MINI_1_0) || (MOTHERBOARD == BOARD_RAMBO_MINI_1_3))
  79. if (portL & 2) dir_bits |= 1;
  80. if (portL & 1) dir_bits |= 2;
  81. if (portL & 4) dir_bits |= 4;
  82. if (portL & 64) dir_bits |= 8;
  83. dir_bits ^= dir_mask;
  84. #elif ((MOTHERBOARD == BOARD_EINSY_1_0a))
  85. if (portL & 1) dir_bits |= 1;
  86. if (portL & 2) dir_bits |= 2;
  87. if (portL & 4) dir_bits |= 4;
  88. if (portL & 64) dir_bits |= 8;
  89. dir_bits ^= dir_mask;
  90. #endif
  91. return dir_bits;
  92. }
  93. void sm4_set_dir_bits(uint8_t dir_bits)
  94. {
  95. register uint8_t portL = PORTL;
  96. portL &= 0xb8; //set direction bits to zero
  97. //TODO -optimize in asm
  98. #if ((MOTHERBOARD == BOARD_RAMBO_MINI_1_0) || (MOTHERBOARD == BOARD_RAMBO_MINI_1_3))
  99. dir_bits ^= dir_mask;
  100. if (dir_bits & 1) portL |= 2; //set X direction bit
  101. if (dir_bits & 2) portL |= 1; //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. #elif ((MOTHERBOARD == BOARD_EINSY_1_0a))
  105. dir_bits ^= dir_mask;
  106. if (dir_bits & 1) portL |= 1; //set X direction bit
  107. if (dir_bits & 2) portL |= 2; //set Y direction bit
  108. if (dir_bits & 4) portL |= 4; //set Z direction bit
  109. if (dir_bits & 8) portL |= 64; //set E direction bit
  110. #endif
  111. PORTL = portL;
  112. asm("nop");
  113. }
  114. void sm4_do_step(uint8_t axes_mask)
  115. {
  116. #if ((MOTHERBOARD == BOARD_RAMBO_MINI_1_0) || (MOTHERBOARD == BOARD_RAMBO_MINI_1_3) || (MOTHERBOARD == BOARD_EINSY_1_0a))
  117. #ifdef TMC2130_DEDGE_STEPPING
  118. PINC = (axes_mask & 0x0f); // toggle step signals by mask
  119. #else
  120. register uint8_t portC = PORTC & 0xf0;
  121. PORTC = portC | (axes_mask & 0x0f); //set step signals by mask
  122. asm("nop");
  123. PORTC = portC; //set step signals to zero
  124. asm("nop");
  125. #endif
  126. #endif //((MOTHERBOARD == BOARD_RAMBO_MINI_1_0) || (MOTHERBOARD == BOARD_RAMBO_MINI_1_3) || (MOTHERBOARD == BOARD_EINSY_1_0a))
  127. }
  128. uint16_t sm4_line_xyze_ui(uint16_t dx, uint16_t dy, uint16_t dz, uint16_t de)
  129. {
  130. 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);
  131. uint16_t nd = dd;
  132. uint16_t cx = dd;
  133. uint16_t cy = dd;
  134. uint16_t cz = dd;
  135. uint16_t ce = dd;
  136. uint16_t x = 0;
  137. uint16_t y = 0;
  138. uint16_t z = 0;
  139. uint16_t e = 0;
  140. while (nd)
  141. {
  142. if (sm4_stop_cb && (*sm4_stop_cb)()) break;
  143. uint8_t sm = 0; //step mask
  144. if (cx <= dx)
  145. {
  146. sm |= 1;
  147. cx += dd;
  148. x++;
  149. }
  150. if (cy <= dy)
  151. {
  152. sm |= 2;
  153. cy += dd;
  154. y++;
  155. }
  156. if (cz <= dz)
  157. {
  158. sm |= 4;
  159. cz += dd;
  160. z++;
  161. }
  162. if (ce <= de)
  163. {
  164. sm |= 8;
  165. ce += dd;
  166. e++;
  167. }
  168. cx -= dx;
  169. cy -= dy;
  170. cz -= dz;
  171. ce -= de;
  172. sm4_do_step(sm);
  173. uint16_t delay = SM4_DEFDELAY;
  174. if (sm4_calc_delay_cb) delay = (*sm4_calc_delay_cb)(nd, dd);
  175. if (delay) delayMicroseconds(delay);
  176. nd--;
  177. }
  178. if (sm4_update_pos_cb) (*sm4_update_pos_cb)(x, y, z, e);
  179. return nd;
  180. }
  181. uint16_t sm4_line_xyz_ui(uint16_t dx, uint16_t dy, uint16_t dz){
  182. uint16_t dd = (uint16_t)(sqrt((float)(((uint32_t)dx)*dx + ((uint32_t)dy*dy) + ((uint32_t)dz*dz))) + 0.5);
  183. uint16_t nd = dd;
  184. uint16_t cx = dd;
  185. uint16_t cy = dd;
  186. uint16_t cz = dd;
  187. uint16_t x = 0;
  188. uint16_t y = 0;
  189. uint16_t z = 0;
  190. while (nd){
  191. if (sm4_stop_cb && (*sm4_stop_cb)()) break;
  192. uint8_t sm = 0; //step mask
  193. if (cx <= dx){
  194. sm |= 1;
  195. cx += dd;
  196. x++;
  197. }
  198. if (cy <= dy){
  199. sm |= 2;
  200. cy += dd;
  201. y++;
  202. }
  203. if (cz <= dz){
  204. sm |= 4;
  205. cz += dd;
  206. z++;
  207. }
  208. cx -= dx;
  209. cy -= dy;
  210. cz -= dz;
  211. sm4_do_step(sm);
  212. uint16_t delay = SM4_DEFDELAY;
  213. if (sm4_calc_delay_cb) delay = (*sm4_calc_delay_cb)(nd, dd);
  214. if (delay) delayMicroseconds(delay);
  215. nd--;
  216. }
  217. if (sm4_update_pos_cb)
  218. (*sm4_update_pos_cb)(x, y, z, 0);
  219. return nd;
  220. }
  221. #endif //NEW_XYZCAL