Browse Source

Merge pull request #4370 from thinkyhead/rc_delta_fwd_kinematics

Delta Forward Kinematics (and LOGICAL_POSITION)
Scott Lahteine 9 years ago
parent
commit
6da3729531
5 changed files with 189 additions and 102 deletions
  1. 3
    2
      .travis.yml
  2. 3
    4
      Marlin/Marlin.h
  3. 179
    92
      Marlin/Marlin_main.cpp
  4. 2
    2
      Marlin/planner_bezier.cpp
  5. 2
    2
      Marlin/ultralcd.cpp

+ 3
- 2
.travis.yml View File

@@ -99,9 +99,9 @@ script:
99 99
   - opt_enable FIX_MOUNTED_PROBE Z_SAFE_HOMING
100 100
   - build_marlin
101 101
   #
102
-  # ...with AUTO_BED_LEVELING_FEATURE & DEBUG_LEVELING_FEATURE
102
+  # ...with AUTO_BED_LEVELING_FEATURE, Z_MIN_PROBE_REPEATABILITY_TEST, & DEBUG_LEVELING_FEATURE
103 103
   #
104
-  - opt_enable AUTO_BED_LEVELING_FEATURE DEBUG_LEVELING_FEATURE
104
+  - opt_enable AUTO_BED_LEVELING_FEATURE Z_MIN_PROBE_REPEATABILITY_TEST DEBUG_LEVELING_FEATURE
105 105
   - build_marlin
106 106
   #
107 107
   # Test a Sled Z Probe
@@ -365,6 +365,7 @@ script:
365 365
   # SCARA Config
366 366
   #
367 367
   - use_example_configs SCARA
368
+  - opt_enable AUTO_BED_LEVELING_FEATURE FIX_MOUNTED_PROBE USE_ZMIN_PLUG
368 369
   - build_marlin
369 370
   #
370 371
   # tvrrug Config need to check board type for sanguino atmega644p

+ 3
- 4
Marlin/Marlin.h View File

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

+ 179
- 92
Marlin/Marlin_main.cpp View File

@@ -331,15 +331,13 @@ float position_shift[3] = { 0 };
331 331
 // Set by M206, M428, or menu item. Saved to EEPROM.
332 332
 float home_offset[3] = { 0 };
333 333
 
334
+#define LOGICAL_POSITION(POS, AXIS) (POS + home_offset[AXIS] + position_shift[AXIS])
334 335
 #define RAW_POSITION(POS, AXIS) (POS - home_offset[AXIS] - position_shift[AXIS])
335 336
 #define RAW_CURRENT_POSITION(AXIS) (RAW_POSITION(current_position[AXIS], AXIS))
336 337
 
337 338
 // Software Endstops. Default to configured limits.
338 339
 float sw_endstop_min[3] = { X_MIN_POS, Y_MIN_POS, Z_MIN_POS };
339 340
 float sw_endstop_max[3] = { X_MAX_POS, Y_MAX_POS, Z_MAX_POS };
340
-#if ENABLED(DELTA)
341
-  float delta_clip_start_height = Z_MAX_POS;
342
-#endif
343 341
 
344 342
 #if FAN_COUNT > 0
345 343
   int fanSpeeds[FAN_COUNT] = { 0 };
@@ -463,6 +461,7 @@ static uint8_t target_extruder;
463 461
   #define TOWER_3 Z_AXIS
464 462
 
465 463
   float delta[3] = { 0 };
464
+  float cartesian_position[3] = { 0 };
466 465
   #define SIN_60 0.8660254037844386
467 466
   #define COS_60 0.5
468 467
   float endstop_adj[3] = { 0 };
@@ -481,12 +480,13 @@ static uint8_t target_extruder;
481 480
   float delta_diagonal_rod_2_tower_1 = sq(delta_diagonal_rod + delta_diagonal_rod_trim_tower_1);
482 481
   float delta_diagonal_rod_2_tower_2 = sq(delta_diagonal_rod + delta_diagonal_rod_trim_tower_2);
483 482
   float delta_diagonal_rod_2_tower_3 = sq(delta_diagonal_rod + delta_diagonal_rod_trim_tower_3);
484
-  //float delta_diagonal_rod_2 = sq(delta_diagonal_rod);
485 483
   float delta_segments_per_second = DELTA_SEGMENTS_PER_SECOND;
484
+  float delta_clip_start_height = Z_MAX_POS;
486 485
   #if ENABLED(AUTO_BED_LEVELING_FEATURE)
487 486
     int delta_grid_spacing[2] = { 0, 0 };
488 487
     float bed_level[AUTO_BED_LEVELING_GRID_POINTS][AUTO_BED_LEVELING_GRID_POINTS];
489 488
   #endif
489
+  float delta_safe_distance_from_top();
490 490
 #else
491 491
   static bool home_all_axis = true;
492 492
 #endif
@@ -564,6 +564,7 @@ void stop();
564 564
 void get_available_commands();
565 565
 void process_next_command();
566 566
 void prepare_move_to_destination();
567
+void set_current_from_steppers();
567 568
 
568 569
 #if ENABLED(ARC_SUPPORT)
569 570
   void plan_arc(float target[NUM_AXIS], float* offset, uint8_t clockwise);
@@ -614,7 +615,7 @@ static void report_current_position();
614 615
     #if ENABLED(DEBUG_LEVELING_FEATURE)
615 616
       if (DEBUGGING(LEVELING)) DEBUG_POS("sync_plan_position_delta", current_position);
616 617
     #endif
617
-    calculate_delta(current_position);
618
+    inverse_kinematics(current_position);
618 619
     planner.set_position_mm(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS]);
