浏览代码

TPARA followup

Scott Lahteine 3 年前
父节点
当前提交
dd388aedfd

+ 1
- 1
Marlin/src/gcode/scara/M360-M364.cpp 查看文件

@@ -31,7 +31,7 @@
31 31
 
32 32
 inline bool SCARA_move_to_cal(const uint8_t delta_a, const uint8_t delta_b) {
33 33
   if (IsRunning()) {
34
-    forward_kinematics_SCARA(delta_a, delta_b);
34
+    forward_kinematics(delta_a, delta_b);
35 35
     do_blocking_move_to_xy(cartes);
36 36
     return true;
37 37
   }

+ 1
- 1
Marlin/src/module/delta.cpp 查看文件

@@ -177,7 +177,7 @@ float delta_safe_distance_from_top() {
177 177
  *
178 178
  * The result is stored in the cartes[] array.
179 179
  */
180
-void forward_kinematics_DELTA(const float &z1, const float &z2, const float &z3) {
180
+void forward_kinematics(const float &z1, const float &z2, const float &z3) {
181 181
   // Create a vector in old coordinates along x axis of new coordinate
182 182
   const float p12[3] = { delta_tower[B_AXIS].x - delta_tower[A_AXIS].x, delta_tower[B_AXIS].y - delta_tower[A_AXIS].y, z2 - z1 },
183 183
 

+ 3
- 3
Marlin/src/module/delta.h 查看文件

@@ -120,10 +120,10 @@ float delta_safe_distance_from_top();
120 120
  *
121 121
  * The result is stored in the cartes[] array.
122 122
  */
123
-void forward_kinematics_DELTA(const float &z1, const float &z2, const float &z3);
123
+void forward_kinematics(const float &z1, const float &z2, const float &z3);
124 124
 
125
-FORCE_INLINE void forward_kinematics_DELTA(const abc_float_t &point) {
126
-  forward_kinematics_DELTA(point.a, point.b, point.c);
125
+FORCE_INLINE void forward_kinematics(const abc_float_t &point) {
126
+  forward_kinematics(point.a, point.b, point.c);
127 127
 }
128 128
 
129 129
 void home_delta();

+ 3
- 3
Marlin/src/module/motion.cpp 查看文件

@@ -263,10 +263,10 @@ void sync_plan_position_e() { planner.set_e_position_mm(current_position.e); }
263 263
  */
264 264
 void get_cartesian_from_steppers() {
265 265
   #if ENABLED(DELTA)
266
-    forward_kinematics_DELTA(planner.get_axis_positions_mm());
266
+    forward_kinematics(planner.get_axis_positions_mm());
267 267
   #else
268 268
     #if IS_SCARA
269
-      forward_kinematics_SCARA(
269
+      forward_kinematics(
270 270
           planner.get_axis_position_degrees(A_AXIS)
271 271
         , planner.get_axis_position_degrees(B_AXIS)
272 272
         #if ENABLED(AXEL_TPARA)
@@ -949,7 +949,7 @@ FORCE_INLINE void segment_idle(millis_t &next_idle_ms) {
949 949
 
950 950
   float x_home_pos(const uint8_t extruder) {
951 951
     if (extruder == 0)
952
-      return base_home_pos(X_AXIS);
952
+      return X_HOME_POS;
953 953
     else
954 954
       /**
955 955
        * In dual carriage mode the extruder offset provides an override of the

+ 18
- 17
Marlin/src/module/scara.cpp 查看文件

@@ -33,10 +33,7 @@
33 33
 #include "planner.h"
34 34
 
35 35
 #if ENABLED(AXEL_TPARA)
36
-  // For homing, as in delta
37
-  #include "planner.h"
38 36
   #include "endstops.h"
39
-  #include "../lcd/marlinui.h"
40 37
   #include "../MarlinCore.h"
41 38
 #endif
42 39
 
@@ -46,30 +43,35 @@ void scara_set_axis_is_at_home(const AxisEnum axis) {
46 43
   if (axis == Z_AXIS)
47 44
     current_position.z = Z_HOME_POS;
48 45
   else {
49
-    xyz_pos_t homeposition;
50
-    LOOP_XYZ(i) homeposition[i] = base_home_pos((AxisEnum)i);
51 46
     #if ENABLED(MORGAN_SCARA)
52 47
       // MORGAN_SCARA uses arm angles for AB home position
48
+      ab_float_t homeposition = { SCARA_OFFSET_THETA1, SCARA_OFFSET_THETA2 };
53 49
       //DEBUG_ECHOLNPAIR("homeposition A:", homeposition.a, " B:", homeposition.b);
54
-      inverse_kinematics(homeposition);
55
-      forward_kinematics_SCARA(delta.a, delta.b);
56
-      current_position[axis] = cartes[axis];
57 50
     #elif ENABLED(MP_SCARA)
58 51
       // MP_SCARA uses a Cartesian XY home position
52
+      xyz_pos_t homeposition = { X_HOME_POS, Y_HOME_POS, Z_HOME_POS };
59 53
       //DEBUG_ECHOPGM("homeposition");
60 54
       //DEBUG_ECHOLNPAIR_P(SP_X_LBL, homeposition.x, SP_Y_LBL, homeposition.y);
61
-      delta.a = SCARA_OFFSET_THETA1;
62
-      delta.b = SCARA_OFFSET_THETA2;
63
-      forward_kinematics_SCARA(delta.a, delta.b);
64
-      current_position[axis] = cartes[axis];
65 55
     #elif ENABLED(AXEL_TPARA)
56
+      xyz_pos_t homeposition = { X_HOME_POS, Y_HOME_POS, Z_HOME_POS };
66 57
       //DEBUG_ECHOPGM("homeposition");
67 58
       //DEBUG_ECHOLNPAIR_P(SP_X_LBL, homeposition.x, SP_Y_LBL, homeposition.y, SP_Z_LBL, homeposition.z);
59
+    #endif
60
+
61
+    #if ENABLED(MORGAN_SCARA)
62
+      delta = homeposition;
63
+    #else
68 64
       inverse_kinematics(homeposition);
69
-      forward_kinematics_TPARA(delta.a, delta.b, delta.c);
70
-      current_position[axis] = cartes[axis];
71 65
     #endif
72 66
 
67
+    #if EITHER(MORGAN_SCARA, MP_SCARA)
68
+      forward_kinematics(delta.a, delta.b);
69
+    #elif ENABLED(AXEL_TPARA)
70
+      forward_kinematics(delta.a, delta.b, delta.c);
71
+    #endif
72
+
73
+    current_position[axis] = cartes[axis];
74
+
73 75
     //DEBUG_ECHOPGM("Cartesian");
74 76
     //DEBUG_ECHOLNPAIR_P(SP_X_LBL, current_position.x, SP_Y_LBL, current_position.y);
75 77
     update_software_endstops(axis);
@@ -85,7 +87,7 @@ void scara_set_axis_is_at_home(const AxisEnum axis) {
85 87
    * Maths and first version by QHARLEY.
86 88
    * Integrated into Marlin and slightly restructured by Joachim Cerny.
87 89
    */
88
-  void forward_kinematics_SCARA(const float &a, const float &b) {
90
+  void forward_kinematics(const float &a, const float &b) {
89 91
     const float a_sin = sin(RADIANS(a)) * L1,
90 92
                 a_cos = cos(RADIANS(a)) * L1,
91 93
                 b_sin = sin(RADIANS(b + TERN0(MP_SCARA, a))) * L2,
@@ -174,7 +176,7 @@ void scara_set_axis_is_at_home(const AxisEnum axis) {
174 176
   static constexpr xyz_pos_t robot_offset = { TPARA_OFFSET_X, TPARA_OFFSET_Y, TPARA_OFFSET_Z };
175 177
 
176 178
   // Convert ABC inputs in degrees to XYZ outputs in mm
177
-  void forward_kinematics_TPARA(const float &a, const float &b, const float &c) {
179
+  void forward_kinematics(const float &a, const float &b, const float &c) {
178 180
     const float w = c - b,
179 181
                 r = L1 * cos(RADIANS(b)) + L2 * sin(RADIANS(w - (90 - b))),
180 182
                 x = r  * cos(RADIANS(a)),
@@ -227,7 +229,6 @@ void scara_set_axis_is_at_home(const AxisEnum axis) {
227 229
     homeaxis(C_AXIS);
228 230
     homeaxis(B_AXIS);
229 231
 
230
-
231 232
     // Set all carriages to their home positions
232 233
     // Do this here all at once for Delta, because
233 234
     // XYZ isn't ABC. Applying this per-tower would

+ 2
- 2
Marlin/src/module/scara.h 查看文件

@@ -35,7 +35,7 @@ extern float delta_segments_per_second;
35 35
                   L1_2 = sq(float(L1)), L1_2_2 = 2.0 * L1_2,
36 36
                   L2_2 = sq(float(L2));
37 37
 
38
-  void forward_kinematics_TPARA(const float &a, const float &b, const float &c);
38
+  void forward_kinematics(const float &a, const float &b, const float &c);
39 39
   void home_TPARA();
40 40
 
41 41
 #else
@@ -44,7 +44,7 @@ extern float delta_segments_per_second;
44 44
                   L1_2 = sq(float(L1)), L1_2_2 = 2.0 * L1_2,
45 45
                   L2_2 = sq(float(L2));
46 46
 
47
-  void forward_kinematics_SCARA(const float &a, const float &b);
47
+  void forward_kinematics(const float &a, const float &b);
48 48
 
49 49
 #endif
50 50
 

正在加载...
取消
保存