Browse Source

Add Delta kinematic optimization options

Scott Lahteine 8 years ago
parent
commit
3913e04ac7
1 changed files with 108 additions and 36 deletions
  1. 108
    36
      Marlin/Marlin_main.cpp

+ 108
- 36
Marlin/Marlin_main.cpp View File

@@ -7789,34 +7789,38 @@ void ok_to_send() {
7789 7789
    * - Use a fast-inverse-sqrt function and add the reciprocal.
7790 7790
    *   (see above)
7791 7791
    */
7792
-  void inverse_kinematics(const float logical[XYZ]) {
7793
-
7794
-    const float cartesian[XYZ] = {
7795
-      RAW_X_POSITION(logical[X_AXIS]),
7796
-      RAW_Y_POSITION(logical[Y_AXIS]),
7797
-      RAW_Z_POSITION(logical[Z_AXIS])
7798
-    };
7799 7792
 
7800
-    // Macro to obtain the Z position of an individual tower
7801
-    #define DELTA_Z(T) cartesian[Z_AXIS] + _SQRT( \
7802
-      delta_diagonal_rod_2_tower_##T - HYPOT2(    \
7803
-          delta_tower##T##_x - cartesian[X_AXIS], \
7804
-          delta_tower##T##_y - cartesian[Y_AXIS]  \
7805
-        )                                         \
7806
-      )
7793
+  // Macro to obtain the Z position of an individual tower
7794
+  #define DELTA_Z(T) raw[Z_AXIS] + _SQRT(    \
7795
+    delta_diagonal_rod_2_tower_##T - HYPOT2( \
7796
+        delta_tower##T##_x - raw[X_AXIS],    \
7797
+        delta_tower##T##_y - raw[Y_AXIS]     \
7798
+      )                                      \
7799
+    )
7800
+
7801
+  #define DELTA_LOGICAL_IK() do {      \
7802
+    const float raw[XYZ] = {           \
7803
+      RAW_X_POSITION(logical[X_AXIS]), \
7804
+      RAW_Y_POSITION(logical[Y_AXIS]), \
7805
+      RAW_Z_POSITION(logical[Z_AXIS])  \
7806
+    };                                 \
7807
+    delta[A_AXIS] = DELTA_Z(1);        \
7808
+    delta[B_AXIS] = DELTA_Z(2);        \
7809
+    delta[C_AXIS] = DELTA_Z(3);        \
7810
+  } while(0)
7811
+
7812
+  #define DELTA_DEBUG() do { \
7813
+      SERIAL_ECHOPAIR("cartesian X:", raw[X_AXIS]); \
7814
+      SERIAL_ECHOPAIR(" Y:", raw[Y_AXIS]);          \
7815
+      SERIAL_ECHOLNPAIR(" Z:", raw[Z_AXIS]);        \
7816
+      SERIAL_ECHOPAIR("delta A:", delta[A_AXIS]);   \
7817
+      SERIAL_ECHOPAIR(" B:", delta[B_AXIS]);        \
7818
+      SERIAL_ECHOLNPAIR(" C:", delta[C_AXIS]);      \
7819
+    } while(0)
7807 7820
 
7808
-    delta[A_AXIS] = DELTA_Z(1);
7809
-    delta[B_AXIS] = DELTA_Z(2);
7810
-    delta[C_AXIS] = DELTA_Z(3);
7811
-
7812
-    /*
7813
-      SERIAL_ECHOPAIR("cartesian X:", cartesian[X_AXIS]);
7814
-      SERIAL_ECHOPAIR(" Y:", cartesian[Y_AXIS]);
7815
-      SERIAL_ECHOLNPAIR(" Z:", cartesian[Z_AXIS]);
7816
-      SERIAL_ECHOPAIR("delta A:", delta[A_AXIS]);
7817
-      SERIAL_ECHOPAIR(" B:", delta[B_AXIS]);
7818
-      SERIAL_ECHOLNPAIR(" C:", delta[C_AXIS]);
7819
-    //*/
7821
+  void inverse_kinematics(const float logical[XYZ]) {
7822
+    DELTA_LOGICAL_IK();
7823
+    // DELTA_DEBUG();
7820 7824
   }
7821 7825
 
