/// <summary>
/// Uses the Bresenham's Algorithm to coordinate 3 axis of stepper movements to create a straight 3D line segment, using
/// the supplied Destination coordinates relative to a start vector of <0,0,0>. delay is the number of milliseconds between step pulses.
/// </summary>
public void PathToLinear(int destinationX, int destinationY, int destinationZ, int delay, out string errorMessage)
{
errorMessage = "";
int x = 0;
int y = 0;
int z = 0;
int deltaX = System.Math.Abs(destinationX);
int deltaY = System.Math.Abs(destinationY);
int deltaZ = System.Math.Abs(destinationZ);
int stepX;
int stepY;
int stepZ;
// Set X Direction
if (destinationX > 0)
{
stepX = 1;
AxisX.StepDirection = true;
}
else
{
stepX = -1;
AxisX.StepDirection = false;
}
// Set Y Direction
if (destinationY > 0)
{
stepY = 1;
AxisY.StepDirection = true;
}
else
{
stepY = -1;
AxisY.StepDirection = false;
}
// Set Z Direction
if (destinationZ > 0)
{
stepZ = 1;
AxisZ.StepDirection = true;
}
else
{
stepZ = -1;
AxisZ.StepDirection = false;
}
// Keep track of the progressive error that accumulates with each step on the major axis to adjust along the way
int errorXY = deltaX - deltaY;
int errorXZ = deltaX - deltaZ;
int errorYZ = deltaY - deltaZ;
while (x != destinationX || y != destinationY || z != destinationZ)
{
if (Program.MotionInterrupt)
{
if (!Program.Paused)
{
errorMessage = GetEStopErrorMessage();
return;
}
}
int offsetXY = errorXY * 2;
int offsetXZ = errorXZ * 2;
int offsetYZ = errorYZ * 2;
// Step X?
if (offsetXY > -deltaY && offsetXZ > -deltaZ)
{
errorXY -= deltaY;
errorXZ -= deltaZ;
x += stepX;
AxisX.Step();
}
// Step Y?
if (offsetXY < deltaX && offsetYZ > -deltaZ)
{
errorXY += deltaX;
errorYZ -= deltaZ;
y += stepY;
AxisY.Step();
}
// Step Z?
if (offsetXZ < deltaX && offsetYZ < deltaY)
{
errorXZ += deltaX;
errorYZ += deltaY;
z += stepZ;
AxisZ.Step();
}
DelayBy(delay);
}
}
/// <summary>
/// Finds the minimum limit switches for all 3 Axes
/// </summary>
public void FindHome(int delayRapid, int delaySlow, bool findX, bool findY, bool findZ)
{
bool wasIgnoringLimits = IgnoreLimitSwitches;
IgnoreLimitSwitches = false;
AxisX.StepDirection = false;
AxisY.StepDirection = false;
AxisZ.StepDirection = false;
bool xSeeking = findX;
bool ySeeking = findY;
bool zSeeking = findZ;
// --- STEP 1 - Rapid to home (All minimum limit switches)
while (xSeeking || ySeeking || zSeeking)
{
if (Program.EStopCondition)
{
return;
}
xSeeking = xSeeking && AxisX.LimitReached != Axis.LimitSwitch.Min && AxisX.LimitReached != Axis.LimitSwitch.Unknown;
ySeeking = ySeeking && AxisY.LimitReached != Axis.LimitSwitch.Min && AxisY.LimitReached != Axis.LimitSwitch.Unknown;
zSeeking = zSeeking && AxisZ.LimitReached != Axis.LimitSwitch.Min && AxisZ.LimitReached != Axis.LimitSwitch.Unknown;
if (xSeeking)
{
AxisX.Step();
}
if (ySeeking)
{
AxisY.Step();
}
if (zSeeking)
{
AxisZ.Step();
}
DelayBy(delayRapid);
}
// --- STEP 2 - After a pause, slowly move each axis back off until the switches are opened
AxisX.StepDirection = true;
AxisY.StepDirection = true;
AxisZ.StepDirection = true;
xSeeking = findX;
ySeeking = findY;
zSeeking = findZ;
Thread.Sleep(500);
while (xSeeking || ySeeking || zSeeking)
{
if (Program.EStopCondition)
{
return;
}
xSeeking = xSeeking && !AxisX.ResetLimitReached();
ySeeking = ySeeking && !AxisY.ResetLimitReached();
zSeeking = zSeeking && !AxisZ.ResetLimitReached();
if (xSeeking)
{
AxisX.Step();
}
if (ySeeking)
{
AxisY.Step();
}
if (zSeeking)
{
AxisZ.Step();
}
DelayBy(delaySlow);
}
IgnoreLimitSwitches = wasIgnoringLimits;
}
/// <summary>
/// Destination coordinates are relative microsteps. delay is supplied as milliseconds to delay between steps.
/// </summary>
public void GotoNonLinear(int destinationX, int destinationY, int destinationZ, int delay, out string errorMessage)
{
errorMessage = "";
int x = 0;
int y = 0;
int z = 0;
int stepX;
int stepY;
int stepZ;
// Set X Direction
if (destinationX > 0)
{
stepX = 1;
AxisX.StepDirection = true;
}
else
{
stepX = -1;
AxisX.StepDirection = false;
}
// Set Y Direction
if (destinationY > 0)
{
stepY = 1;
AxisY.StepDirection = true;
}
else
{
stepY = -1;
AxisY.StepDirection = false;
}
// Set Z Direction
if (destinationZ > 0)
{
stepZ = 1;
AxisZ.StepDirection = true;
}
else
{
stepZ = -1;
AxisZ.StepDirection = false;
}
while (x != destinationX || y != destinationY || z != destinationZ)
{
if (Program.MotionInterrupt)
{
if (!Program.Paused)
{
errorMessage = GetEStopErrorMessage();
return;
}
}
// Step X?
if (x != destinationX)
{
x += stepX;
AxisX.Step();
}
// Step Y?
if (y != destinationY)
{
y += stepY;
AxisY.Step();
}
// Step Z?
if (z != destinationZ)
{
z += stepZ;
AxisZ.Step();
}
DelayBy(delay);
}
}
