CLS
LOCATE 5, 4: PRINT "X": LOCATE 5, 14: PRINT "Y": LOCATE 5, 24: PRINT "Z"
DIM steppatxz(8)
DIM steppaty(8)
steppatxz(1) = 1: steppatxz(2) = 5: steppatxz(3) = 4: steppatxz(4) = 6: steppatxz(5) = 2: steppatxz(6) = 10: steppatxz(7) = 8: steppatxz(8) = 9
steppaty(1) = 16: steppaty(2) = 80: steppaty(3) = 64: steppaty(4) = 96: steppaty(5) = 32: steppaty(6) = 160: steppaty(7) = 128: steppaty(8) = 144
DEFDBL I-K, P, X-Z
pi = 3.141592654#
xmot = 0: ymot = 0: zmot = 0
xcomp = 0: ycomp = 0: zcomp = 0
xstep = 0: ystep = 0: zstep = 0
xnew = 0: ynew = 0: znew = 0: inew = 0: jnew = 0: knew = 0:
xrend = 0: yrend = 0: zrend = 0
xcen = 0: ysen = 0: zcen = 0
xdir = 0: ydir = 0: zdir = 0
xstart = 0: ystart = 0: zstart = 0
xrot = 0: yrot = 0: zrot = 0
xlinemult = 0: ylinemult = 0: zlinemalt = 0
xyzlineleng = 0
xyzlineang = 0
xyzrad = 0
xyzstartang = 0
xyzendang = 0
REM initial values for testing
feed = 40
xcomp = 0: ycomp = 0: comp = 0
xstep = 0: ystep = 0: zstep = 0
xnew = 0: ynew = 0: znew = 0: inew = 0: jnew = 0
LOCATE 4, 5: PRINT USING "###.####"; xcomp: LOCATE 4, 15: PRINT USING "###.####"; ycomp: LOCATE 4, 25: PRINT USING "###.####"; zcomp
LOCATE 6, 5: PRINT USING "###.####"; xnew: LOCATE 6, 15: PRINT USING "###.####"; ynew: LOCATE 6, 25: PRINT USING "###.####"; znew: LOCATE 6, 35: PRINT USING "###.####"; inew: LOCATE 6, 45: PRINT USING "###.####"; jnew

REM start of main program
REM GOSUB gthree
REM END
GOSUB openfile
restart:
GOSUB nextline
LOCATE 4, 5: PRINT USING "###.####"; xcomp: LOCATE 4, 15: PRINT USING "###.####"; ycomp: LOCATE 4, 25: PRINT USING "###.####"; zcomp
LOCATE 6, 5: PRINT USING "###.####"; xnew: LOCATE 6, 15: PRINT USING "###.####"; ynew: LOCATE 6, 25: PRINT USING "###.####"; znew: LOCATE 6, 35: PRINT USING "###.####"; inew: LOCATE 6, 45: PRINT USING "###.###"; jnew

IF inew = 0 AND jnew = 0 AND xnew - xcomp = 0 AND ynew - ycomp = 0 AND znew - zcomp = 0 THEN
ELSE
IF gcode = 1 OR gcode = 2 OR gcode = 3 THEN
OUT 890, 0
ELSE
OUT 890, 255
END IF
IF gcode = 0 OR gcode = 1 THEN GOSUB gone
IF gcode = 2 THEN GOSUB gtwo
IF gcode = 3 THEN GOSUB gthree
END IF
REM PRINT gcode, xnew, ynew, inew, jnew
GOTO restart



END

gone:
REM PRINT xnew, ynew
xstart = xcomp: ystart = ycomp: zstart = zcomp
xyzlineleng = SQR((xnew - xcomp) ^ 2 + (ynew - ycomp) ^ 2 + (znew - zcomp) ^ 2)
xlinemult = (xnew - xcomp) / xyzlineleng
ylinemult = (ynew - ycomp) / xyzlineleng
zlinemult = (znew - zcomp) / xyzlineleng
REM PRINT xlinemult, ylinemult, zlinemult
REM PRINT USING "####.####"; xyzlineleng
xyztot = xyztot + xyzlineleng
FOR i = 0 TO xyzlineleng STEP .0001
xcomp = xstart + i * xlinemult
ycomp = ystart + i * ylinemult
zcomp = zstart + i * zlinemult
IF ABS(xcomp - xstep) >= .0039 THEN
IF xcomp > xstep THEN
xdir = 1: xstep = xstep + .00401295#
ELSE
xdir = -1: xstep = xstep - .00401295#
END IF
END IF
IF ABS(ycomp - ystep) >= .0039 THEN
IF ycomp > ystep THEN
ydir = 1: ystep = ystep + .00401295#
ELSE
ydir = -1: ystep = ystep - .00401295#
END IF
END IF
IF ABS(zcomp - zstep) >= .0039 THEN
IF zcomp > zstep THEN
zdir = 1: zstep = zstep + .00401295#
ELSE
ydir = -1: ystep = ystep - .00401295#
END IF
END IF
IF xdir <> 0 OR ydir <> 0 OR zdir <> 0 THEN GOSUB stepout
NEXT i
xcomp = xnew: ycomp = ynew: zcomp = znew
RETURN

