' gantry plotter demo DPJ 2020 username "DaveJacko"
' uses Pythag's theorem and Bresenham's line drawing algorithm

init
Pin(enabpin)=0
ls=9333 'steps=350mm = plot area size in steps
xnow=ls/2 'start at centre of plot area
ynow=ls/2
l1now=ls*0.707 '1/sqr(2)
l2now=ls*0.707 '=250mm

mainlp:  ' --- the main loop

t$=Inkey$   ' keypresses, u/d = pen raise/lower
If t$="" Then GoTo mainlp
If t$=" " Then Pin(enabpin)=1:PWM 2,stop: End
If t$="u" Then penup
If t$="d" Then pendn

a$=a$+t$
If Len(a$)>3 Then a$=Right$(a$,3)

If a$=lc$ Then moveby -1,0  'cursor keys move the pen around
If a$=rc$ Then moveby 1,0
If a$=uc$ Then moveby 0,-1
If a$=dc$ Then moveby 0,1

GoTo mainlp


Sub MoveBy(mbx,mby)  ' moveby ie move pen relative to current posn.

mbx=mbx*266 '=1 cm
mby=mby*266

' calc turns from mbx,y

l1now=Sqr(xnow*xnow+ynow*ynow) 'maybe +residual l1now
l2now=Sqr((ls-xnow)*(ls-xnow)+ynow*ynow)
xtarg=xnow+mbx
ytarg=ynow+mby
l1targ=Sqr(xtarg*xtarg+ytarg*ytarg)
l2targ=Sqr((ls-xtarg)*(ls-xtarg)+ytarg*ytarg)
l1mov=l1targ-l1now
l2mov=l2targ-l2now

Print "mbx,mby",,mbx,mby
Print "xnow,ynow",xnow,ynow
Print "xtarg,ytarg",xtarg,ytarg
Print "l1now,l2now",l1now,l2now
Print "l1targ,l2targ",l1targ,l2targ
Print "l1mov,l2mov",l1mov,l2mov
Print

xnow=xtarg
ynow=ytarg

' now turn motors by l1mov,l2mov

'directions..
If l1mov>0 Then Pin(dirxpin)=0 Else Pin(dirxpin)=1
If l2mov>0 Then Pin(dirypin)=1 Else Pin(dirypin)=0

'major axis is..
If Abs(l1mov)>=Abs(l2mov) Then 'l1 is major
l1i=Sgn(l1mov)
l2i=l2mov/Abs(l1mov)
ni=Abs(l1mov) 'num of incs
Print "l1 major"
Else 'l2 is major axis
l2i=Sgn(l2mov)
l1i=l1i/Abs(l2mov)
ni=Abs(l2mov)
Print "l2 major"
EndIf


l1acc=0:l2acc=0

For i=1 To ni ' do move
l1acc=l1acc+l1i
l2acc=l2acc+l2i

If l1acc>=1 Then l1acc=l1acc-1:Pin(stpxpin)=1
If l1acc<=-1 Then l1acc=l1acc+1:Pin(stpxpin)=1
If l2acc>=1 Then l2acc=l2acc-1:Pin(stpypin)=1
If l2acc<=-1 Then l2acc=l2acc+1:Pin(stpypin)=1

wait1
Pin(stpxpin)=0
Pin(stpypin)=0

Next i

End Sub

Sub init '---- init

stpxpin=2  ' define step pins
stpypin=3
stpzpin=4
dirxpin=5   '..direction pins
dirypin=6
dirzpin=7
enabpin=9
stoppin=15
'servopin=26 pwm

SetPin 2,dout
SetPin 3,dout
SetPin 4,dout
SetPin 5,dout
SetPin 6,dout
SetPin 7,dout
Pin(9)=1 'enable off
SetPin 9,dout
SetPin 15,din,pullup 'int?

e$=Chr$(27) 'define escape
uc$=e$+"[A" 'cursor keys up/down/right/left
dc$=e$+"[B"
rc$=e$+"[C"
lc$=e$+"[D"

SetTick 1,int1ms  'timer

Pause 100

End Sub

Sub penup   ' lift the pen
PWM 2,50,7
End Sub

Sub pendn   ' drop the pen
PWM 2,50,10
End Sub

Sub int1ms   ' --timer subs
iflag=1
End Sub

Sub wait1
w1lp:
If Not iflag Then GoTo w1lp
iflag=0
End Sub