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.

scara.cpp 4.7KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155
  1. /**
  2. * Marlin 3D Printer Firmware
  3. * Copyright (C) 2016 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_AXIS] = Z_HOME_POS;
  34. else {
  35. /**
  36. * SCARA homes XY at the same time
  37. */
  38. float homeposition[XYZ];
  39. LOOP_XYZ(i) homeposition[i] = base_home_pos((AxisEnum)i);
  40. // SERIAL_ECHOPAIR("homeposition X:", homeposition[X_AXIS]);
  41. // SERIAL_ECHOLNPAIR(" Y:", homeposition[Y_AXIS]);
  42. /**
  43. * Get Home position SCARA arm angles using inverse kinematics,
  44. * and calculate homing offset using forward kinematics
  45. */
  46. inverse_kinematics(homeposition);
  47. forward_kinematics_SCARA(delta[A_AXIS], delta[B_AXIS]);
  48. // SERIAL_ECHOPAIR("Cartesian X:", cartes[X_AXIS]);
  49. // SERIAL_ECHOLNPAIR(" Y:", cartes[Y_AXIS]);
  50. current_position[axis] = cartes[axis];
  51. /**
  52. * SCARA home positions are based on configuration since the actual
  53. * limits are determined by the inverse kinematic transform.
  54. */
  55. soft_endstop_min[axis] = base_min_pos(axis); // + (cartes[axis] - base_home_pos(axis));
  56. soft_endstop_max[axis] = base_max_pos(axis); // + (cartes[axis] - base_home_pos(axis));
  57. }
  58. }
  59. /**
  60. * Morgan SCARA Forward Kinematics. Results in cartes[].
  61. * Maths and first version by QHARLEY.
  62. * Integrated into Marlin and slightly restructured by Joachim Cerny.
  63. */
  64. void forward_kinematics_SCARA(const float &a, const float &b) {
  65. const float a_sin = sin(RADIANS(a)) * L1,
  66. a_cos = cos(RADIANS(a)) * L1,
  67. b_sin = sin(RADIANS(b)) * L2,
  68. b_cos = cos(RADIANS(b)) * L2;
  69. cartes[X_AXIS] = a_cos + b_cos + SCARA_OFFSET_X; //theta
  70. cartes[Y_AXIS] = a_sin + b_sin + SCARA_OFFSET_Y; //theta+phi
  71. /*
  72. SERIAL_ECHOPAIR("SCARA FK Angle a=", a);
  73. SERIAL_ECHOPAIR(" b=", b);
  74. SERIAL_ECHOPAIR(" a_sin=", a_sin);
  75. SERIAL_ECHOPAIR(" a_cos=", a_cos);
  76. SERIAL_ECHOPAIR(" b_sin=", b_sin);
  77. SERIAL_ECHOLNPAIR(" b_cos=", b_cos);
  78. SERIAL_ECHOPAIR(" cartes[X_AXIS]=", cartes[X_AXIS]);
  79. SERIAL_ECHOLNPAIR(" cartes[Y_AXIS]=", cartes[Y_AXIS]);
  80. //*/
  81. }
  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. void inverse_kinematics(const float raw[XYZ]) {
  91. static float C2, S2, SK1, SK2, THETA, PSI;
  92. float sx = raw[X_AXIS] - SCARA_OFFSET_X, // Translate SCARA to standard X Y
  93. sy = raw[Y_AXIS] - SCARA_OFFSET_Y; // With scaling factor.
  94. if (L1 == L2)
  95. C2 = HYPOT2(sx, sy) / L1_2_2 - 1;
  96. else
  97. C2 = (HYPOT2(sx, sy) - (L1_2 + L2_2)) / (2.0 * L1 * L2);
  98. S2 = SQRT(1 - 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(sx, sy);
  105. // Angle of Arm2
  106. PSI = ATAN2(S2, C2);
  107. delta[A_AXIS] = DEGREES(THETA); // theta is support arm angle
  108. delta[B_AXIS] = DEGREES(THETA + PSI); // equal to sub arm angle (inverted motor)
  109. delta[C_AXIS] = raw[Z_AXIS];
  110. /*
  111. DEBUG_POS("SCARA IK", raw);
  112. DEBUG_POS("SCARA IK", delta);
  113. SERIAL_ECHOPAIR(" SCARA (x,y) ", sx);
  114. SERIAL_ECHOPAIR(",", sy);
  115. SERIAL_ECHOPAIR(" C2=", C2);
  116. SERIAL_ECHOPAIR(" S2=", S2);
  117. SERIAL_ECHOPAIR(" Theta=", THETA);
  118. SERIAL_ECHOLNPAIR(" Phi=", PHI);
  119. //*/
  120. }
  121. void scara_report_positions() {
  122. SERIAL_PROTOCOLPAIR("SCARA Theta:", planner.get_axis_position_degrees(A_AXIS));
  123. SERIAL_PROTOCOLLNPAIR(" Psi+Theta:", planner.get_axis_position_degrees(B_AXIS));
  124. SERIAL_EOL();
  125. }
  126. #endif // IS_SCARA