' 2 stepper MMBasic Demo DPJ 2020
' cursor keys move pen
' 'p' plots Hello world from G-Code
'    video on YouTube.. https://youtu.be/Buybf25-wbg

init
Pin(enabpin)=0
ls=9333 ' steps=350mm, max size of plot
xnow=ls/2 ' set the centre of plot area
ynow=ls/2 ' 27steps=1mm
l1now=ls*0.707 ' 1/sqr(2) 'set centre wire lengths
l2now=ls*0.707 ' =250mm

mainlp:

t$=Inkey$
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
If t$="p" Then plotdata

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

If a$=lc$ Then moveby -266,0
If a$=rc$ Then moveby 266,0
If a$=uc$ Then moveby 0,-266
If a$=dc$ Then moveby 0,266

GoTo mainlp

Sub MoveTo(mtx,mty) '======  MoveTo
moveby mtx-xnow,mty-ynow
End Sub


Sub MoveBy(mbx,mby)  ' ======  Move By

' calc turns from mbx,y

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

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
Else 'l2 is major axis
l2i=Sgn(l2mov)
l1i=l1mov/Abs(l2mov)
ni=Abs(l2mov)
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 PlotData ' ====== plot G-Code Data

Restore
xb=xnow:yb=ynow

gcloop:

Read t$

If Left$(t$,3)="G00" Then penup
If Left$(t$,3)="G01" Then pendn
If t$="M02" Then GoTo exgc

xcp=Instr(1,t$,"X")
ycp=Instr(1,t$,"Y")
'can also find F's and Z's

lcp=Len(t$)
If xcp<>0 Then
nx=Val(Mid$(t$,xcp+1,ycp-1))
ny=Val(Mid$(t$,ycp+1,lcp-ycp))
moveto xb+nx*30,yb-ny*30
EndIf

GoTo gcloop

exgc:

End Sub


Sub init '---- init

stpxpin=2
stpypin=3
stpzpin=4
dirxpin=5
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) 'escape
uc$=e$+"[A" 'cursor keys
dc$=e$+"[B"
rc$=e$+"[C"
lc$=e$+"[D"

SetTick 1,int1ms  'timer

Pause 100

End Sub

Sub penup
PWM 2,50,7
Pause 100
End Sub

Sub pendn
PWM 2,50,10
Pause 100
End Sub

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

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


'Hello World'
'in G-Code rendered with Stickfont
'converted with G-Code to Data prog.txt
'- a simple program producing data statements for MMbas

' DPJ sept2020

Data "G00Z0.0"
Data "X1.9048Y10.0"
Data "G01Z0.0F0.0"
Data "X1.9048Y0.0"
Data "G00Z0.0"
Data "X8.5714Y10.0"
Data "G01Z0.0F0.0"
Data "X8.5714Y0.0"
Data "G00Z0.0"
Data "X1.9048Y5.2381"
Data "G01Z0.0F0.0"
Data "X8.5714Y5.2381"
Data "G00Z0.0"
Data "X12.1764Y3.8095"
Data "G01Z0.0F0.0"
Data "X17.8907Y3.8095"
Data "X17.8907Y4.7619"
Data "X17.4145Y5.7143"
Data "X16.9383Y6.1905"
Data "X15.9859Y6.6667"
Data "X14.5573Y6.6667"
Data "X13.6049Y6.1905"
Data "X12.6526Y5.2381"
Data "X12.1764Y3.8095"
Data "X12.1764Y2.8571"
Data "X12.6526Y1.4286"
Data "X13.6049Y0.4762"
Data "X14.5573Y0.0"
Data "X15.9859Y0.0"
Data "X16.9383Y0.4762"
Data "X17.8907Y1.4286"
Data "G00Z0.0"
Data "X21.4462Y10.0"
Data "G01Z0.0F0.0"
Data "X21.4462Y0.0"
Data "G00Z0.0"
Data "X25.3545Y10.0"
Data "G01Z0.0F0.0"
Data "X25.3545Y0.0"
Data "G00Z0.0"
Data "X31.1675Y6.6667"
Data "G01Z0.0F0.0"
Data "X30.2152Y6.1905"
Data "X29.2628Y5.2381"
Data "X28.7866Y3.8095"
Data "X28.7866Y2.8571"
Data "X29.2628Y1.4286"
Data "X30.2152Y0.4762"
Data "X31.1675Y0.0"
Data "X32.5961Y0.0"
Data "X33.5485Y0.4762"
Data "X34.5009Y1.4286"
Data "X34.9771Y2.8571"
Data "X34.9771Y3.8095"
Data "X34.5009Y5.2381"
Data "X33.5485Y6.1905"
Data "X32.5961Y6.6667"
Data "X31.1675Y6.6667"
Data "G00Z0.0"
Data "X43.9436Y10.0"
Data "G01Z0.0F0.0"
Data "X46.3245Y0.0"
Data "X48.7055Y10.0"
Data "X51.0864Y0.0"
Data "X53.4674Y10.0"
Data "G00Z0.0"
Data "X58.5256Y6.6667"
Data "G01Z0.0F0.0"
Data "X57.5732Y6.1905"
Data "X56.6208Y5.2381"
Data "X56.1446Y3.8095"
Data "X56.1446Y2.8571"
Data "X56.6208Y1.4286"
Data "X57.5732Y0.4762"
Data "X58.5256Y0.0"
Data "X59.9541Y0.0"
Data "X60.9065Y0.4762"
Data "X61.8589Y1.4286"
Data "X62.3351Y2.8571"
Data "X62.3351Y3.8095"
Data "X61.8589Y5.2381"
Data "X60.9065Y6.1905"
Data "X59.9541Y6.6667"
Data "X58.5256Y6.6667"
Data "G00Z0.0"
Data "X65.903Y6.6667"
Data "G01Z0.0F0.0"
Data "X65.903Y0.0"
Data "G00Z0.0"
Data "X65.903Y3.8095"
Data "G01Z0.0F0.0"
Data "X66.3792Y5.2381"
Data "X67.3316Y6.1905"
Data "X68.284Y6.6667"
Data "X69.7125Y6.6667"
Data "G00Z0.0"
Data "X72.254Y10.0"
Data "G01Z0.0F0.0"
Data "X72.254Y0.0"
Data "G00Z0.0"
Data "X81.4004Y10.0"
Data "G01Z0.0F0.0"
Data "X81.4004Y0.0"
Data "G00Z0.0"
Data "X81.4004Y5.2381"
Data "G01Z0.0F0.0"
Data "X80.448Y6.1905"
Data "X79.4956Y6.6667"
Data "X78.067Y6.6667"
Data "X77.1146Y6.1905"
Data "X76.1623Y5.2381"
Data "X75.6861Y3.8095"
Data "X75.6861Y2.8571"
Data "X76.1623Y1.4286"
Data "X77.1146Y0.4762"
Data "X78.067Y0.0"
Data "X79.4956Y0.0"
Data "X80.448Y0.4762"
Data "X81.4004Y1.4286"
Data "G00Z0.0"

Data "M02"   'end of prog