Deadband=1mm (in encoder counts)

do
 If abs(command_position - encoder_position) > Deadband then
   Enable_motor_and_or_PID 'bring the motor back to command_position
 else
   Disable_motor_and_or_PID 'we are within the deadband so kill the power
 end if
loop

loop while a$=""  'exit when any key not r

loop while a$<>"q"  'exit when any key not r

'step/dir_decoder_pico_PIO_version 4

setpin gp0,pio1    'the STEP signal
setpin gp1,pio1    'the DIRECTION signal

'Generic defines
f= 63e6             '63 MHz PIO frequency
pio clear 1
dim dat%(4)         'array to store FIFO data

dim p%(7)=(&h4800c520a02020,&ha0290047a0290008,&h8000a0c1,0,0,0,0,0)
pio program 1,p%()

'configure PIO 1 statemachine 0 (PIO 1 is free on PicoMiteVGA)
p0=pio(pinctrl 0,0,0,gp0)           'gp0 is lowest IN pin (gp0,gp1)
e0=pio(execctrl gp1,0,31)           'GP1=dir, wrap and wrap target
s0=pio(shiftctrl 0,0,0,0,0,0)       'shift IN direction is left (the 5'th '0')

'initialize PIO 1 state machine 0
pio init machine 1,0,f,p0,e0,s0,0  'init machine @ start loop

'start the quadrature decoder PIO 1 statemachine 0
pio start 1,0

setpin gp14,dout
setpin gp15,dout

'main MMBasic code -------------------------------------------------------------
 do:loop while inkey$<>""
'main loop
 
do
  a$=inkey$                                     'just for control
 
  'get the data from the fifo
  pio read 1,0,5,dat%()                         'read whole fifo
  posi%=dat%(3)                                  'last data in fifo
  if posi%>2147483647 then inc posi%,-4294967296 '2'th complement
  print posi%                                    'show position
 
  'just some pause
  pin(gp14)=1
  pause 10  
  pin(gp14)=0                                   'any delay needed
  pause 10
 
  if posi% >1000 then
     pin(gp15)=0
  elseif posi% <-1000 then
     pin(gp15)=1
  endif
 
  'reset position (PIO X register) under control of keyboard
  if a$="r" then                                'press r to zero position
    pio execute 1,0,&hA023    '= assembly "mov X, null" (zero the X register)
    a$=""
  end if
 
  'set defined position in register X under control of the keyboard
  if a$="s" then
    input "desired count ";cnt%
    cnt%=cnt% and &hffffffff
    pio write 1,0,1,cnt%       'write the count cnt% in the fifo
    pio execute 1,0,&h8080  '= assemble "pull" (moves FIFO to OSR register)
    PIO execute 1,0,&hA027  '= assemble "mov X, OSR" moves the OSR value to X
    a$=""
  end if
 
loop while a$<>"q"  'exit when any key not r

end

Do
 a$ = Inkey$                                     'just for control
 
 'Check if there is any data in the FIFO (PIO 1, SM 0)
 rx_count% = Pio(rxlevel 1, 0)
 
 If rx_count% > 0 Then
   'Drain the entire FIFO buffer, keeping only the newest value
   For i% = 1 To rx_count%
     Pio Read 1, 0, 1, dat%()
   Next i%
   
   posi% = dat%(0)                               'dat%(0) holds the freshest value
   
   'Convert to 32-bit signed 2's complement
   If posi% > 2147483647 Then posi% = posi% - 4294967296
   
   Print posi%                                   'instantaneous position
 EndIf
 
 Pause 100                                       'main loop interval
Loop While a$ = ""