gtwo: REM clockwise
xyzrad = SQR(inew ^ 2 + jnew ^ 2)
REM start ang
IF jnew = 0 THEN
xyzstartang = pi / 2
ELSE
xyzstartang = ATN(ABS(inew / jnew))
END IF
IF inew <= 0 AND jnew <= 0 THEN xyzstartang = (pi / 2 - xyzstartang) + 2 * pi
IF inew <= 0 AND jnew > 0 THEN xyzstartang = (pi * 1.5 + xyzstartang) + 2 * pi
IF inew > 0 AND jnew <= 0 THEN xyzstartang = (pi / 2 + xyzstartang) + 2 * pi
IF inew > 0 AND jnew > 0 THEN xyzstartang = (pi * 1.5 - xyzstartang) + 2 * pi
REM end angle
xrend = (xcomp + inew) - xnew: yrend = (ycomp + jnew) - ynew
IF yrend = 0 THEN
xyzendang = pi / 2
ELSE
xyzendang = ATN(ABS(xrend / yrend))
END IF
IF xrend <= 0 AND yrend <= 0 THEN xyzendang = (pi / 2 - xyzendang) + 2 * pi
IF xrend <= 0 AND yrend > 0 THEN xyzendang = (pi * 1.5 + xyzendang) + 2 * pi
IF xrend > 0 AND yrend <= 0 THEN xyzendang = (pi / 2 + xyzendang) + 2 * pi
IF xrend > 0 AND yrend > 0 THEN xyzendang = (pi * 1.5 - xyzendang) + 2 * pi
IF xyzstartang <= xyzendang THEN xyzstartang = xyzstartang + 2 * pi
xyzlineleng = 2 * pi * xyzrad * ((xyzstartang - xyzendang) / (2 * pi))
xyzstep = -1 * (xyzstartang - xyzendang) / (xyzlineleng / .0001)
xcen = xcomp + inew: ycen = ycomp + jnew
REM Print: Print: Print
REM Print xyzlineleng
REM Print xyzstartang * 180 / pi, xyzendang * 180 / pi
REM Print xcen, ycen

FOR i = xyzstartang TO xyzendang STEP xyzstep
xcomp = xcen + (COS(i)) * xyzrad
ycomp = ycen + (SIN(i)) * xyzrad
IF ABS(xcomp - xstep) >= .0039 THEN
IF xcomp < xstep THEN
xrot = -1
ELSE
xrot = 1
END IF
xdir = 1 * xrot: xstep = xstep + .00401295# * xrot
END IF
IF ABS(ycomp - ystep) >= .0039 THEN
IF ycomp < ystep THEN
yrot = -1
ELSE
yrot = 1
END IF
ydir = 1 * yrot: ystep = ystep + .00401295# * yrot
END IF
IF xdir <> 0 OR ydir <> 0 THEN GOSUB stepout
FOR delayloop = 1 TO 0
NEXT delayloop
NEXT i
xcomp = xnew: ycomp = ynew: inew = 0: jnew = 0
RETURN


