Kaynağa Gözat

Generalize kinematics function names

Scott Lahteine 9 yıl önce
ebeveyn
işleme
d5e2d523c7
4 değiştirilmiş dosya ile 34 ekleme ve 39 silme
  1. 3
    3
      Marlin/Marlin.h
  2. 28
    33
      Marlin/Marlin_main.cpp
  3. 1
    1
      Marlin/planner_bezier.cpp
  4. 2
    2
      Marlin/ultralcd.cpp

+ 3
- 3
Marlin/Marlin.h Dosyayı Görüntüle

315
   extern float delta_diagonal_rod_trim_tower_1;
315
   extern float delta_diagonal_rod_trim_tower_1;
316
   extern float delta_diagonal_rod_trim_tower_2;
316
   extern float delta_diagonal_rod_trim_tower_2;
317
   extern float delta_diagonal_rod_trim_tower_3;
317
   extern float delta_diagonal_rod_trim_tower_3;
318
-  void calculate_delta(float cartesian[3]);
318
+  void inverse_kinematics(float cartesian[3]);
319
   void recalc_delta_settings(float radius, float diagonal_rod);
319
   void recalc_delta_settings(float radius, float diagonal_rod);
320
   #if ENABLED(AUTO_BED_LEVELING_FEATURE)
320
   #if ENABLED(AUTO_BED_LEVELING_FEATURE)
321
     extern int delta_grid_spacing[2];
321
     extern int delta_grid_spacing[2];
323
   #endif
323
   #endif
324
 #elif ENABLED(SCARA)
324
 #elif ENABLED(SCARA)
325
   extern float axis_scaling[3];  // Build size scaling
325
   extern float axis_scaling[3];  // Build size scaling
326
-  void calculate_delta(float cartesian[3]);
327
-  void calculate_SCARA_forward_Transform(float f_scara[3]);
326
+  void inverse_kinematics(float cartesian[3]);
327
+  void forward_kinematics_SCARA(float f_scara[3]);
328
 #endif
328
 #endif
329
 
329
 
330
 #if ENABLED(Z_DUAL_ENDSTOPS)
330
 #if ENABLED(Z_DUAL_ENDSTOPS)

+ 28
- 33
Marlin/Marlin_main.cpp Dosyayı Görüntüle

613
     #if ENABLED(DEBUG_LEVELING_FEATURE)
613
     #if ENABLED(DEBUG_LEVELING_FEATURE)
614
       if (DEBUGGING(LEVELING)) DEBUG_POS("sync_plan_position_delta", current_position);
614
       if (DEBUGGING(LEVELING)) DEBUG_POS("sync_plan_position_delta", current_position);
615
     #endif
615
     #endif
616
-    calculate_delta(current_position);
616
+    inverse_kinematics(current_position);
617
     planner.set_position_mm(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS]);
617
     planner.set_position_mm(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS]);
618
   }
618
   }
619
   #define SYNC_PLAN_POSITION_KINEMATIC() sync_plan_position_delta()
619
   #define SYNC_PLAN_POSITION_KINEMATIC() sync_plan_position_delta()
1528
        * Works out real Homeposition angles using inverse kinematics,
1528
        * Works out real Homeposition angles using inverse kinematics,
1529
        * and calculates homing offset using forward kinematics
1529
        * and calculates homing offset using forward kinematics
1530
        */
1530
        */
1531
-      calculate_delta(homeposition);
1531
+      inverse_kinematics(homeposition);
1532
 
1532
 
1533
       // SERIAL_ECHOPGM("base Theta= "); SERIAL_ECHO(delta[X_AXIS]);
1533
       // SERIAL_ECHOPGM("base Theta= "); SERIAL_ECHO(delta[X_AXIS]);
1534
       // SERIAL_ECHOPGM(" base Psi+Theta="); SERIAL_ECHOLN(delta[Y_AXIS]);
1534
       // SERIAL_ECHOPGM(" base Psi+Theta="); SERIAL_ECHOLN(delta[Y_AXIS]);
1540
       // SERIAL_ECHOPGM(" addhome Theta="); SERIAL_ECHO(delta[X_AXIS]);
1540
       // SERIAL_ECHOPGM(" addhome Theta="); SERIAL_ECHO(delta[X_AXIS]);
1541
       // SERIAL_ECHOPGM(" addhome Psi+Theta="); SERIAL_ECHOLN(delta[Y_AXIS]);
