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
 
335
 
336
     #if IS_KINEMATIC
336
     #if IS_KINEMATIC
337
       const float seconds = cart_xy_mm / scaled_fr_mm_s;                             // Duration of XY move at requested rate
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
                seglimit = LROUND(cart_xy_mm * RECIPROCAL(DELTA_SEGMENT_MIN_LENGTH)); // Number of segments at minimum segment length
339
                seglimit = LROUND(cart_xy_mm * RECIPROCAL(DELTA_SEGMENT_MIN_LENGTH)); // Number of segments at minimum segment length
340
       NOMORE(segments, seglimit);                                                    // Limit to minimum segment length (fewer segments)
340
       NOMORE(segments, seglimit);                                                    // Limit to minimum segment length (fewer segments)
341
     #else
341
     #else

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

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

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

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

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

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

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

763
 
763
 
764
     // The number of segments-per-second times the duration
764
     // The number of segments-per-second times the duration
765
     // gives the number of segments
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
     // For SCARA enforce a minimum segment size
768
     // For SCARA enforce a minimum segment size
769
     #if IS_SCARA
769
     #if IS_SCARA

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

37
   #include "../MarlinCore.h"
37
   #include "../MarlinCore.h"
38
 #endif
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
 #if EITHER(MORGAN_SCARA, MP_SCARA)
42
 #if EITHER(MORGAN_SCARA, MP_SCARA)
82
 
43
 
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
    * Morgan SCARA Inverse Kinematics. Results are stored in 'delta'.
95
    * Morgan SCARA Inverse Kinematics. Results are stored in 'delta'.
114
    *
96
    *
156
 
138
 
157
 #elif ENABLED(MP_SCARA)
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
   void inverse_kinematics(const xyz_pos_t &raw) {
164
   void inverse_kinematics(const xyz_pos_t &raw) {
160
     const float x = raw.x, y = raw.y, c = HYPOT(x, y),
165
     const float x = raw.x, y = raw.y, c = HYPOT(x, y),
161
                 THETA3 = ATAN2(y, x),
166
                 THETA3 = ATAN2(y, x),
175
 
180
 
176
   static constexpr xyz_pos_t robot_offset = { TPARA_OFFSET_X, TPARA_OFFSET_Y, TPARA_OFFSET_Z };
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
   // Convert ABC inputs in degrees to XYZ outputs in mm
199
   // Convert ABC inputs in degrees to XYZ outputs in mm
179
   void forward_kinematics(const float &a, const float &b, const float &c) {
200
   void forward_kinematics(const float &a, const float &b, const float &c) {
180
     const float w = c - b,
201
     const float w = c - b,

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

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

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

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

Loading…
Cancel
Save