|
@@ -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
|
|
-
|
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
|
+
|
|
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
|
+
|
7820
|
7824
|
}
|
7821
|
7825
|
|
7822
|
7826
|
|
|
@@ -8090,19 +8094,87 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) {
|
8090
|
8094
|
|
8091
|
8095
|
|
8092
|
8096
|
|
8093
|
|
-
|
8094
|
|
- LOOP_XYZE(i) logical[i] = current_position[i];
|
8095
|
|
-
|
8096
|
8097
|
|
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
|
|
-
|
8102
|
|
-
|
|
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
|
+
|
|
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
|
+
|
|
8128
|
+ LOOP_XYZE(i) logical[i] = current_position[i];
|
|
8129
|
+
|
|
8130
|
+ #endif
|
|
8131
|
+
|
|
8132
|
+ #if ENABLED(USE_DELTA_IK_INTERPOLATION)
|
|
8133
|
+
|
|
8134
|
+
|
|
8135
|
+ if (segments >= 2) inverse_kinematics(logical);
|
|
8136
|
+
|
|
8137
|
+ for (uint16_t s = segments + 1; --s;) {
|
|
8138
|
+ if (s > 1) {
|
|
8139
|
+
|
|
8140
|
+ float prev_delta[ABC] = { delta[A_AXIS], delta[B_AXIS], delta[C_AXIS] };
|
|
8141
|
+
|
|
8142
|
+
|
|
8143
|
+ DELTA_NEXT(segment_distance[i] + segment_distance[i]);
|
|
8144
|
+ DELTA_IK();
|
|
8145
|
+
|
|
8146
|
+
|
|
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
|
+
|
|
8155
|
+ --s;
|
|
8156
|
+ }
|
|
8157
|
+ else {
|
|
8158
|
+
|
|
8159
|
+ DELTA_NEXT(segment_distance[i])
|
|
8160
|
+ DELTA_IK();
|
|
8161
|
+ }
|
|
8162
|
+
|
|
8163
|
+
|
|
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
|
+
|
|
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
|
|