1541
       // SERIAL_ECHOPGM(" addhome Psi+Theta="); SERIAL_ECHOLN(delta[Y_AXIS]);
1542
 
1542
 
1543
-      calculate_SCARA_forward_Transform(delta);
1543
+      forward_kinematics_SCARA(delta);
1544
 
1544
 
1545
       // SERIAL_ECHOPGM("Delta X="); SERIAL_ECHO(delta[X_AXIS]);
1545
       // SERIAL_ECHOPGM("Delta X="); SERIAL_ECHO(delta[X_AXIS]);
1546
       // SERIAL_ECHOPGM(" Delta Y="); SERIAL_ECHOLN(delta[Y_AXIS]);
1546
       // SERIAL_ECHOPGM(" Delta Y="); SERIAL_ECHOLN(delta[Y_AXIS]);
1658
       if (DEBUGGING(LEVELING)) DEBUG_POS("prepare_move_to_destination_raw", destination);
1658
       if (DEBUGGING(LEVELING)) DEBUG_POS("prepare_move_to_destination_raw", destination);
1659
     #endif
1659
     #endif
1660
     refresh_cmd_timeout();
1660
     refresh_cmd_timeout();
1661
-    calculate_delta(destination);
1661
+    inverse_kinematics(destination);
1662
     planner.buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], destination[E_AXIS], MMM_TO_MMS_SCALED(feedrate_mm_m), active_extruder);
1662
     planner.buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], destination[E_AXIS], MMM_TO_MMS_SCALED(feedrate_mm_m), active_extruder);
1663
     set_current_to_destination();
1663
     set_current_to_destination();
1664
   }
1664
   }
5886
       //gcode_get_destination(); // For X Y Z E F
5886
       //gcode_get_destination(); // For X Y Z E F
5887
       delta[X_AXIS] = delta_x;
5887
       delta[X_AXIS] = delta_x;
5888
       delta[Y_AXIS] = delta_y;
5888
       delta[Y_AXIS] = delta_y;
5889
-      calculate_SCARA_forward_Transform(delta);
5889
+      forward_kinematics_SCARA(delta);
5890
       destination[X_AXIS] = delta[X_AXIS] / axis_scaling[X_AXIS];
5890
       destination[X_AXIS] = delta[X_AXIS] / axis_scaling[X_AXIS];
5891
       destination[Y_AXIS] = delta[Y_AXIS] / axis_scaling[Y_AXIS];
5891
       destination[Y_AXIS] = delta[Y_AXIS] / axis_scaling[Y_AXIS];
5892
       prepare_move_to_destination();
5892
       prepare_move_to_destination();
6275
 
6275
 
6276
     // Define runplan for move axes
6276
     // Define runplan for move axes
6277
     #if ENABLED(DELTA)
6277
     #if ENABLED(DELTA)
6278
-      #define RUNPLAN(RATE_MM_S) calculate_delta(destination); \
6278
+      #define RUNPLAN(RATE_MM_S) inverse_kinematics(destination); \
6279
                                  planner.buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], destination[E_AXIS], RATE_MM_S, active_extruder);
6279
                                  planner.buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], destination[E_AXIS], RATE_MM_S, active_extruder);
6280
     #else
6280
     #else
6281
       #define RUNPLAN(RATE_MM_S) line_to_destination(MMS_TO_MMM(RATE_MM_S));
6281
       #define RUNPLAN(RATE_MM_S) line_to_destination(MMS_TO_MMM(RATE_MM_S));
6397
 
6397
 
6398
     #if ENABLED(DELTA)
6398
     #if ENABLED(DELTA)
6399
       // Move XYZ to starting position, then E
6399
       // Move XYZ to starting position, then E
6400
-      calculate_delta(lastpos);
6400
+      inverse_kinematics(lastpos);
6401
       planner.buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], destination[E_AXIS], FILAMENT_CHANGE_XY_FEEDRATE, active_extruder);
6401
       planner.buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], destination[E_AXIS], FILAMENT_CHANGE_XY_FEEDRATE, active_extruder);
6402
       planner.buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], lastpos[E_AXIS], FILAMENT_CHANGE_XY_FEEDRATE, active_extruder);
6402
       planner.buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], lastpos[E_AXIS], FILAMENT_CHANGE_XY_FEEDRATE, active_extruder);
6403
     #else
6403
     #else
7737
     delta_diagonal_rod_2_tower_3 = sq(diagonal_rod + delta_diagonal_rod_trim_tower_3);
