|
@@ -462,7 +462,7 @@ static uint8_t target_extruder;
|
462
|
462
|
#define TOWER_3 Z_AXIS
|
463
|
463
|
|
464
|
464
|
float delta[3] = { 0 };
|
465
|
|
- float cartesian[3] = { 0 };
|
|
465
|
+ float cartesian_position[3] = { 0 };
|
466
|
466
|
#define SIN_60 0.8660254037844386
|
467
|
467
|
#define COS_60 0.5
|
468
|
468
|
float endstop_adj[3] = { 0 };
|
|
@@ -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);
|
|
@@ -7801,7 +7802,7 @@ void clamp_to_software_endstops(float target[3]) {
|
7801
|
7802
|
// based on a Java function from
|
7802
|
7803
|
// "Delta Robot Kinematics by Steve Graves" V3
|
7803
|
7804
|
|
7804
|
|
- // Result is in cartesian[].
|
|
7805
|
+ // Result is in cartesian_position[].
|
7805
|
7806
|
|
7806
|
7807
|
//Create a vector in old coordinates along x axis of new coordinate
|
7807
|
7808
|
float p12[3] = { delta_tower2_x - delta_tower1_x, delta_tower2_y - delta_tower1_y, z2 - z1 };
|
|
@@ -7845,9 +7846,9 @@ void clamp_to_software_endstops(float target[3]) {
|
7845
|
7846
|
//Now we can start from the origin in the old coords and
|
7846
|
7847
|
//add vectors in the old coords that represent the
|
7847
|
7848
|
//Xnew, Ynew and Znew to find the point in the old system
|
7848
|
|
- cartesian[X_AXIS] = delta_tower1_x + ex[0]*Xnew + ey[0]*Ynew - ez[0]*Znew;
|
7849
|
|
- cartesian[Y_AXIS] = delta_tower1_y + ex[1]*Xnew + ey[1]*Ynew - ez[1]*Znew;
|
7850
|
|
- cartesian[Z_AXIS] = z1 + ex[2]*Xnew + ey[2]*Ynew - ez[2]*Znew;
|
|
7849
|
+ cartesian_position[X_AXIS] = delta_tower1_x + ex[0]*Xnew + ey[0]*Ynew - ez[0]*Znew;
|
|
7850
|
+ cartesian_position[Y_AXIS] = delta_tower1_y + ex[1]*Xnew + ey[1]*Ynew - ez[1]*Znew;
|
|
7851
|
+ cartesian_position[Z_AXIS] = z1 + ex[2]*Xnew + ey[2]*Ynew - ez[2]*Znew;
|
7851
|
7852
|
};
|
7852
|
7853
|
|
7853
|
7854
|
void forwardKinematics(float point[3]) {
|
|
@@ -7860,13 +7861,6 @@ void clamp_to_software_endstops(float target[3]) {
|
7860
|
7861
|
stepper.get_axis_position_mm(Z_AXIS));
|
7861
|
7862
|
}
|
7862
|
7863
|
|
7863
|
|
- void set_current_from_steppers() {
|
7864
|
|
- set_cartesian_from_steppers();
|
7865
|
|
- current_position[X_AXIS] = cartesian[X_AXIS];
|
7866
|
|
- current_position[Y_AXIS] = cartesian[Y_AXIS];
|
7867
|
|
- current_position[Z_AXIS] = cartesian[Z_AXIS];
|
7868
|
|
- }
|
7869
|
|
-
|
7870
|
7864
|
#if ENABLED(AUTO_BED_LEVELING_FEATURE)
|
7871
|
7865
|
|
7872
|
7866
|
// Adjust print surface height by linear interpolation over the bed_level array.
|
|
@@ -7911,6 +7905,24 @@ void clamp_to_software_endstops(float target[3]) {
|
7911
|
7905
|
|
7912
|
7906
|
#endif // DELTA
|
7913
|
7907
|
|
|
7908
|
+void set_current_from_steppers() {
|
|
7909
|
+ #if ENABLED(DELTA)
|
|
7910
|
+ set_cartesian_from_steppers();
|
|
7911
|
+ current_position[X_AXIS] = cartesian_position[X_AXIS];
|
|
7912
|
+ current_position[Y_AXIS] = cartesian_position[Y_AXIS];
|
|
7913
|
+ current_position[Z_AXIS] = cartesian_position[Z_AXIS];
|
|
7914
|
+ #elif ENABLED(AUTO_BED_LEVELING_FEATURE)
|
|
7915
|
+ vector_3 pos = planner.adjusted_position(); // values directly from steppers...
|
|
7916
|
+ current_position[X_AXIS] = pos.x;
|
|
7917
|
+ current_position[Y_AXIS] = pos.y;
|
|
7918
|
+ current_position[Z_AXIS] = pos.z;
|
|
7919
|
+ #else
|
|
7920
|
+ current_position[X_AXIS] = stepper.get_axis_position_mm(X_AXIS); // CORE handled transparently
|
|
7921
|
+ current_position[Y_AXIS] = stepper.get_axis_position_mm(Y_AXIS);
|
|
7922
|
+ current_position[Z_AXIS] = stepper.get_axis_position_mm(Z_AXIS);
|
|
7923
|
+ #endif
|
|
7924
|
+}
|
|
7925
|
+
|
7914
|
7926
|
#if ENABLED(MESH_BED_LEVELING)
|
7915
|
7927
|
|
7916
|
7928
|
// This function is used to split lines on mesh borders so each segment is only part of one mesh area
|