Browse Source

Fix CoreXY speed calculation

For cartesian bots, the X_AXIS is the real X movement and same for
Y_AXIS.
But for corexy bots, that is not true. The "X_AXIS" and "Y_AXIS" motors
(that should be named to A_AXIS
and B_AXIS) cannot be used for X and Y length, because A=X+Y and B=X-Y.
So we need to create other 2 "AXIS", named X_HEAD and Y_HEAD, meaning
the real displacement of the Head.
Having the real displacement of the head, we can calculate the total
movement length and apply the desired speed.
Alex Borro 9 years ago
parent
commit
422a958a34
2 changed files with 17 additions and 3 deletions
  1. 1
    1
      Marlin/Marlin.h
  2. 16
    2
      Marlin/planner.cpp

+ 1
- 1
Marlin/Marlin.h View File

@@ -171,7 +171,7 @@ void manage_inactivity(bool ignore_stepper_queue=false);
171 171
 #endif
172 172
 
173 173
 
174
-enum AxisEnum {X_AXIS=0, Y_AXIS=1, Z_AXIS=2, E_AXIS=3};
174
+enum AxisEnum {X_AXIS=0, Y_AXIS=1, Z_AXIS=2, E_AXIS=3, X_HEAD=4, Y_HEAD=5};
175 175
 
176 176
 
177 177
 void FlushSerialRequestResend();

+ 16
- 2
Marlin/planner.cpp View File

@@ -715,11 +715,21 @@ block->steps_y = labs((target[X_AXIS]-position[X_AXIS]) - (target[Y_AXIS]-positi
715 715
     if(feed_rate<minimumfeedrate) feed_rate=minimumfeedrate;
716 716
   } 
717 717
 
718
-  float delta_mm[4];
718
+/* This part of the code calculates the total length of the movement. 
719
+For cartesian bots, the X_AXIS is the real X movement and same for Y_AXIS.
720
+But for corexy bots, that is not true. The "X_AXIS" and "Y_AXIS" motors (that should be named to A_AXIS
721
+and B_AXIS) cannot be used for X and Y length, because A=X+Y and B=X-Y.
722
+So we need to create other 2 "AXIS", named X_HEAD and Y_HEAD, meaning the real displacement of the Head. 
723
+Having the real displacement of the head, we can calculate the total movement length and apply the desired speed.
724
+*/ 
719 725
   #ifndef COREXY
726
+    float delta_mm[4];
720 727
     delta_mm[X_AXIS] = (target[X_AXIS]-position[X_AXIS])/axis_steps_per_unit[X_AXIS];
721 728
     delta_mm[Y_AXIS] = (target[Y_AXIS]-position[Y_AXIS])/axis_steps_per_unit[Y_AXIS];
722 729
   #else
730
+    float delta_mm[6];
731
+    delta_mm[X_HEAD] = (target[X_AXIS]-position[X_AXIS])/axis_steps_per_unit[X_AXIS];
732
+    delta_mm[Y_HEAD] = (target[Y_AXIS]-position[Y_AXIS])/axis_steps_per_unit[Y_AXIS];
723 733
     delta_mm[X_AXIS] = ((target[X_AXIS]-position[X_AXIS]) + (target[Y_AXIS]-position[Y_AXIS]))/axis_steps_per_unit[X_AXIS];
724 734
     delta_mm[Y_AXIS] = ((target[X_AXIS]-position[X_AXIS]) - (target[Y_AXIS]-position[Y_AXIS]))/axis_steps_per_unit[Y_AXIS];
725 735
   #endif
@@ -731,7 +741,11 @@ block->steps_y = labs((target[X_AXIS]-position[X_AXIS]) - (target[Y_AXIS]-positi
731 741
   } 
732 742
   else
733 743
   {
734
-    block->millimeters = sqrt(square(delta_mm[X_AXIS]) + square(delta_mm[Y_AXIS]) + square(delta_mm[Z_AXIS]));
744
+    #ifndef COREXY
745
+      block->millimeters = sqrt(square(delta_mm[X_AXIS]) + square(delta_mm[Y_AXIS]) + square(delta_mm[Z_AXIS]));
746
+	#else
747
+	  block->millimeters = sqrt(square(delta_mm[X_HEAD]) + square(delta_mm[Y_HEAD]) + square(delta_mm[Z_AXIS]));
748
+    #endif	
735 749
   }
736 750
   float inverse_millimeters = 1.0/block->millimeters;  // Inverse millimeters to remove multiple divides 
737 751
 

Loading…
Cancel
Save