7737
     delta_diagonal_rod_2_tower_3 = sq(diagonal_rod + delta_diagonal_rod_trim_tower_3);
7738
   }
7738
   }
7739
 
7739
 
7740
-  void calculate_delta(float cartesian[3]) {
7740
+  void inverse_kinematics(float cartesian[3]) {
7741
 
7741
 
7742
     delta[TOWER_1] = sqrt(delta_diagonal_rod_2_tower_1
7742
     delta[TOWER_1] = sqrt(delta_diagonal_rod_2_tower_1
7743
                           - sq(delta_tower1_x - cartesian[X_AXIS])
7743
                           - sq(delta_tower1_x - cartesian[X_AXIS])
7764
 
7764
 
7765
   float delta_safe_distance_from_top() {
7765
   float delta_safe_distance_from_top() {
7766
     float cartesian[3] = { 0 };
7766
     float cartesian[3] = { 0 };
7767
-    calculate_delta(cartesian);
7767
+    inverse_kinematics(cartesian);
7768
     float distance = delta[TOWER_3];
7768
     float distance = delta[TOWER_3];
7769
     cartesian[Y_AXIS] = DELTA_PRINTABLE_RADIUS;
7769
     cartesian[Y_AXIS] = DELTA_PRINTABLE_RADIUS;
7770
-    calculate_delta(cartesian);
7770
+    inverse_kinematics(cartesian);
7771
     return abs(distance - delta[TOWER_3]);
7771
     return abs(distance - delta[TOWER_3]);
7772
   }
7772
   }
7773
 
7773
 
7774
-  void forwardKinematics(float z1, float z2, float z3) {
7774
+  void forward_kinematics_DELTA(float z1, float z2, float z3) {
7775
     //As discussed in Wikipedia "Trilateration"
7775
     //As discussed in Wikipedia "Trilateration"
7776
     //we are establishing a new coordinate
7776
     //we are establishing a new coordinate
7777
     //system in the plane of the three carriage points.
7777
     //system in the plane of the three carriage points.
7840
     cartesian_position[Z_AXIS] = z1             + ex[2]*Xnew + ey[2]*Ynew - ez[2]*Znew;
7840
     cartesian_position[Z_AXIS] = z1             + ex[2]*Xnew + ey[2]*Ynew - ez[2]*Znew;
7841
   };
7841
   };
7842
 
7842
 
7843
-  void forwardKinematics(float point[3]) {
7844
-    forwardKinematics(point[X_AXIS], point[Y_AXIS], point[Z_AXIS]);
7843
+  void forward_kinematics_DELTA(float point[3]) {
7844
+    forward_kinematics_DELTA(point[X_AXIS], point[Y_AXIS], point[Z_AXIS]);
7845
   }
7845
   }
7846
 
7846
 
7847
   void set_cartesian_from_steppers() {
7847
   void set_cartesian_from_steppers() {
7848
-    forwardKinematics(stepper.get_axis_position_mm(X_AXIS),
7848
+    forward_kinematics_DELTA(stepper.get_axis_position_mm(X_AXIS),
7849
                       stepper.get_axis_position_mm(Y_AXIS),
7849
                       stepper.get_axis_position_mm(Y_AXIS),
7850
                       stepper.get_axis_position_mm(Z_AXIS));
7850
                       stepper.get_axis_position_mm(Z_AXIS));
7851
   }
7851
   }
7973
 
7973
 
7974
 #if ENABLED(DELTA) || ENABLED(SCARA)
7974
 #if ENABLED(DELTA) || ENABLED(SCARA)
7975
 
7975
 
7976
-  inline bool prepare_delta_move_to(float target[NUM_AXIS]) {
7976
+  inline bool prepare_kinematic_move_to(float target[NUM_AXIS]) {
7977
     float difference[NUM_AXIS];
7977
     float difference[NUM_AXIS];
7978
     for (int8_t i = 0; i < NUM_AXIS; i++) difference[i] = target[i] - current_position[i];
7978
     for (int8_t i = 0; i < NUM_AXIS; i++) difference[i] = target[i] - current_position[i];
7979
 
7979
 
7996
       for (int8_t i = 0; i < NUM_AXIS; i++)
7996
       for (int8_t i = 0; i < NUM_AXIS; i++)
7997
         target[i] = current_position[i] + difference[i] * fraction;
7997
         target[i] = current_position[i] + difference[i] * fraction;
7998
 
7998
 
7999
-      calculate_delta(target);
7999
+      inverse_kinematics(target);
8000
 
8000
 
8001
       #if ENABLED(AUTO_BED_LEVELING_FEATURE)
8001
       #if ENABLED(AUTO_BED_LEVELING_FEATURE)
8002
         if (!bed_leveling_in_progress) adjust_delta(target);
8002
         if (!bed_leveling_in_progress) adjust_delta(target);
8003
       #endif
8003
       #endif
8004
 
8004
 
8005
-      //DEBUG_POS("prepare_delta_move_to", target);
8006
-      //DEBUG_POS("prepare_delta_move_to", delta);
8005
+      //DEBUG_POS("prepare_kinematic_move_to", target);
8006
+      //DEBUG_POS("prepare_kinematic_move_to", delta);
8007
 
8007
 
8008
       planner.buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], target[E_AXIS], _feedrate_mm_s, active_extruder);
8008
       planner.buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], target[E_AXIS], _feedrate_mm_s, active_extruder);
8009
     }
8009
     }