gthree: REM cclockwise
xyzrad = SQR(inew ^ 2 + jnew ^ 2)
REM start ang
IF jnew = 0 THEN
xyzstartang = pi / 2
ELSE
xyzstartang = ATN(ABS(inew / jnew))
END IF
IF inew <= 0 AND jnew <= 0 THEN xyzstartang = (pi / 2 - xyzstartang) + 2 * pi
IF inew <= 0 AND jnew > 0 THEN xyzstartang = (pi * 1.5 + xyzstartang) + 2 * pi
IF inew > 0 AND jnew <= 0 THEN xyzstartang = (pi / 2 + xyzstartang) + 2 * pi
IF inew > 0 AND jnew > 0 THEN xyzstartang = (pi * 1.5 - xyzstartang) + 2 * pi
REM end angle
xrend = (xcomp + inew) - xnew: yrend = (ycomp + jnew) - ynew
IF yrend = 0 THEN
xyzendang = pi / 2
ELSE
xyzendang = ATN(ABS(xrend / yrend))
END IF
IF xrend <= 0 AND yrend <= 0 THEN xyzendang = (pi / 2 - xyzendang) + 2 * pi
IF xrend <= 0 AND yrend > 0 THEN xyzendang = (pi * 1.5 + xyzendang) + 2 * pi
IF xrend > 0 AND yrend <= 0 THEN xyzendang = (pi / 2 + xyzendang) + 2 * pi
IF xrend > 0 AND yrend > 0 THEN xyzendang = (pi * 1.5 - xyzendang) + 2 * pi
IF xyzstartang >= xyzendang THEN xyzendang = xyzendang + 2 * pi
xyzlineleng = 2 * pi * xyzrad * ((xyzendang - xyzstartang) / (2 * pi))
xyzstep = (xyzendang - xyzstartang) / (xyzlineleng / .0001)
xcen = xcomp + inew: ycen = ycomp + jnew
REM PRINT xyzlineleng
REM PRINT xyzstartang * 180 / pi, xyzendang * 180 / pi
REM PRINT xcen, ycen, xyzstep, xyzlineleng

FOR i = xyzstartang TO xyzendang STEP xyzstep
xcomp = xcen + (COS(i)) * xyzrad
ycomp = ycen + (SIN(i)) * xyzrad
IF ABS(xcomp - xstep) >= .0039 THEN
IF xcomp < xstep THEN
xrot = -1
ELSE
xrot = 1
END IF
xdir = 1 * xrot: xstep = xstep + .00401295# * xrot
END IF
IF ABS(ycomp - ystep) >= .0039 THEN
IF ycomp < ystep THEN
yrot = -1
ELSE
yrot = 1
END IF
ydir = 1 * yrot: ystep = ystep + .00401295# * yrot
END IF
IF xdir <> 0 OR ydir <> 0 THEN GOSUB stepout
FOR delayloop = 1 TO 0
NEXT delayloop
NEXT i
xcomp = xnew: ycomp = ynew: inew = 0: jnew = 0
RETURN




stepout:
xmot = xmot + xdir
ymot = ymot + ydir
zmot = zmot + zdir
IF xmot >= 9 THEN xmot = 1
IF xmot <= 0 THEN xmot = 8
IF ymot >= 9 THEN ymot = 1
IF ymot <= 0 THEN ymot = 8
IF zmot >= 9 THEN zmot = 1
IF zmot <= 0 THEN zmot = 8
OUT 888, steppatxz(xmot) + steppaty(ymot)
OUT 889, steppatxz(zmot)
LOCATE 5, 5: PRINT USING "###.####"; xstep: LOCATE 5, 15: PRINT USING "###.####"; ystep: LOCATE 5, 25: PRINT USING "###.####"; zstep
xdir = 0
ydir = 0
zdir = 0
FOR accdelay = 1 TO 0
NEXT accdelay
RETURN

openfile:
OPEN "by3.fan" FOR INPUT AS #1
RETURN

nextline:
IF EOF(1) THEN PRINT xyztot: END
LINE INPUT #1, machline$
FOR strpoint = 1 TO LEN(machline$)
IF MID$(machline$, strpoint, 1) = "G" THEN gcode = VAL(MID$(machline$, strpoint + 1, LEN(machline$) - strpoint))
IF MID$(machline$, strpoint, 1) = "X" THEN xnew = VAL(MID$(machline$, strpoint + 1, LEN(machline$) - strpoint))
IF MID$(machline$, strpoint, 1) = "Y" THEN ynew = VAL(MID$(machline$, strpoint + 1, LEN(machline$) - strpoint))
IF MID$(machline$, strpoint, 1) = "I" THEN inew = VAL(MID$(machline$, strpoint + 1, LEN(machline$) - strpoint))
IF MID$(machline$, strpoint, 1) = "J" THEN jnew = VAL(MID$(machline$, strpoint + 1, LEN(machline$) - strpoint))
IF MID$(machline$, strpoint, 1) = "F" THEN feed = VAL(MID$(machline$, strpoint + 1, LEN(machline$) - strpoint))

NEXT strpoint
LOCATE 8, 4: PRINT "                                 "
LOCATE 8, 4: PRINT machline$
RETURN