/* * 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); } } }