8012
 
8012
 
8013
 #endif // DELTA || SCARA
8013
 #endif // DELTA || SCARA
8014
 
8014
 
8015
-#if ENABLED(SCARA)
8016
-  inline bool prepare_scara_move_to(float target[NUM_AXIS]) { return prepare_delta_move_to(target); }
8017
-#endif
8018
-
8019
 #if ENABLED(DUAL_X_CARRIAGE)
8015
 #if ENABLED(DUAL_X_CARRIAGE)
8020
 
8016
 
8021
   inline bool prepare_move_to_destination_dualx() {
8017
   inline bool prepare_move_to_destination_dualx() {
8114
     prevent_dangerous_extrude(current_position[E_AXIS], destination[E_AXIS]);
8110
     prevent_dangerous_extrude(current_position[E_AXIS], destination[E_AXIS]);
8115
   #endif
8111
   #endif
8116
 
8112
 
8117
-  #if ENABLED(SCARA)
8118
-    if (!prepare_scara_move_to(destination)) return;
8119
-  #elif ENABLED(DELTA)
8120
-    if (!prepare_delta_move_to(destination)) return;
8113
+  #if ENABLED(DELTA) || ENABLED(SCARA)
8114
+    if (!prepare_kinematic_move_to(destination)) return;
8121
   #else
8115
   #else
8122
     #if ENABLED(DUAL_X_CARRIAGE)
8116
     #if ENABLED(DUAL_X_CARRIAGE)
8123
       if (!prepare_move_to_destination_dualx()) return;
8117
       if (!prepare_move_to_destination_dualx()) return;
8253
       clamp_to_software_endstops(arc_target);
8247
       clamp_to_software_endstops(arc_target);
8254
 
8248
 
8255
       #if ENABLED(DELTA) || ENABLED(SCARA)
8249
       #if ENABLED(DELTA) || ENABLED(SCARA)
8256
-        calculate_delta(arc_target);
8250
+        inverse_kinematics(arc_target);
8257
         #if ENABLED(AUTO_BED_LEVELING_FEATURE)
8251
         #if ENABLED(AUTO_BED_LEVELING_FEATURE)
8258
           adjust_delta(arc_target);
8252
           adjust_delta(arc_target);
8259
         #endif
8253
         #endif
8265
 
8259
 
8266
     // Ensure last segment arrives at target location.
8260
     // Ensure last segment arrives at target location.
8267
     #if ENABLED(DELTA) || ENABLED(SCARA)
8261
     #if ENABLED(DELTA) || ENABLED(SCARA)
8268
-      calculate_delta(target);
8262
+      inverse_kinematics(target);
8269
       #if ENABLED(AUTO_BED_LEVELING_FEATURE)
8263
       #if ENABLED(AUTO_BED_LEVELING_FEATURE)
8270
         adjust_delta(target);
8264
         adjust_delta(target);
8271
       #endif
8265
       #endif
8333
 
8327
 
8334
 #if ENABLED(SCARA)
8328
 #if ENABLED(SCARA)
8335
 
8329
 
8336
-  void calculate_SCARA_forward_Transform(float f_scara[3]) {
8330
+  void forward_kinematics_SCARA(float f_scara[3]) {
8337
     // Perform forward kinematics, and place results in delta[3]
8331
     // Perform forward kinematics, and place results in delta[3]
8338
     // The maths and first version has been done by QHARLEY . Integrated into masterbranch 06/2014 and slightly restructured by Joachim Cerny in June 2014
8332
     // The maths and first version has been done by QHARLEY . Integrated into masterbranch 06/2014 and slightly restructured by Joachim Cerny in June 2014
8339
 
8333
 
8359
     //SERIAL_ECHOPGM(" delta[Y_AXIS]="); SERIAL_ECHOLN(delta[Y_AXIS]);
8353
     //SERIAL_ECHOPGM(" delta[Y_AXIS]="); SERIAL_ECHOLN(delta[Y_AXIS]);
8360
   }
8354
   }
8361
 
8355
 
8362
-  void calculate_delta(float cartesian[3]) {
8363
-    //reverse kinematics.
8364
-    // Perform reversed kinematics, and place results in delta[3]
8365
-    // The maths and first version has been done by QHARLEY . Integrated into masterbranch 06/2014 and slightly restructured by Joachim Cerny in June 2014
8356
+  void inverse_kinematics(float cartesian[3]) {
8357
+    // Inverse kinematics.
8358
+    // Perform SCARA IK and place results in delta[3].
8359
+    // The maths and first version were done by QHARLEY.
8360
+    // Integrated, tweaked by Joachim Cerny in June 2014.
8366
 
8361
 
8367
     float SCARA_pos[2];
8362
     float SCARA_pos[2];
8368
     static float SCARA_C2, SCARA_S2, SCARA_K1, SCARA_K2, SCARA_theta, SCARA_psi;
8363
     static float SCARA_C2, SCARA_S2, SCARA_K1, SCARA_K2, SCARA_theta, SCARA_psi;

+ 1
- 1
Marlin/planner_bezier.cpp Dosyayı Görüntüle

189
     clamp_to_software_endstops(bez_target);
189
     clamp_to_software_endstops(bez_target);
190
 
190
 
191
     #if ENABLED(DELTA) || ENABLED(SCARA)
191
     #if ENABLED(DELTA) || ENABLED(SCARA)
192
-      calculate_delta(bez_target);
192
+      inverse_kinematics(bez_target);
193
       #if ENABLED(AUTO_BED_LEVELING_FEATURE)
193
       #if ENABLED(AUTO_BED_LEVELING_FEATURE)
194
         adjust_delta(bez_target);
194
         adjust_delta(bez_target);
195
       #endif
195
       #endif

+ 2
- 2
Marlin/ultralcd.cpp Dosyayı Görüntüle

564
 
564
 
565
   inline void line_to_current(AxisEnum axis) {
565
   inline void line_to_current(AxisEnum axis) {
566
     #if ENABLED(DELTA)
566
     #if ENABLED(DELTA)
567
-      calculate_delta(current_position);
567
+      inverse_kinematics(current_position);
568
       planner.buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS], MMM_TO_MMS(manual_feedrate_mm_m[axis]), active_extruder);
568
       planner.buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS], MMM_TO_MMS(manual_feedrate_mm_m[axis]), active_extruder);
569
     #else // !DELTA
569
     #else // !DELTA
570
       planner.buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], MMM_TO_MMS(manual_feedrate_mm_m[axis]), active_extruder);
