My Marlin configs for Fabrikator Mini and CTC i3 Pro B
Vous ne pouvez pas sélectionner plus de 25 sujets Les noms de sujets doivent commencer par une lettre ou un nombre, peuvent contenir des tirets ('-') et peuvent comporter jusqu'à 35 caractères.

scara.cpp 4.8KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162
  1. /**
  2. * Marlin 3D Printer Firmware
  3. * Copyright (c) 2019 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
  4. *
  5. * Based on Sprinter and grbl.
  6. * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
  7. *
  8. * This program is free software: you can redistribute it and/or modify
  9. * it under the terms of the GNU General Public License as published by
  10. * the Free Software Foundation, either version 3 of the License, or
  11. * (at your option) any later version.
  12. *
  13. * This program is distributed in the hope that it will be useful,
  14. * but WITHOUT ANY WARRANTY; without even the implied warranty of
  15. * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  16. * GNU General Public License for more details.
  17. *
  18. * You should have received a copy of the GNU General Public License
  19. * along with this program. If not, see <http://www.gnu.org/licenses/>.
  20. *
  21. */
  22. /**
  23. * scara.cpp
  24. */
  25. #include "../inc/MarlinConfig.h"
  26. #if IS_SCARA
  27. #include "scara.h"
  28. #include "motion.h"
  29. #include "planner.h"
  30. float delta_segments_per_second = SCARA_SEGMENTS_PER_SECOND;
  31. void scara_set_axis_is_at_home(const AxisEnum axis) {
  32. if (axis == Z_AXIS)
  33. current_position.z = Z_HOME_POS;
  34. else {
  35. /**
  36. * SCARA homes XY at the same time
  37. */
  38. xyz_pos_t homeposition;
  39. LOOP_XYZ(i) homeposition[i] = base_home_pos((AxisEnum)i);
  40. #if ENABLED(MORGAN_SCARA)
  41. // MORGAN_SCARA uses arm angles for AB home position
  42. // SERIAL_ECHOLNPAIR("homeposition A:", homeposition.a, " B:", homeposition.b);
  43. inverse_kinematics(homeposition);
  44. forward_kinematics_SCARA(delta.a, delta.b);
  45. current_position[axis] = cartes[axis];
  46. #else
  47. // MP_SCARA uses a Cartesian XY home position
  48. // SERIAL_ECHOLNPAIR("homeposition X:", homeposition.x, " Y:", homeposition.y);
  49. current_position[axis] = homeposition[axis];
  50. #endif
  51. // SERIAL_ECHOLNPAIR("Cartesian X:", current_position.x, " Y:", current_position.y);
  52. update_software_endstops(axis);
  53. }
  54. }
  55. static constexpr xy_pos_t scara_offset = { SCARA_OFFSET_X, SCARA_OFFSET_Y };
  56. /**
  57. * Morgan SCARA Forward Kinematics. Results in 'cartes'.
  58. * Maths and first version by QHARLEY.
  59. * Integrated into Marlin and slightly restructured by Joachim Cerny.
  60. */
  61. void forward_kinematics_SCARA(const float &a, const float &b) {
  62. const float a_sin = sin(RADIANS(a)) * L1,
  63. a_cos = cos(RADIANS(a)) * L1,
  64. b_sin = sin(RADIANS(b)) * L2,
  65. b_cos = cos(RADIANS(b)) * L2;
  66. cartes.set(a_cos + b_cos + scara_offset.x, // theta
  67. a_sin + b_sin + scara_offset.y); // theta+phi
  68. /*
  69. SERIAL_ECHOLNPAIR(
  70. "SCARA FK Angle a=", a,
  71. " b=", b,
  72. " a_sin=", a_sin,
  73. " a_cos=", a_cos,
  74. " b_sin=", b_sin,
  75. " b_cos=", b_cos
  76. );
  77. SERIAL_ECHOLNPAIR(" cartes (X,Y) = "(cartes.x, ", ", cartes.y, ")");
  78. //*/
  79. }
  80. void inverse_kinematics(const xyz_pos_t &raw) {
  81. #if ENABLED(MORGAN_SCARA)
  82. /**
  83. * Morgan SCARA Inverse Kinematics. Results in 'delta'.
  84. *
  85. * See http://forums.reprap.org/read.php?185,283327
  86. *
  87. * Maths and first version by QHARLEY.
  88. * Integrated into Marlin and slightly restructured by Joachim Cerny.
  89. */
  90. float C2, S2, SK1, SK2, THETA, PSI;
  91. // Translate SCARA to standard XY with scaling factor
  92. const xy_pos_t spos = raw - scara_offset;
  93. const float H2 = HYPOT2(spos.x, spos.y);
  94. if (L1 == L2)
  95. C2 = H2 / L1_2_2 - 1;
  96. else
  97. C2 = (H2 - (L1_2 + L2_2)) / (2.0f * L1 * L2);
  98. S2 = SQRT(1.0f - sq(C2));
  99. // Unrotated Arm1 plus rotated Arm2 gives the distance from Center to End
  100. SK1 = L1 + L2 * C2;
  101. // Rotated Arm2 gives the distance from Arm1 to Arm2
  102. SK2 = L2 * S2;
  103. // Angle of Arm1 is the difference between Center-to-End angle and the Center-to-Elbow
  104. THETA = ATAN2(SK1, SK2) - ATAN2(spos.x, spos.y);
  105. // Angle of Arm2
  106. PSI = ATAN2(S2, C2);
  107. delta.set(DEGREES(THETA), DEGREES(THETA + PSI), raw.z);
  108. /*
  109. DEBUG_POS("SCARA IK", raw);
  110. DEBUG_POS("SCARA IK", delta);
  111. SERIAL_ECHOLNPAIR(" SCARA (x,y) ", sx, ",", sy, " C2=", C2, " S2=", S2, " Theta=", THETA, " Phi=", PHI);
  112. //*/
  113. #else // MP_SCARA
  114. const float x = raw.x, y = raw.y, c = HYPOT(x, y),
  115. THETA3 = ATAN2(y, x),
  116. THETA1 = THETA3 + ACOS((sq(c) + sq(L1) - sq(L2)) / (2.0f * c * L1)),
  117. THETA2 = THETA3 - ACOS((sq(c) + sq(L2) - sq(L1)) / (2.0f * c * L2));
  118. delta.set(DEGREES(THETA1), DEGREES(THETA2), raw.z);
  119. /*
  120. DEBUG_POS("SCARA IK", raw);
  121. DEBUG_POS("SCARA IK", delta);
  122. SERIAL_ECHOLNPAIR(" SCARA (x,y) ", x, ",", y," Theta1=", THETA1, " Theta2=", THETA2);
  123. //*/
  124. #endif // MP_SCARA
  125. }
  126. void scara_report_positions() {
  127. SERIAL_ECHOLNPAIR("SCARA Theta:", planner.get_axis_position_degrees(A_AXIS), " Psi+Theta:", planner.get_axis_position_degrees(B_AXIS));
  128. SERIAL_EOL();
  129. }
  130. #endif // IS_SCARA