619 620
   }
620 621
   #define SYNC_PLAN_POSITION_KINEMATIC() sync_plan_position_delta()
@@ -1403,7 +1404,7 @@ XYZ_CONSTS_FROM_CONFIG(signed char, home_dir, HOME_DIR);
1403 1404
 
1404 1405
   static float x_home_pos(int extruder) {
1405 1406
     if (extruder == 0)
1406
-      return base_home_pos(X_AXIS) + home_offset[X_AXIS];
1407
+      return LOGICAL_POSITION(base_home_pos(X_AXIS), X_AXIS);
1407 1408
     else
1408 1409
       /**
1409 1410
        * In dual carriage mode the extruder offset provides an override of the
@@ -1438,7 +1439,7 @@ XYZ_CONSTS_FROM_CONFIG(signed char, home_dir, HOME_DIR);
1438 1439
  * at the same positions relative to the machine.
1439 1440
  */
1440 1441
 static void update_software_endstops(AxisEnum axis) {
1441
-  float offs = home_offset[axis] + position_shift[axis];
1442
+  float offs = LOGICAL_POSITION(0, axis);
1442 1443
 
1443 1444
   #if ENABLED(DUAL_X_CARRIAGE)
1444 1445
     if (axis == X_AXIS) {
@@ -1509,7 +1510,7 @@ static void set_axis_is_at_home(AxisEnum axis) {
1509 1510
       if (active_extruder != 0)
1510 1511
         current_position[X_AXIS] = x_home_pos(active_extruder);
1511 1512
       else
1512
-        current_position[X_AXIS] = base_home_pos(X_AXIS) + home_offset[X_AXIS];
1513
+        current_position[X_AXIS] = LOGICAL_POSITION(base_home_pos(X_AXIS), X_AXIS);
1513 1514
       update_software_endstops(X_AXIS);
1514 1515
       return;
1515 1516
     }
@@ -1520,7 +1521,8 @@ static void set_axis_is_at_home(AxisEnum axis) {
1520 1521
     if (axis == X_AXIS || axis == Y_AXIS) {
1521 1522
 
1522 1523
       float homeposition[3];
1523
-      for (int i = 0; i < 3; i++) homeposition[i] = base_home_pos(i);
1524
+      for (uint8_t i = X_AXIS; i <= Z_AXIS; i++)
1525
+        homeposition[i] = LOGICAL_POSITION(base_home_pos(i), i);
1524 1526
 
1525 1527
       // SERIAL_ECHOPGM("homeposition[x]= "); SERIAL_ECHO(homeposition[0]);
1526 1528
       // SERIAL_ECHOPGM("homeposition[y]= "); SERIAL_ECHOLN(homeposition[1]);
@@ -1529,24 +1531,13 @@ static void set_axis_is_at_home(AxisEnum axis) {
1529 1531
        * Works out real Homeposition angles using inverse kinematics,
1530 1532
        * and calculates homing offset using forward kinematics
1531 1533
        */
1532
-      calculate_delta(homeposition);
1533
-
1534
-      // SERIAL_ECHOPGM("base Theta= "); SERIAL_ECHO(delta[X_AXIS]);
1535
-      // SERIAL_ECHOPGM(" base Psi+Theta="); SERIAL_ECHOLN(delta[Y_AXIS]);
1536
-
1537
-      for (int i = 0; i < 2; i++) delta[i] -= home_offset[i];
1538
-
1539
-      // SERIAL_ECHOPGM("addhome X="); SERIAL_ECHO(home_offset[X_AXIS]);
1540
-      // SERIAL_ECHOPGM(" addhome Y="); SERIAL_ECHO(home_offset[Y_AXIS]);
1541
-      // SERIAL_ECHOPGM(" addhome Theta="); SERIAL_ECHO(delta[X_AXIS]);
1542
-      // SERIAL_ECHOPGM(" addhome Psi+Theta="); SERIAL_ECHOLN(delta[Y_AXIS]);
1543
-
1544
-      calculate_SCARA_forward_Transform(delta);
1534
+      inverse_kinematics(homeposition);
1535
+      forward_kinematics_SCARA(delta);
1545 1536
 
1546
-      // SERIAL_ECHOPGM("Delta X="); SERIAL_ECHO(delta[X_AXIS]);
1537
+      // SERIAL_ECHOPAIR("Delta X=", delta[X_AXIS]);
1547 1538
       // SERIAL_ECHOPGM(" Delta Y="); SERIAL_ECHOLN(delta[Y_AXIS]);
1548 1539
 
1549
-      current_position[axis] = delta[axis];
1540
+      current_position[axis] = LOGICAL_POSITION(delta[axis], axis);
1550 1541
 
1551 1542
       /**
1552 1543
        * SCARA home positions are based on configuration since the actual
@@ -1558,7 +1549,7 @@ static void set_axis_is_at_home(AxisEnum axis) {
1558 1549
     else
1559 1550
   #endif
1560 1551
   {
1561
-    current_position[axis] = base_home_pos(axis) + home_offset[axis];
1552
+    current_position[axis] = LOGICAL_POSITION(base_home_pos(axis), axis);
1562 1553
     update_software_endstops(axis);
1563 1554
 
1564 1555
     #if HAS_BED_PROBE && Z_HOME_DIR < 0 && DISABLED(Z_MIN_PROBE_ENDSTOP)
@@ -1659,7 +1650,7 @@ inline void set_destination_to_current() { memcpy(destination, current_position,
1659 1650
       if (DEBUGGING(LEVELING)) DEBUG_POS("prepare_move_to_destination_raw", destination);
1660 1651
     #endif
1661 1652
     refresh_cmd_timeout();
1662
-    calculate_delta(destination);
1653
+    inverse_kinematics(destination);
1663 1654
     planner.buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], destination[E_AXIS], MMM_TO_MMS_SCALED(feedrate_mm_m), active_extruder);
1664 1655
     set_current_to_destination();
1665 1656
   }
@@ -1787,7 +1778,7 @@ static void clean_up_after_endstop_or_probe_move() {
1787 1778
         SERIAL_ECHOLNPGM(")");
1788 1779
       }
1789 1780
     #endif
1790
-    float z_dest = home_offset[Z_AXIS] + z_raise;
1781
+    float z_dest = LOGICAL_POSITION(z_raise, Z_AXIS);
1791 1782
 
1792 1783
     if (zprobe_zoffset < 0)
1793 1784
       z_dest -= zprobe_zoffset;
@@ -2088,9 +2079,9 @@ static void clean_up_after_endstop_or_probe_move() {
2088 2079
   }
2089 2080
 
2090 2081
   #if ENABLED(DELTA)
2091
-    #define Z_FROM_STEPPERS() z_before + stepper.get_axis_position_mm(Z_AXIS) - z_mm
2082
+    #define SET_Z_FROM_STEPPERS() set_current_from_steppers()
2092 2083
   #else
2093
-    #define Z_FROM_STEPPERS() stepper.get_axis_position_mm(Z_AXIS)
2084
+    #define SET_Z_FROM_STEPPERS() current_position[Z_AXIS] = LOGICAL_POSITION(stepper.get_axis_position_mm(Z_AXIS), Z_AXIS)
2094 2085
   #endif
2095 2086
 
2096 2087
   // Do a single Z probe and return with current_position[Z_AXIS]
@@ -2111,7 +2102,7 @@ static void clean_up_after_endstop_or_probe_move() {
2111 2102
 
2112 2103
     do_blocking_move_to_z(-(Z_MAX_LENGTH + 10), Z_PROBE_SPEED_FAST);
2113 2104
     endstops.hit_on_purpose();
2114
-    current_position[Z_AXIS] = Z_FROM_STEPPERS();
2105
+    SET_Z_FROM_STEPPERS();
2115 2106
     SYNC_PLAN_POSITION_KINEMATIC();
2116 2107
 
2117 2108
     // move up the retract distance
@@ -2125,7 +2116,7 @@ static void clean_up_after_endstop_or_probe_move() {
2125 2116
     // move back down slowly to find bed
2126 2117
     do_blocking_move_to_z(current_position[Z_AXIS] - home_bump_mm(Z_AXIS) * 2, Z_PROBE_SPEED_SLOW);
2127 2118
     endstops.hit_on_purpose();
2128
-    current_position[Z_AXIS] = Z_FROM_STEPPERS();
2119
+    SET_Z_FROM_STEPPERS();
2129 2120
     SYNC_PLAN_POSITION_KINEMATIC();
2130 2121
 
2131 2122
     #if ENABLED(DEBUG_LEVELING_FEATURE)
@@ -2959,7 +2950,7 @@ inline void gcode_G28() {
2959 2950
 
2960 2951
       if (home_all_axis || homeX || homeY) {
2961 2952
         // Raise Z before homing any other axes and z is not already high enough (never lower z)
2962
-        destination[Z_AXIS] = home_offset[Z_AXIS] + MIN_Z_HEIGHT_FOR_HOMING;
2953
+        destination[Z_AXIS] = LOGICAL_POSITION(MIN_Z_HEIGHT_FOR_HOMING, Z_AXIS);
2963 2954
         if (destination[Z_AXIS] > current_position[Z_AXIS]) {
2964 2955
 
2965 2956
           #if ENABLED(DEBUG_LEVELING_FEATURE)
@@ -3214,12 +3205,12 @@ inline void gcode_G28() {
3214 3205
     ;
3215 3206
     line_to_current_position();
3216 3207
 
3217
-    current_position[X_AXIS] = x + home_offset[X_AXIS];
3218
-    current_position[Y_AXIS] = y + home_offset[Y_AXIS];
3208
+    current_position[X_AXIS] = LOGICAL_POSITION(x, X_AXIS);
3209
+    current_position[Y_AXIS] = LOGICAL_POSITION(y, Y_AXIS);
3219 3210
     line_to_current_position();
3220 3211
 
3221 3212
     #if Z_RAISE_BETWEEN_PROBINGS > 0 || MIN_Z_HEIGHT_FOR_HOMING > 0
3222
-      current_position[Z_AXIS] = MESH_HOME_SEARCH_Z;
3213
+      current_position[Z_AXIS] = LOGICAL_POSITION(MESH_HOME_SEARCH_Z, Z_AXIS);
3223 3214
       line_to_current_position();
3224 3215
     #endif
3225 3216
 
@@ -3637,14 +3628,14 @@ inline void gcode_G28() {
3637 3628
       #endif
3638 3629
 
3639 3630
       // Probe at 3 arbitrary points
3640
-      float z_at_pt_1 = probe_pt( ABL_PROBE_PT_1_X + home_offset[X_AXIS],
3641
-                                  ABL_PROBE_PT_1_Y + home_offset[Y_AXIS],
3631
+      float z_at_pt_1 = probe_pt( LOGICAL_POSITION(ABL_PROBE_PT_1_X, X_AXIS),
3632
+                                  LOGICAL_POSITION(ABL_PROBE_PT_1_Y, Y_AXIS),
3642 3633
                                   stow_probe_after_each, verbose_level),
3643
-            z_at_pt_2 = probe_pt( ABL_PROBE_PT_2_X + home_offset[X_AXIS],
3644
-                                  ABL_PROBE_PT_2_Y + home_offset[Y_AXIS],
3634
+            z_at_pt_2 = probe_pt( LOGICAL_POSITION(ABL_PROBE_PT_2_X, X_AXIS),
3635
+                                  LOGICAL_POSITION(ABL_PROBE_PT_2_Y, Y_AXIS),
3645 3636
                                   stow_probe_after_each, verbose_level),
3646
-            z_at_pt_3 = probe_pt( ABL_PROBE_PT_3_X + home_offset[X_AXIS],
3647
-                                  ABL_PROBE_PT_3_Y + home_offset[Y_AXIS],
3637
+            z_at_pt_3 = probe_pt( LOGICAL_POSITION(ABL_PROBE_PT_3_X, X_AXIS),
3638
+                                  LOGICAL_POSITION(ABL_PROBE_PT_3_Y, Y_AXIS),
3648 3639
                                   stow_probe_after_each, verbose_level);
3649 3640
 
3650 3641
       if (!dryrun) set_bed_level_equation_3pts(z_at_pt_1, z_at_pt_2, z_at_pt_3);
@@ -5168,9 +5159,9 @@ static void report_current_position() {
5168 5159
     SERIAL_EOL;
5169 5160
 
5170 5161
     SERIAL_PROTOCOLPGM("SCARA Cal - Theta:");
5171
-    SERIAL_PROTOCOL(delta[X_AXIS] + home_offset[X_AXIS]);
5162
+    SERIAL_PROTOCOL(delta[X_AXIS]);
5172 5163
     SERIAL_PROTOCOLPGM("   Psi+Theta (90):");
5173
-    SERIAL_PROTOCOL(delta[Y_AXIS] - delta[X_AXIS] - 90 + home_offset[Y_AXIS]);
5164
+    SERIAL_PROTOCOL(delta[Y_AXIS] - delta[X_AXIS] - 90);
5174 5165
     SERIAL_EOL;
5175 5166
 
5176 5167
     SERIAL_PROTOCOLPGM("SCARA step Cal - Theta:");
@@ -5880,7 +5871,7 @@ inline void gcode_M303() {
5880 5871
       //gcode_get_destination(); // For X Y Z E F
5881 5872
       delta[X_AXIS] = delta_x;
5882 5873
       delta[Y_AXIS] = delta_y;
5883
-      calculate_SCARA_forward_Transform(delta);
5874
+      forward_kinematics_SCARA(delta);
5884 5875
       destination[X_AXIS] = delta[X_AXIS] / axis_scaling[X_AXIS];
5885 5876
       destination[Y_AXIS] = delta[Y_AXIS] / axis_scaling[Y_AXIS];
5886 5877
       prepare_move_to_destination();
@@ -6068,18 +6059,9 @@ inline void gcode_M400() { stepper.synchronize(); }
6068 6059
 
6069 6060
 void quickstop_stepper() {
6070 6061
   stepper.quick_stop();
6071
-  #if DISABLED(DELTA) && DISABLED(SCARA)
6062
+  #if DISABLED(SCARA)
6072 6063
     stepper.synchronize();
6073
-    #if ENABLED(AUTO_BED_LEVELING_FEATURE)
6074
-      vector_3 pos = planner.adjusted_position(); // values directly from steppers...
6075
-      current_position[X_AXIS] = pos.x;
6076
-      current_position[Y_AXIS] = pos.y;
6077
-      current_position[Z_AXIS] = pos.z;
6078
-    #else
6079
-      current_position[X_AXIS] = stepper.get_axis_position_mm(X_AXIS);
6080
-      current_position[Y_AXIS] = stepper.get_axis_position_mm(Y_AXIS);
6081
-      current_position[Z_AXIS] = stepper.get_axis_position_mm(Z_AXIS);
6082
-    #endif
6064
+    set_current_from_steppers();
6083 6065
     sync_plan_position();                       // ...re-apply to planner position
6084 6066
   #endif
6085 6067
 }
@@ -6146,7 +6128,7 @@ inline void gcode_M428() {
6146 6128
   for (int8_t i = X_AXIS; i <= Z_AXIS; i++) {
6147 6129
     if (axis_homed[i]) {
6148 6130
       float base = (current_position[i] > (sw_endstop_min[i] + sw_endstop_max[i]) / 2) ? base_home_pos(i) : 0,
6149
-            diff = current_position[i] - base;
6131
+            diff = current_position[i] - LOGICAL_POSITION(base, i);
6150 6132
       if (diff > -20 && diff < 20) {
6151 6133
         set_home_offset((AxisEnum)i, home_offset[i] - diff);
6152 6134
       }
@@ -6278,7 +6260,7 @@ inline void gcode_M503() {
6278 6260
 
6279 6261
     // Define runplan for move axes
6280 6262
     #if ENABLED(DELTA)
6281
-      #define RUNPLAN(RATE_MM_S) calculate_delta(destination); \
6263
+      #define RUNPLAN(RATE_MM_S) inverse_kinematics(destination); \
6282 6264
                                  planner.buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], destination[E_AXIS], RATE_MM_S, active_extruder);
6283 6265
     #else
6284 6266
       #define RUNPLAN(RATE_MM_S) line_to_destination(MMS_TO_MMM(RATE_MM_S));
@@ -6400,7 +6382,7 @@ inline void gcode_M503() {
6400 6382
 
6401 6383
     #if ENABLED(DELTA)
6402 6384
       // Move XYZ to starting position, then E
6403
-      calculate_delta(lastpos);
6385
+      inverse_kinematics(lastpos);
6404 6386
       planner.buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], destination[E_AXIS], FILAMENT_CHANGE_XY_FEEDRATE, active_extruder);
6405 6387
       planner.buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], lastpos[E_AXIS], FILAMENT_CHANGE_XY_FEEDRATE, active_extruder);
6406 6388
     #else
@@ -7740,7 +7722,13 @@ void clamp_to_software_endstops(float target[3]) {
7740 7722
     delta_diagonal_rod_2_tower_3 = sq(diagonal_rod + delta_diagonal_rod_trim_tower_3);
7741 7723
   }
7742 7724
 
7743
-  void calculate_delta(float cartesian[3]) {
7725
+  void inverse_kinematics(const float in_cartesian[3]) {
7726
+
7727
+    const float cartesian[3] = {
7728
+      RAW_POSITION(in_cartesian[X_AXIS], X_AXIS),
7729
+      RAW_POSITION(in_cartesian[Y_AXIS], Y_AXIS),
7730
+      RAW_POSITION(in_cartesian[Z_AXIS], Z_AXIS)
7731
+    };
7744 7732
 
7745 7733
     delta[TOWER_1] = sqrt(delta_diagonal_rod_2_tower_1
7746 7734
                           - sq(delta_tower1_x - cartesian[X_AXIS])
@@ -7766,14 +7754,97 @@ void clamp_to_software_endstops(float target[3]) {
7766 7754
   }
7767 7755
 
7768 7756
   float delta_safe_distance_from_top() {
7769
-    float cartesian[3] = { 0 };
7770
-    calculate_delta(cartesian);
7757
+    float cartesian[3] = {
7758
+      LOGICAL_POSITION(0, X_AXIS),
7759
+      LOGICAL_POSITION(0, Y_AXIS),
7760
+      LOGICAL_POSITION(0, Z_AXIS)
7761
+    };
7762
+    inverse_kinematics(cartesian);
7771 7763
     float distance = delta[TOWER_3];
7772
-    cartesian[Y_AXIS] = DELTA_PRINTABLE_RADIUS;
7773
-    calculate_delta(cartesian);
7764
+    cartesian[Y_AXIS] = LOGICAL_POSITION(DELTA_PRINTABLE_RADIUS, Y_AXIS);
7765
+    inverse_kinematics(cartesian);
7774 7766
     return abs(distance - delta[TOWER_3]);
7775 7767
   }
7776 7768
 
7769
+  void forward_kinematics_DELTA(float z1, float z2, float z3) {
7770
+    //As discussed in Wikipedia "Trilateration"
7771
+    //we are establishing a new coordinate
7772
+    //system in the plane of the three carriage points.
7773
+    //This system will have the origin at tower1 and
7774
+    //tower2 is on the x axis. tower3 is in the X-Y
7775
+    //plane with a Z component of zero. We will define unit
7776
+    //vectors in this coordinate system in our original
7777
+    //coordinate system. Then when we calculate the
7778
+    //Xnew, Ynew and Znew values, we can translate back into
7779
+    //the original system by moving along those unit vectors
7780
+    //by the corresponding values.
7781
+    // https://en.wikipedia.org/wiki/Trilateration
7782
+
7783
+    // Variable names matched to Marlin, c-version
7784
+    // and avoiding a vector library
7785
+    // by Andreas Hardtung 2016-06-7
7786
+    // based on a Java function from
7787
+    // "Delta Robot Kinematics by Steve Graves" V3
7788
+
7789
+    // Result is in cartesian_position[].
7790
+
7791
+    //Create a vector in old coordinates along x axis of new coordinate
7792
+    float p12[3] = { delta_tower2_x - delta_tower1_x, delta_tower2_y - delta_tower1_y, z2 - z1 };
7793
+
7794
+    //Get the Magnitude of vector.
7795
+    float d = sqrt( p12[0]*p12[0] + p12[1]*p12[1] + p12[2]*p12[2] );
7796
+
7797
+    //Create unit vector by dividing by magnitude.
7798
+    float ex[3] = { p12[0]/d, p12[1]/d, p12[2]/d };
7799
+
7800
+    //Now find vector from the origin of the new system to the third point.
7801
+    float p13[3] = { delta_tower3_x - delta_tower1_x, delta_tower3_y - delta_tower1_y, z3 - z1 };
7802
+
7803
+    //Now use dot product to find the component of this vector on the X axis.
7804
+    float i = ex[0]*p13[0] + ex[1]*p13[1] + ex[2]*p13[2];
7805
+
7806
+    //Now create a vector along the x axis that represents the x component of p13.
7807
+    float iex[3] = { ex[0]*i,  ex[1]*i,  ex[2]*i  };
7808
+
7809
+    //Now subtract the X component away from the original vector leaving only the Y component. We use the
7810
+    //variable that will be the unit vector after we scale it.
7811
+    float ey[3] = { p13[0] - iex[0], p13[1] - iex[1], p13[2] - iex[2]};
7812
+
7813
+    //The magnitude of Y component
7814
+    float j = sqrt(sq(ey[0]) + sq(ey[1]) + sq(ey[2]));
7815
+
7816
+    //Now make vector a unit vector
7817
+    ey[0] /= j; ey[1] /= j;  ey[2] /= j;
7818
+
7819
+    //The cross product of the unit x and y is the unit z
7820
+    //float[] ez = vectorCrossProd(ex, ey);
7821
+    float ez[3] = { ex[1]*ey[2] - ex[2]*ey[1], ex[2]*ey[0] - ex[0]*ey[2], ex[0]*ey[1] - ex[1]*ey[0] };
7822
+
7823
+    //Now we have the d, i and j values defined in Wikipedia.
7824
+    //We can plug them into the equations defined in
7825
+    //Wikipedia for Xnew, Ynew and Znew
7826
+    float Xnew = (delta_diagonal_rod_2_tower_1 - delta_diagonal_rod_2_tower_2 + d*d)/(d*2);
7827
+    float Ynew = ((delta_diagonal_rod_2_tower_1 - delta_diagonal_rod_2_tower_3 + i*i + j*j)/2 - i*Xnew) /j;
7828
+    float Znew = sqrt(delta_diagonal_rod_2_tower_1 - Xnew*Xnew - Ynew*Ynew);
7829
+
7830
+    //Now we can start from the origin in the old coords and
7831
+    //add vectors in the old coords that represent the
7832
+    //Xnew, Ynew and Znew to find the point in the old system
7833
+    cartesian_position[X_AXIS] = delta_tower1_x + ex[0]*Xnew + ey[0]*Ynew - ez[0]*Znew;
7834
+    cartesian_position[Y_AXIS] = delta_tower1_y + ex[1]*Xnew + ey[1]*Ynew - ez[1]*Znew;
7835
+    cartesian_position[Z_AXIS] = z1             + ex[2]*Xnew + ey[2]*Ynew - ez[2]*Znew;
7836
+  };
7837
+
7838
+  void forward_kinematics_DELTA(float point[3]) {
7839
+    forward_kinematics_DELTA(point[X_AXIS], point[Y_AXIS], point[Z_AXIS]);
7840
+  }
7841
+
7842
+  void set_cartesian_from_steppers() {
7843
+    forward_kinematics_DELTA(stepper.get_axis_position_mm(X_AXIS),
7844
+                             stepper.get_axis_position_mm(Y_AXIS),
7845
+                             stepper.get_axis_position_mm(Z_AXIS));
7846
+  }
7847
+
7777 7848
   #if ENABLED(AUTO_BED_LEVELING_FEATURE)
7778 7849
 
7779 7850
     // Adjust print surface height by linear interpolation over the bed_level array.
@@ -7782,8 +7853,8 @@ void clamp_to_software_endstops(float target[3]) {
7782 7853
 
7783 7854
       int half = (AUTO_BED_LEVELING_GRID_POINTS - 1) / 2;
7784 7855
       float h1 = 0.001 - half, h2 = half - 0.001,
7785
-            grid_x = max(h1, min(h2, cartesian[X_AXIS] / delta_grid_spacing[0])),
7786
-            grid_y = max(h1, min(h2, cartesian[Y_AXIS] / delta_grid_spacing[1]));
7856
+            grid_x = max(h1, min(h2, RAW_POSITION(cartesian[X_AXIS], X_AXIS) / delta_grid_spacing[0])),
7857
+            grid_y = max(h1, min(h2, RAW_POSITION(cartesian[Y_AXIS], Y_AXIS) / delta_grid_spacing[1]));
7787 7858
       int floor_x = floor(grid_x), floor_y = floor(grid_y);
7788 7859
       float ratio_x = grid_x - floor_x, ratio_y = grid_y - floor_y,
7789 7860
             z1 = bed_level[floor_x + half][floor_y + half],
@@ -7818,6 +7889,27 @@ void clamp_to_software_endstops(float target[3]) {
7818 7889
 
7819 7890
 #endif // DELTA
7820 7891
 
7892
+void set_current_from_steppers() {
7893
+  #if ENABLED(DELTA)
7894
+    set_cartesian_from_steppers();
7895
+    current_position[X_AXIS] = cartesian_position[X_AXIS];
7896
+    current_position[Y_AXIS] = cartesian_position[Y_AXIS];
7897
+    current_position[Z_AXIS] = cartesian_position[Z_AXIS];
7898
+  #elif ENABLED(AUTO_BED_LEVELING_FEATURE)
7899
+    vector_3 pos = planner.adjusted_position(); // values directly from steppers...
7900
+    current_position[X_AXIS] = pos.x;
7901
+    current_position[Y_AXIS] = pos.y;
7902
+    current_position[Z_AXIS] = pos.z;
7903
+  #else
7904
+    current_position[X_AXIS] = stepper.get_axis_position_mm(X_AXIS); // CORE handled transparently
7905
+    current_position[Y_AXIS] = stepper.get_axis_position_mm(Y_AXIS);
7906
+    current_position[Z_AXIS] = stepper.get_axis_position_mm(Z_AXIS);
7907
+  #endif
7908
+
7909
+  for (uint8_t i = X_AXIS; i <= Z_AXIS; i++)
7910
+    current_position[i] += LOGICAL_POSITION(0, i);
7911
+}
7912
+
7821 7913
 #if ENABLED(MESH_BED_LEVELING)
7822 7914
 
7823 7915
 // This function is used to split lines on mesh borders so each segment is only part of one mesh area
@@ -7846,14 +7938,14 @@ void mesh_line_to_destination(float fr_mm_m, uint8_t x_splits = 0xff, uint8_t y_
7846 7938
   int8_t gcx = max(cx1, cx2), gcy = max(cy1, cy2);
7847 7939
   if (cx2 != cx1 && TEST(x_splits, gcx)) {
7848 7940
     memcpy(end, destination, sizeof(end));
7849
-    destination[X_AXIS] = mbl.get_probe_x(gcx) + home_offset[X_AXIS] + position_shift[X_AXIS];
7941
+    destination[X_AXIS] = LOGICAL_POSITION(mbl.get_probe_x(gcx), X_AXIS);
7850 7942
     normalized_dist = (destination[X_AXIS] - current_position[X_AXIS]) / (end[X_AXIS] - current_position[X_AXIS]);
7851 7943
     destination[Y_AXIS] = MBL_SEGMENT_END(Y);
7852 7944
     CBI(x_splits, gcx);
7853 7945
   }
7854 7946
   else if (cy2 != cy1 && TEST(y_splits, gcy)) {
7855 7947
     memcpy(end, destination, sizeof(end));
7856
-    destination[Y_AXIS] = mbl.get_probe_y(gcy) + home_offset[Y_AXIS] + position_shift[Y_AXIS];
7948
+    destination[Y_AXIS] = LOGICAL_POSITION(mbl.get_probe_y(gcy), Y_AXIS);
7857 7949
     normalized_dist = (destination[Y_AXIS] - current_position[Y_AXIS]) / (end[Y_AXIS] - current_position[Y_AXIS]);
7858 7950
     destination[X_AXIS] = MBL_SEGMENT_END(X);
7859 7951
     CBI(y_splits, gcy);
@@ -7879,7 +7971,7 @@ void mesh_line_to_destination(float fr_mm_m, uint8_t x_splits = 0xff, uint8_t y_
7879 7971
 
7880 7972
 #if ENABLED(DELTA) || ENABLED(SCARA)
7881 7973
 
7882
-  inline bool prepare_delta_move_to(float target[NUM_AXIS]) {
7974
+  inline bool prepare_kinematic_move_to(float target[NUM_AXIS]) {
7883 7975
     float difference[NUM_AXIS];
7884 7976
     for (int8_t i = 0; i < NUM_AXIS; i++) difference[i] = target[i] - current_position[i];
7885 7977
 
@@ -7902,14 +7994,14 @@ void mesh_line_to_destination(float fr_mm_m, uint8_t x_splits = 0xff, uint8_t y_
7902 7994
       for (int8_t i = 0; i < NUM_AXIS; i++)
7903 7995
         target[i] = current_position[i] + difference[i] * fraction;
7904 7996
 
7905
-      calculate_delta(target);
7997
+      inverse_kinematics(target);
7906 7998
 
7907
-      #if ENABLED(AUTO_BED_LEVELING_FEATURE)
7999
+      #if ENABLED(DELTA) && ENABLED(AUTO_BED_LEVELING_FEATURE)
7908 8000
         if (!bed_leveling_in_progress) adjust_delta(target);
7909 8001
       #endif
7910 8002
 
7911
-      //DEBUG_POS("prepare_delta_move_to", target);
7912
-      //DEBUG_POS("prepare_delta_move_to", delta);
8003
+      //DEBUG_POS("prepare_kinematic_move_to", target);
8004
+      //DEBUG_POS("prepare_kinematic_move_to", delta);
7913 8005
 
7914 8006
       planner.buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], target[E_AXIS], _feedrate_mm_s, active_extruder);
7915 8007
     }
@@ -7918,10 +8010,6 @@ void mesh_line_to_destination(float fr_mm_m, uint8_t x_splits = 0xff, uint8_t y_
7918 8010
 
7919 8011
 #endif // DELTA || SCARA
7920 8012
 
7921
-#if ENABLED(SCARA)
7922
-  inline bool prepare_scara_move_to(float target[NUM_AXIS]) { return prepare_delta_move_to(target); }
7923
-#endif
7924
-
7925 8013
 #if ENABLED(DUAL_X_CARRIAGE)
7926 8014
 
7927 8015
   inline bool prepare_move_to_destination_dualx() {
@@ -8020,10 +8108,8 @@ void prepare_move_to_destination() {
8020 8108
     prevent_dangerous_extrude(current_position[E_AXIS], destination[E_AXIS]);
8021 8109
   #endif
8022 8110
 
8023
-  #if ENABLED(SCARA)
8024
-    if (!prepare_scara_move_to(destination)) return;
8025
-  #elif ENABLED(DELTA)
8026
-    if (!prepare_delta_move_to(destination)) return;
8111
+  #if ENABLED(DELTA) || ENABLED(SCARA)
8112
+    if (!prepare_kinematic_move_to(destination)) return;
8027 8113
   #else
8028 8114
     #if ENABLED(DUAL_X_CARRIAGE)
8029 8115
       if (!prepare_move_to_destination_dualx()) return;
@@ -8159,8 +8245,8 @@ void prepare_move_to_destination() {
8159 8245
       clamp_to_software_endstops(arc_target);
8160 8246
 
8161 8247
       #if ENABLED(DELTA) || ENABLED(SCARA)
8162
-        calculate_delta(arc_target);
8163
-        #if ENABLED(AUTO_BED_LEVELING_FEATURE)
8248
+        inverse_kinematics(arc_target);
8249
+        #if ENABLED(DELTA) && ENABLED(AUTO_BED_LEVELING_FEATURE)
8164 8250
           adjust_delta(arc_target);
8165 8251
         #endif
8166 8252
         planner.buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], arc_target[E_AXIS], fr_mm_s, active_extruder);
@@ -8171,8 +8257,8 @@ void prepare_move_to_destination() {
8171 8257
 
8172 8258
     // Ensure last segment arrives at target location.
8173 8259
     #if ENABLED(DELTA) || ENABLED(SCARA)
8174
-      calculate_delta(target);
8175
-      #if ENABLED(AUTO_BED_LEVELING_FEATURE)
8260
+      inverse_kinematics(target);
8261
+      #if ENABLED(DELTA) && ENABLED(AUTO_BED_LEVELING_FEATURE)
8176 8262
         adjust_delta(target);
8177 8263
       #endif
8178 8264
       planner.buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], target[E_AXIS], fr_mm_s, active_extruder);
@@ -8239,7 +8325,7 @@ void prepare_move_to_destination() {
8239 8325
 
8240 8326
 #if ENABLED(SCARA)
8241 8327
 
8242
-  void calculate_SCARA_forward_Transform(float f_scara[3]) {
8328
+  void forward_kinematics_SCARA(float f_scara[3]) {
8243 8329
     // Perform forward kinematics, and place results in delta[3]
8244 8330
     // The maths and first version has been done by QHARLEY . Integrated into masterbranch 06/2014 and slightly restructured by Joachim Cerny in June 2014
8245 8331
 
@@ -8265,16 +8351,17 @@ void prepare_move_to_destination() {
8265 8351
     //SERIAL_ECHOPGM(" delta[Y_AXIS]="); SERIAL_ECHOLN(delta[Y_AXIS]);
8266 8352
   }
8267 8353
 
8268
-  void calculate_delta(float cartesian[3]) {
8269
-    //reverse kinematics.
8270
-    // Perform reversed kinematics, and place results in delta[3]
8271
-    // The maths and first version has been done by QHARLEY . Integrated into masterbranch 06/2014 and slightly restructured by Joachim Cerny in June 2014
8354
+  void inverse_kinematics(const float cartesian[3]) {
8355
+    // Inverse kinematics.
8356
+    // Perform SCARA IK and place results in delta[3].
8357
+    // The maths and first version were done by QHARLEY.
8358
+    // Integrated, tweaked by Joachim Cerny in June 2014.
8272 8359
 
8273 8360
     float SCARA_pos[2];
8274 8361
     static float SCARA_C2, SCARA_S2, SCARA_K1, SCARA_K2, SCARA_theta, SCARA_psi;
8275 8362
 
8276
-    SCARA_pos[X_AXIS] = cartesian[X_AXIS] * axis_scaling[X_AXIS] - SCARA_offset_x;  //Translate SCARA to standard X Y
8277
-    SCARA_pos[Y_AXIS] = cartesian[Y_AXIS] * axis_scaling[Y_AXIS] - SCARA_offset_y;  // With scaling factor.
8363
+    SCARA_pos[X_AXIS] = RAW_POSITION(cartesian[X_AXIS], X_AXIS) * axis_scaling[X_AXIS] - SCARA_offset_x;  //Translate SCARA to standard X Y
8364
+    SCARA_pos[Y_AXIS] = RAW_POSITION(cartesian[Y_AXIS], Y_AXIS) * axis_scaling[Y_AXIS] - SCARA_offset_y;  // With scaling factor.
8278 8365
 
8279 8366
     #if (Linkage_1 == Linkage_2)
8280 8367
       SCARA_C2 = ((sq(SCARA_pos[X_AXIS]) + sq(SCARA_pos[Y_AXIS])) / (2 * (float)L1_2)) - 1;
@@ -8292,7 +8379,7 @@ void prepare_move_to_destination() {
8292 8379
 
8293 8380
     delta[X_AXIS] = SCARA_theta * SCARA_RAD2DEG;  // Multiply by 180/Pi  -  theta is support arm angle
8294 8381
     delta[Y_AXIS] = (SCARA_theta + SCARA_psi) * SCARA_RAD2DEG;  //       -  equal to sub arm angle (inverted motor)
8295
-    delta[Z_AXIS] = cartesian[Z_AXIS];
8382
+    delta[Z_AXIS] = RAW_POSITION(cartesian[Z_AXIS], Z_AXIS);
8296 8383
 
8297 8384
     /**
8298 8385
     SERIAL_ECHOPGM("cartesian x="); SERIAL_ECHO(cartesian[X_AXIS]);

+ 2
- 2
Marlin/planner_bezier.cpp View File

@@ -189,8 +189,8 @@ void cubic_b_spline(const float position[NUM_AXIS], const float target[NUM_AXIS]
189 189
     clamp_to_software_endstops(bez_target);
190 190
 
191 191
     #if ENABLED(DELTA) || ENABLED(SCARA)
192
-      calculate_delta(bez_target);
193
-      #if ENABLED(AUTO_BED_LEVELING_FEATURE)
192
+      inverse_kinematics(bez_target);
193
+      #if ENABLED(DELTA) && ENABLED(AUTO_BED_LEVELING_FEATURE)
194 194
         adjust_delta(bez_target);
195 195
       #endif
196 196
       planner.buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], bez_target[E_AXIS], fr_mm_s, extruder);

+ 2
- 2
Marlin/ultralcd.cpp View File

@@ -564,7 +564,7 @@ void kill_screen(const char* lcd_msg) {
564 564
 
565 565
   inline void line_to_current(AxisEnum axis) {
566 566
     #if ENABLED(DELTA)
567
-      calculate_delta(current_position);
567
+      inverse_kinematics(current_position);
568 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 569
     #else // !DELTA
570 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,7 +1301,7 @@ void kill_screen(const char* lcd_msg) {
1301 1301
   inline void manage_manual_move() {
1302 1302
     if (manual_move_axis != (int8_t)NO_AXIS && ELAPSED(millis(), manual_move_start_time) && !planner.is_full()) {
1303 1303
       #if ENABLED(DELTA)
1304
-        calculate_delta(current_position);
1304
+        inverse_kinematics(current_position);
1305 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 1306
       #else
1307 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…
Cancel
Save