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.
This commit is contained in:
parent
1a57644b0c
commit
422a958a34
|
@ -171,7 +171,7 @@ void manage_inactivity(bool ignore_stepper_queue=false);
|
|||
#endif
|
||||
|
||||
|
||||
enum AxisEnum {X_AXIS=0, Y_AXIS=1, Z_AXIS=2, E_AXIS=3};
|
||||
enum AxisEnum {X_AXIS=0, Y_AXIS=1, Z_AXIS=2, E_AXIS=3, X_HEAD=4, Y_HEAD=5};
|
||||
|
||||
|
||||
void FlushSerialRequestResend();
|
||||
|
|
|
@ -715,11 +715,21 @@ block->steps_y = labs((target[X_AXIS]-position[X_AXIS]) - (target[Y_AXIS]-positi
|
|||
if(feed_rate<minimumfeedrate) feed_rate=minimumfeedrate;
|
||||
}
|
||||
|
||||
float delta_mm[4];
|
||||
/* This part of the code calculates the total length of the movement.
|
||||
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.
|
||||
*/
|
||||
#ifndef COREXY
|
||||
float delta_mm[4];
|
||||
delta_mm[X_AXIS] = (target[X_AXIS]-position[X_AXIS])/axis_steps_per_unit[X_AXIS];
|
||||
delta_mm[Y_AXIS] = (target[Y_AXIS]-position[Y_AXIS])/axis_steps_per_unit[Y_AXIS];
|
||||
#else
|
||||
float delta_mm[6];
|
||||
delta_mm[X_HEAD] = (target[X_AXIS]-position[X_AXIS])/axis_steps_per_unit[X_AXIS];
|
||||
delta_mm[Y_HEAD] = (target[Y_AXIS]-position[Y_AXIS])/axis_steps_per_unit[Y_AXIS];
|
||||
delta_mm[X_AXIS] = ((target[X_AXIS]-position[X_AXIS]) + (target[Y_AXIS]-position[Y_AXIS]))/axis_steps_per_unit[X_AXIS];
|
||||
delta_mm[Y_AXIS] = ((target[X_AXIS]-position[X_AXIS]) - (target[Y_AXIS]-position[Y_AXIS]))/axis_steps_per_unit[Y_AXIS];
|
||||
#endif
|
||||
|
@ -731,7 +741,11 @@ block->steps_y = labs((target[X_AXIS]-position[X_AXIS]) - (target[Y_AXIS]-positi
|
|||
}
|
||||
else
|
||||
{
|
||||
#ifndef COREXY
|
||||
block->millimeters = sqrt(square(delta_mm[X_AXIS]) + square(delta_mm[Y_AXIS]) + square(delta_mm[Z_AXIS]));
|
||||
#else
|
||||
block->millimeters = sqrt(square(delta_mm[X_HEAD]) + square(delta_mm[Y_HEAD]) + square(delta_mm[Z_AXIS]));
|
||||
#endif
|
||||
}
|
||||
float inverse_millimeters = 1.0/block->millimeters; // Inverse millimeters to remove multiple divides
|
||||
|
||||
|
|
Loading…
Reference in a new issue