570
       planner.buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], MMM_TO_MMS(manual_feedrate_mm_m[axis]), active_extruder);
1301
   inline void manage_manual_move() {
1301
   inline void manage_manual_move() {
1302
     if (manual_move_axis != (int8_t)NO_AXIS && ELAPSED(millis(), manual_move_start_time) && !planner.is_full()) {
1302
     if (manual_move_axis != (int8_t)NO_AXIS && ELAPSED(millis(), manual_move_start_time) && !planner.is_full()) {
1303
       #if ENABLED(DELTA)
1303
       #if ENABLED(DELTA)
1304
-        calculate_delta(current_position);
1304
+        inverse_kinematics(current_position);
1305
         planner.buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS], MMM_TO_MMS(manual_feedrate_mm_m[manual_move_axis]), manual_move_e_index);
1305
         planner.buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS], MMM_TO_MMS(manual_feedrate_mm_m[manual_move_axis]), manual_move_e_index);
1306
       #else
1306
       #else
1307
         planner.buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], MMM_TO_MMS(manual_feedrate_mm_m[manual_move_axis]), manual_move_e_index);
1307
         planner.buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], MMM_TO_MMS(manual_feedrate_mm_m[manual_move_axis]), manual_move_e_index);

Loading…
İptal
Kaydet