Browse Source

Followup to MP_SCARA/TPARA patches (#21248)

Tanguy Pruvot 4 years ago
parent
commit
a0d312396a
No account linked to committer's email address

+ 1
- 1
Marlin/src/feature/bedlevel/ubl/ubl_motion.cpp View File

@@ -335,7 +335,7 @@
335 335
 
336 336
     #if IS_KINEMATIC
337 337
       const float seconds = cart_xy_mm / scaled_fr_mm_s;                             // Duration of XY move at requested rate
338
-      uint16_t segments = LROUND(delta_segments_per_second * seconds),               // Preferred number of segments for distance @ feedrate
338
+      uint16_t segments = LROUND(segments_per_second * seconds),                     // Preferred number of segments for distance @ feedrate
339 339
                seglimit = LROUND(cart_xy_mm * RECIPROCAL(DELTA_SEGMENT_MIN_LENGTH)); // Number of segments at minimum segment length
340 340
       NOMORE(segments, seglimit);                                                    // Limit to minimum segment length (fewer segments)
341 341
     #else

+ 2
- 2
Marlin/src/gcode/calibrate/M665.cpp View File

@@ -48,7 +48,7 @@
48 48
     if (parser.seenval('H')) delta_height              = parser.value_linear_units();
49 49
     if (parser.seenval('L')) delta_diagonal_rod        = parser.value_linear_units();
50 50
     if (parser.seenval('R')) delta_radius              = parser.value_linear_units();
51
-    if (parser.seenval('S')) delta_segments_per_second = parser.value_float();
51
+    if (parser.seenval('S')) segments_per_second       = parser.value_float();
52 52
     if (parser.seenval('X')) delta_tower_angle_trim.a  = parser.value_float();
53 53
     if (parser.seenval('Y')) delta_tower_angle_trim.b  = parser.value_float();
54 54
     if (parser.seenval('Z')) delta_tower_angle_trim.c  = parser.value_float();
@@ -76,7 +76,7 @@
76 76
    *   B, T, and Y are all aliases for the elbow angle
77 77
    */
78 78
   void GcodeSuite::M665() {
79
-    if (parser.seenval('S')) delta_segments_per_second = parser.value_float();
79
+    if (parser.seenval('S')) segments_per_second = parser.value_float();
80 80
 
81 81
     #if HAS_SCARA_OFFSET
82 82
 

+ 1
- 1
Marlin/src/module/delta.cpp View File

@@ -54,7 +54,7 @@ float delta_height;
54 54
 abc_float_t delta_endstop_adj{0};
55 55
 float delta_radius,
56 56
       delta_diagonal_rod,
57
-      delta_segments_per_second;
57
+      segments_per_second;
58 58
 abc_float_t delta_tower_angle_trim;
59 59
 xy_float_t delta_tower[ABC];
60 60
 abc_float_t delta_diagonal_rod_2_tower;

+ 1
- 1
Marlin/src/module/delta.h View File

@@ -32,7 +32,7 @@ extern float delta_height;
32 32
 extern abc_float_t delta_endstop_adj;
33 33
 extern float delta_radius,
34 34
              delta_diagonal_rod,
35
-             delta_segments_per_second;
35
+             segments_per_second;
36 36
 extern abc_float_t delta_tower_angle_trim;
37 37
 extern xy_float_t delta_tower[ABC];
38 38
 extern abc_float_t delta_diagonal_rod_2_tower;

+ 1
- 1
Marlin/src/module/motion.cpp View File

@@ -763,7 +763,7 @@ FORCE_INLINE void segment_idle(millis_t &next_idle_ms) {
763 763
 
764 764
     // The number of segments-per-second times the duration
765 765
     // gives the number of segments
766
-    uint16_t segments = delta_segments_per_second * seconds;
766
+    uint16_t segments = segments_per_second * seconds;
767 767
 
768 768
     // For SCARA enforce a minimum segment size
769 769
     #if IS_SCARA

+ 61
- 40
Marlin/src/module/scara.cpp View File

@@ -37,46 +37,7 @@
37 37
   #include "../MarlinCore.h"
38 38
 #endif
39 39
 
40
-float delta_segments_per_second = TERN(AXEL_TPARA, TPARA_SEGMENTS_PER_SECOND, SCARA_SEGMENTS_PER_SECOND);
41
-
42
-void scara_set_axis_is_at_home(const AxisEnum axis) {
43
-  if (axis == Z_AXIS)
44
-    current_position.z = Z_HOME_POS;
45
-  else {
46
-    #if ENABLED(MORGAN_SCARA)
47
-      // MORGAN_SCARA uses arm angles for AB home position
48
-      ab_float_t homeposition = { SCARA_OFFSET_THETA1, SCARA_OFFSET_THETA2 };
49
-      //DEBUG_ECHOLNPAIR("homeposition A:", homeposition.a, " B:", homeposition.b);
50
-    #elif ENABLED(MP_SCARA)
51
-      // MP_SCARA uses a Cartesian XY home position
52
-      xyz_pos_t homeposition = { X_HOME_POS, Y_HOME_POS, Z_HOME_POS };
53
-      //DEBUG_ECHOPGM("homeposition");
54
-      //DEBUG_ECHOLNPAIR_P(SP_X_LBL, homeposition.x, SP_Y_LBL, homeposition.y);
55
-    #elif ENABLED(AXEL_TPARA)
56
-      xyz_pos_t homeposition = { X_HOME_POS, Y_HOME_POS, Z_HOME_POS };
57
-      //DEBUG_ECHOPGM("homeposition");
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
64
-      inverse_kinematics(homeposition);
65
-    #endif
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
-
75
-    //DEBUG_ECHOPGM("Cartesian");
76
-    //DEBUG_ECHOLNPAIR_P(SP_X_LBL, current_position.x, SP_Y_LBL, current_position.y);
77
-    update_software_endstops(axis);
78
-  }
79
-}
40
+float segments_per_second = TERN(AXEL_TPARA, TPARA_SEGMENTS_PER_SECOND, SCARA_SEGMENTS_PER_SECOND);
80 41
 
81 42
 #if EITHER(MORGAN_SCARA, MP_SCARA)
82 43
 
@@ -109,6 +70,27 @@ void scara_set_axis_is_at_home(const AxisEnum axis) {
109 70
     //*/
110 71
   }
111 72
 
73
+#endif
74
+
75
+#if ENABLED(MORGAN_SCARA)
76
+
77
+  void scara_set_axis_is_at_home(const AxisEnum axis) {
78
+    if (axis == Z_AXIS)
79
+      current_position.z = Z_HOME_POS;
80
+    else {
81
+      // MORGAN_SCARA uses a Cartesian XY home position
82
+      xyz_pos_t homeposition = { X_HOME_POS, Y_HOME_POS, Z_HOME_POS };
83
+      //DEBUG_ECHOLNPAIR_P(PSTR("homeposition X"), homeposition.x, SP_Y_LBL, homeposition.y);
84
+
85
+      delta = homeposition;
86
+      forward_kinematics(delta.a, delta.b);
87
+      current_position[axis] = cartes[axis];
88
+
89
+      //DEBUG_ECHOLNPAIR_P(PSTR("Cartesian X"), current_position.x, SP_Y_LBL, current_position.y);
90
+      update_software_endstops(axis);
91
+    }
92
+  }
93
+
112 94
   /**
113 95
    * Morgan SCARA Inverse Kinematics. Results are stored in 'delta'.
114 96
    *
@@ -156,6 +138,29 @@ void scara_set_axis_is_at_home(const AxisEnum axis) {
156 138
 
157 139
 #elif ENABLED(MP_SCARA)
158 140
 
141
+  void scara_set_axis_is_at_home(const AxisEnum axis) {
142
+    if (axis == Z_AXIS)
143
+      current_position.z = Z_HOME_POS;
144
+    else {
145
+      // MP_SCARA uses arm angles for AB home position
146
+      #ifndef SCARA_OFFSET_THETA1
147
+        #define SCARA_OFFSET_THETA1  12 // degrees
148
+      #endif
149
+      #ifndef SCARA_OFFSET_THETA2
150
+        #define SCARA_OFFSET_THETA2 131 // degrees
151
+      #endif
152
+      ab_float_t homeposition = { SCARA_OFFSET_THETA1, SCARA_OFFSET_THETA2 };
153
+      //DEBUG_ECHOLNPAIR("homeposition A:", homeposition.a, " B:", homeposition.b);
154
+
155
+      inverse_kinematics(homeposition);
156
+      forward_kinematics(delta.a, delta.b);
157
+      current_position[axis] = cartes[axis];
158
+
159
+      //DEBUG_ECHOLNPAIR_P(PSTR("Cartesian X"), current_position.x, SP_Y_LBL, current_position.y);
160
+      update_software_endstops(axis);
161
+    }
162
+  }
163
+
159 164
   void inverse_kinematics(const xyz_pos_t &raw) {
160 165
     const float x = raw.x, y = raw.y, c = HYPOT(x, y),
161 166
                 THETA3 = ATAN2(y, x),
@@ -175,6 +180,22 @@ void scara_set_axis_is_at_home(const AxisEnum axis) {
175 180
 
176 181
   static constexpr xyz_pos_t robot_offset = { TPARA_OFFSET_X, TPARA_OFFSET_Y, TPARA_OFFSET_Z };
177 182
 
183
+  void scara_set_axis_is_at_home(const AxisEnum axis) {
184
+    if (axis == Z_AXIS)
185
+      current_position.z = Z_HOME_POS;
186
+    else {
187
+      xyz_pos_t homeposition = { X_HOME_POS, Y_HOME_POS, Z_HOME_POS };
188
+      //DEBUG_ECHOLNPAIR_P(PSTR("homeposition X"), homeposition.x, SP_Y_LBL, homeposition.y, SP_Z_LBL, homeposition.z);
189
+
190
+      inverse_kinematics(homeposition);
191
+      forward_kinematics(delta.a, delta.b, delta.c);
192
+      current_position[axis] = cartes[axis];
193
+
194
+      //DEBUG_ECHOLNPAIR_P(PSTR("Cartesian X"), current_position.x, SP_Y_LBL, current_position.y);
195
+      update_software_endstops(axis);
196
+    }
197
+  }
198
+
178 199
   // Convert ABC inputs in degrees to XYZ outputs in mm
179 200
   void forward_kinematics(const float &a, const float &b, const float &c) {
180 201
     const float w = c - b,

+ 1
- 1
Marlin/src/module/scara.h View File

@@ -27,7 +27,7 @@
27 27
 
28 28
 #include "../core/macros.h"
29 29
 
30
-extern float delta_segments_per_second;
30
+extern float segments_per_second;
31 31
 
32 32
 #if ENABLED(AXEL_TPARA)
33 33
 

+ 6
- 6
Marlin/src/module/settings.cpp View File

@@ -279,7 +279,7 @@ typedef struct SettingsDataStruct {
279 279
     abc_float_t delta_endstop_adj;                      // M666 X Y Z
280 280
     float delta_radius,                                 // M665 R
281 281
           delta_diagonal_rod,                           // M665 L
282
-          delta_segments_per_second;                    // M665 S
282
+          segments_per_second;                          // M665 S
283 283
     abc_float_t delta_tower_angle_trim,                 // M665 X Y Z
284 284
                 delta_diagonal_rod_trim;                // M665 A B C
285 285
   #elif HAS_EXTRA_ENDSTOPS
@@ -840,7 +840,7 @@ void MarlinSettings::postprocess() {
840 840
         EEPROM_WRITE(delta_endstop_adj);         // 3 floats
841 841
         EEPROM_WRITE(delta_radius);              // 1 float
842 842
         EEPROM_WRITE(delta_diagonal_rod);        // 1 float
843
-        EEPROM_WRITE(delta_segments_per_second); // 1 float
843
+        EEPROM_WRITE(segments_per_second);       // 1 float
844 844
         EEPROM_WRITE(delta_tower_angle_trim);    // 3 floats
845 845
         EEPROM_WRITE(delta_diagonal_rod_trim);   // 3 floats
846 846
 
@@ -1721,7 +1721,7 @@ void MarlinSettings::postprocess() {
1721 1721
           EEPROM_READ(delta_endstop_adj);         // 3 floats
1722 1722
           EEPROM_READ(delta_radius);              // 1 float
1723 1723
           EEPROM_READ(delta_diagonal_rod);        // 1 float
1724
-          EEPROM_READ(delta_segments_per_second); // 1 float
1724
+          EEPROM_READ(segments_per_second);       // 1 float
1725 1725
           EEPROM_READ(delta_tower_angle_trim);    // 3 floats
1726 1726
           EEPROM_READ(delta_diagonal_rod_trim);   // 3 floats
1727 1727
 
@@ -2711,7 +2711,7 @@ void MarlinSettings::reset() {
2711 2711
     delta_endstop_adj = adj;
2712 2712
     delta_radius = DELTA_RADIUS;
2713 2713
     delta_diagonal_rod = DELTA_DIAGONAL_ROD;
2714
-    delta_segments_per_second = DELTA_SEGMENTS_PER_SECOND;
2714
+    segments_per_second = DELTA_SEGMENTS_PER_SECOND;
2715 2715
     delta_tower_angle_trim = dta;
2716 2716
     delta_diagonal_rod_trim = ddr;
2717 2717
   #endif
@@ -3320,7 +3320,7 @@ void MarlinSettings::reset() {
3320 3320
       CONFIG_ECHO_HEADING("SCARA settings: S<seg-per-sec> P<theta-psi-offset> T<theta-offset>");
3321 3321
       CONFIG_ECHO_START();
3322 3322
       SERIAL_ECHOLNPAIR_P(
3323
-          PSTR("  M665 S"), delta_segments_per_second
3323
+          PSTR("  M665 S"), segments_per_second
3324 3324
         , SP_P_STR, scara_home_offset.a
3325 3325
         , SP_T_STR, scara_home_offset.b
3326 3326
         , SP_Z_STR, LINEAR_UNIT(scara_home_offset.z)
@@ -3342,7 +3342,7 @@ void MarlinSettings::reset() {
3342 3342
           PSTR("  M665 L"), LINEAR_UNIT(delta_diagonal_rod)
3343 3343
         , PSTR(" R"), LINEAR_UNIT(delta_radius)
3344 3344
         , PSTR(" H"), LINEAR_UNIT(delta_height)
3345
-        , PSTR(" S"), delta_segments_per_second
3345
+        , PSTR(" S"), segments_per_second
3346 3346
         , SP_X_STR, LINEAR_UNIT(delta_tower_angle_trim.a)
3347 3347
         , SP_Y_STR, LINEAR_UNIT(delta_tower_angle_trim.b)
3348 3348
         , SP_Z_STR, LINEAR_UNIT(delta_tower_angle_trim.c)

Loading…
Cancel
Save