7822 7826
   /**
@@ -8090,19 +8094,87 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) {
8090 8094
     // SERIAL_ECHOPAIR(" seconds=", seconds);
8091 8095
     // SERIAL_ECHOLNPAIR(" segments=", segments);
8092 8096
 
8093
-    // Set the target to the current position to start
8094
-    LOOP_XYZE(i) logical[i] = current_position[i];
8095
-
8096 8097
     // Send all the segments to the planner
8097
-    for (uint16_t s = 0; s < segments; s++) {
8098
-      LOOP_XYZE(i) logical[i] += segment_distance[i];
8099
-      inverse_kinematics(logical);
8100 8098
 
8101
-      //DEBUG_POS("prepare_kinematic_move_to", logical);
8102
-      //DEBUG_POS("prepare_kinematic_move_to", delta);
8099
+    #if ENABLED(DELTA) && ENABLED(USE_RAW_KINEMATICS)
8100
+
8101
+      #define DELTA_E raw[E_AXIS]
8102
+      #define DELTA_NEXT(ADDEND) LOOP_XYZE(i) raw[i] += ADDEND;
8103
+      #define DELTA_IK() do {       \
8104
+        delta[A_AXIS] = DELTA_Z(1); \
8105
+        delta[B_AXIS] = DELTA_Z(2); \
8106
+        delta[C_AXIS] = DELTA_Z(3); \
8107
+      } while(0)
8108
+
8109
+      // Get the raw current position as starting point
8110
+      float raw[ABC] = {
8111
+        RAW_CURRENT_POSITION(X_AXIS),
8112
+        RAW_CURRENT_POSITION(Y_AXIS),
8113
+        RAW_CURRENT_POSITION(Z_AXIS)
8114
+      };
8115
+
8116
+    #else
8117
+
8118
+      #define DELTA_E logical[E_AXIS]
8119
+      #define DELTA_NEXT(ADDEND) LOOP_XYZE(i) logical[i] += ADDEND;
8120
+
8121
+      #if ENABLED(DELTA)
8122
+        #define DELTA_IK() DELTA_LOGICAL_IK()
8123
+      #else
8124
+        #define DELTA_IK() inverse_kinematics(logical)
8125
+      #endif
8126
+
8127
+      // Get the logical current position as starting point
8128
+      LOOP_XYZE(i) logical[i] = current_position[i];
8129
+
8130
+    #endif
8131
+
8132
+    #if ENABLED(USE_DELTA_IK_INTERPOLATION)
8133
+
8134
+      // Get the starting delta for interpolation
8135
+      if (segments >= 2) inverse_kinematics(logical);
8136
+
8137
+      for (uint16_t s = segments + 1; --s;) {
8138
+        if (s > 1) {
8139
+          // Save the previous delta for interpolation
8140
+          float prev_delta[ABC] = { delta[A_AXIS], delta[B_AXIS], delta[C_AXIS] };
8141
+
8142
+          // Get the delta 2 segments ahead (rather than the next)
8143
+          DELTA_NEXT(segment_distance[i] + segment_distance[i]);
8144
+          DELTA_IK();
8145
+
8146
+          // Move to the interpolated delta position first
8147
+          planner.buffer_line(
8148
+            (prev_delta[A_AXIS] + delta[A_AXIS]) * 0.5,
8149
+            (prev_delta[B_AXIS] + delta[B_AXIS]) * 0.5,
8150
+            (prev_delta[C_AXIS] + delta[C_AXIS]) * 0.5,
8151
+            logical[E_AXIS], _feedrate_mm_s, active_extruder
8152
+          );
8153
+
8154
+          // Do an extra decrement of the loop
8155
+          --s;
8156
+        }
8157
+        else {
8158
+          // Get the last segment delta (only when segments is odd)
8159
+          DELTA_NEXT(segment_distance[i])
8160
+          DELTA_IK();
8161
+        }
8162
+
8163
+        // Move to the non-interpolated position
8164
+        planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], DELTA_E, _feedrate_mm_s, active_extruder);
8165
+      }
8166
+
8167
+    #else
8168
+
8169
+      // For non-interpolated delta calculate every segment
8170
+      for (uint16_t s = segments + 1; --s;) {
8171
+        DELTA_NEXT(segment_distance[i])
8172
+        DELTA_IK();
8173
+        planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], logical[E_AXIS], _feedrate_mm_s, active_extruder);
8174
+      }
8175
+
8176
+    #endif
8103 8177
 
8104
-      planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], logical[E_AXIS], _feedrate_mm_s, active_extruder);
8105
-    }
8106 8178
     return true;
8107 8179
   }
8108 8180
 

Loading…
Cancel
Save