/*
* public void MoveAxes(int xSteps, int ySteps)
* {
* int max = (xSteps > ySteps) ? xSteps : ySteps;
* uint time = (uint)max * FeedRate;
*
* MoveAxes(xSteps, ySteps, time);
* }
*
* public void MoveAxes(int xSteps, int ySteps, uint timeInUs)
* {
* if (xSteps == 0 && ySteps == 0) return;
*
* // Single axis move
*
* if (xSteps == 0)
* {
* MoveSingleAxis(YStepPort, ySteps, (uint)FeedRate);
* return;
* }
*
* if (ySteps == 0)
* {
* MoveSingleAxis(XStepPort, xSteps, (uint)FeedRate);
* return;
* }
*
* // Coordinated move
*
* uint xstep = timeInUs / (uint)xSteps / 2;
* uint ystep = timeInUs / (uint)ySteps / 2;
*
* // Proc2
* // This method waits the time needed for the next closest state change. It's
* // pretty accurate and independent of the numbers.
*
* uint uxstep = (uint)xstep;
* uint uystep = (uint)ystep;
*
* uint xtime = uxstep;
* uint ytime = uystep;
*
* uint ellapsed = 0;
*
* while (ellapsed < timeInUs)
* {
* if (xtime > ytime)
* {
* NetduinoDevice.Delay.Microseconds(ytime);
* ellapsed += ytime;
* xtime -= ytime;
* YLastState = !YLastState;
* YStepPort.Write(YLastState);
* ytime = uystep;
* }
* else
* {
* NetduinoDevice.Delay.Microseconds(xtime);
* ellapsed += xtime;
* ytime -= xtime;
* XLastState = !XLastState;
* XStepPort.Write(XLastState);
* xtime = uxstep;
* }
* }
*
* AbsoluteXSteps += GetAbsoluteSteps(xSteps, XDirectionPort);
* AbsoluteYSteps += GetAbsoluteSteps(ySteps, YDirectionPort);
* }
*/
/*
* public void MoveAxes3(int xSteps, int ySteps, int zSteps)
* {
* // 3D line Bresenham
*
* int dx = xSteps;
* int dy = ySteps;
* int dz = zSteps;
*
* int x_inc = (dx < 0) ? -1 : 1;
* int y_inc = (dy < 0) ? -1 : 1;
* int z_inc = (dz < 0) ? -1 : 1;
*
* int Adx = Abs(dx);
* int Ady = Abs(dy);
* int Adz = Abs(dz);
*
* int dx2 = Adx * 2;
* int dy2 = Ady * 2;
* int dz2 = Adz * 2;
*
* if ((Adx >= Ady) && (Adx >= Adz))
* {
* int err_1 = dy2 - Adx;
* int err_2 = dz2 - Adx;
*
* for (int c = 0; c < Adx; ++c)
* {
* if (err_1 > 0)
* {
* YLastState = !YLastState;
* YStepPort.Write(YLastState);
* NetduinoDevice.Delay.Microseconds(StepInterval);
* err_1 -= dx2;
* }
*
* if (err_2 > 0)
* {
* ZLastState = !ZLastState;
* ZStepPort.Write(ZLastState);
* NetduinoDevice.Delay.Microseconds(StepInterval);
* err_2 -= dx2;
* }
*
* err_1 += dy2;
* err_2 += dz2;
* XLastState = !XLastState;
* XStepPort.Write(XLastState);
* NetduinoDevice.Delay.Microseconds(StepInterval);
* }
* }
*
* if ((Ady > Adx) && (Ady >= Adz))
* {
* int err_1 = dx2 - Ady;
* int err_2 = dz2 - Ady;
*
* for (int c = 0; c < Ady; ++c)
* {
* if (err_1 > 0)
* {
* XLastState = !XLastState;
* XStepPort.Write(XLastState);
* NetduinoDevice.Delay.Microseconds(StepInterval);
* err_1 -= dy2;
* }
*
* if (err_2 > 0)
* {
* ZLastState = !ZLastState;
* ZStepPort.Write(ZLastState);
* NetduinoDevice.Delay.Microseconds(StepInterval);
* err_2 -= dy2;
* }
*
* err_1 += dx2;
* err_2 += dz2;
* YLastState = !YLastState;
* YStepPort.Write(YLastState);
* NetduinoDevice.Delay.Microseconds(StepInterval);
* }
* }
*
* if ((Adz > Adx) && (Adz > Ady))
* {
* int err_1 = dy2 - Adz;
* int err_2 = dx2 - Adz;
*
* for (int c = 0; c < Adz; ++c)
* {
* if (err_1 > 0)
* {
* YLastState = !YLastState;
* YStepPort.Write(YLastState);
* NetduinoDevice.Delay.Microseconds(StepInterval);
* err_1 -= dz2;
* }
*
* if (err_2 > 0)
* {
* XLastState = !XLastState;
* XStepPort.Write(XLastState);
* NetduinoDevice.Delay.Microseconds(StepInterval);
* err_2 -= dz2;
* }
*
* err_1 += dy2;
* err_2 += dx2;
* ZLastState = !ZLastState;
* ZStepPort.Write(ZLastState);
* NetduinoDevice.Delay.Microseconds(StepInterval);
* }
* }
* }
*/
public void MoveAxes4(int xSteps, int ySteps, int zSteps)
{
// 3D line Bresenham
int dx = xSteps;
int dy = ySteps;
int dz = zSteps;
int x_inc = (dx < 0) ? -1 : 1;
int y_inc = (dy < 0) ? -1 : 1;
int z_inc = (dz < 0) ? -1 : 1;
int Adx = Abs(dx);
int Ady = Abs(dy);
int Adz = Abs(dz);
int dx2 = Adx * 2;
int dy2 = Ady * 2;
int dz2 = Adz * 2;
if ((Adx >= Ady) && (Adx >= Adz))
{
int err_1 = dy2 - Adx;
int err_2 = dz2 - Adx;
for (int c = 0; c < Adx; ++c)
{
if (err_1 > 0)
{
YLastState = !YLastState;
YStepPort.Write(YLastState);
Delay.Axis(Axis.Y, XYStepInterval);
err_1 -= dx2;
}
if (err_2 > 0)
{
ZLastState = !ZLastState;
ZStepPort.Write(ZLastState);
Delay.Axis(Axis.Z, ZStepInterval);
err_2 -= dx2;
}
err_1 += dy2;
err_2 += dz2;
XLastState = !XLastState;
XStepPort.Write(XLastState);
Delay.Axis(Axis.X, XYStepInterval);
}
}
if ((Ady > Adx) && (Ady >= Adz))
{
int err_1 = dx2 - Ady;
int err_2 = dz2 - Ady;
for (int c = 0; c < Ady; ++c)
{
if (err_1 > 0)
{
XLastState = !XLastState;
XStepPort.Write(XLastState);
Delay.Axis(Axis.X, XYStepInterval);
err_1 -= dy2;
}
if (err_2 > 0)
{
ZLastState = !ZLastState;
ZStepPort.Write(ZLastState);
Delay.Axis(Axis.Z, ZStepInterval);
err_2 -= dy2;
}
err_1 += dx2;
err_2 += dz2;
YLastState = !YLastState;
YStepPort.Write(YLastState);
Delay.Axis(Axis.Y, XYStepInterval);
}
}
if ((Adz > Adx) && (Adz > Ady))
{
int err_1 = dy2 - Adz;
int err_2 = dx2 - Adz;
for (int c = 0; c < Adz; ++c)
{
if (err_1 > 0)
{
YLastState = !YLastState;
YStepPort.Write(YLastState);
Delay.Axis(Axis.Y, XYStepInterval);
err_1 -= dz2;
}
if (err_2 > 0)
{
XLastState = !XLastState;
XStepPort.Write(XLastState);
Delay.Axis(Axis.X, XYStepInterval);
err_2 -= dz2;
}
err_1 += dy2;
err_2 += dx2;
ZLastState = !ZLastState;
ZStepPort.Write(ZLastState);
Delay.Axis(Axis.Z, ZStepInterval);
}
}
}
