from PiicoDev_VL53L1X import PiicoDev_VL53L1X
from time import sleep

import serial
ser = serial.Serial(
  port='/dev/ttyAMA0',
  baudrate = 9600,
  parity=serial.PARITY_NONE,
  stopbits=serial.STOPBITS_ONE,
  bytesize=serial.EIGHTBITS,
  timeout=1
         )

distSensor = PiicoDev_VL53L1X()

while True:
   dist = distSensor.read() # read the distance in millimetres
   print(str(dist) + " mm") # convert the number to a string and print
   sleep(1)

'Pico@Wit TBS.BAS
'Uses Com1 to talk to a WitMotion V53L laser ditance sensor
'Com2 writes to an HC12, with a checksum should you wish.
'
'Connections:
'Pico    V53L
' 3V3 to Vcc
' GND to GND
' GP0 to RXD (Green)
' GP1 to TXD (Yellow)
'        SCL n/c
'        SDA n/c
'
'Pico    HC12
' 3v3 to Vcc
' GND to GND
' GP4 to RXD
' GP5 to TxD
'        SET n/c
'
'option Explicit  'zzz next job
'Option Autorun ON  'best done from the Console
 
 Dim String Dat, Range, In_Char, Out, Message
 Dim Integer OK, Rng_Ave, Ave_Count = 5, Count 'Flag set if "Range Valid" (VALID is an MMBasic keyword)
 Dim Integer Range_Start, Range_End, Chksum
 Dim Integer Range_Freq = 4000                 'Frequency of getting Range data m_seconds
 SETPIN GP0, GP1, COM1                         'Assign the I/O pins for the first serial port for V53L
 setpin GP4, GP5, COM2                         '           I/O pins for the HC12

 
 OPEN "COM1:115200" AS #1 'open the first serial port (V53L default speed is 115200)
 open "COM2:9600"   as #2 'open the second serial port with a speed of 9600 baud (for my HC12 network)
 
 Send_Message "Starting"
 
 SETTICK Range_Freq, Get_Range, 1  'Get a range every 'Get_Range' m_seconds - change to suit
 
 do'zzz Do something useful in here?
   Watchdog Range_Freq * 3 'If program is hung start again
   
 Loop
 
 CLOSE #1
end
 
Sub Send_Message(Txt$)
 Txt$ = Txt$ + "," + time$ + "," + Date$
 Print Txt$
 Out = "$AGRNG," + Txt$  'These are my NMEA-like sentences
 Out = CRC$(Out)
 print #2, Out
End Sub 'Send_Message
 
Sub Get_Range
 Local Integer OK = 0, Count = 0, Rng_Ave = 0
 Watchdog Range_Freq * 3
'All other variables are Global
 
 Timer = 0
 do
   Dat = ""
   If Timer > Range_Freq * 0.9 then
     Send_Message "Quitting Sub"
     Exit Do  '
   End If
   do
     IF LOC(#1) > 0 THEN
       In_Char = INPUT$(1,#1)
'       Do we have a LF or CR character = end of line?
       If asc(mid$(In_Char,len(In_Char),1))=10 or asc(mid$(In_Char,len(In_Char),1))=13) then
         Exit do
       Else
         Dat = Dat + In_Char
       end if
     END IF
   loop
   If Instr(Dat, "Range Valid") then
     If Not OK then
       OK = 1  'This will hold for the duration of the Sub
     End if
   End If
   if Instr( Dat, "Signal Fail") then OK = 0: Send_Message "Signal Fail"
   if Instr( Dat, "No Update")   then OK = 0: Send_Message "No Update"
   
   If OK then  'This will be OK for subsequent reads until next time period. It will then start as false
     Range_Start = Instr(Dat,"d:")
     Range_End =   Instr(Dat," mm")
     If Range_Start >0 and Range_End >0 then 'We have a valid range (it has both a Start and an End)
       Range = mid$(Dat, 1, Range_End)
       Range = mid$(Range, Range_Start+2)
       Range = Trim$(Range, " ")
       If Count < Ave_Count then
         Rng_Ave = Rng_Ave + vaL(Range)
         Count = Count + 1 'ZZ use to count valid Ranges for average
         print Range, Count
       Else
         Rng_Ave = Rng_Ave / Count
         Range = str$(Rng_Ave)
         Out = "$AGRNG,"+ Range
         Out = CRC$(Out)
         print "Ave = "; Range; tab(15);Out$
         Print
         print #2, Out
         OK = 0
         Exit Do
       End if
     Else
     End If
   End if
 loop

End Sub  'Get_Range
 
' trim any characters in c$ from the start and end of s$
Function Trim$(s$, c$)
 Trim$ = RTrim$(LTrim$(s$, c$), c$)
End Function
' trim any characters in c$ from the end of s$
Function RTrim$(s$, c$)
 RTrim$ = s$
 Do While Instr(c$, Right$(RTrim$, 1))
   RTrim$ = Mid$(RTrim$, 1, Len(RTrim$) - 1)
 Loop
End Function
' trim any characters in c$ from the start of s$
Function LTrim$(s$, c$)
 LTrim$ = s$
 Do While Instr(c$, Left$(LTrim$, 1))
   LTrim$ = Mid$(LTrim$, 2)
 Loop
End Function
 
FUNCTION CRC$(Txt$ AS String) 'Returns the Txt$ passed to it with "*hh" added, where hh is the NMEA Hex checksum
'The receiving station checks the checksum - if not OK it just ignores that sentence
'Local String Txt$
 Local Integer n
 Chksum = 0
 FOR n = 2 TO LEN(Txt$)  'n=2 makes it skip the leading $
   Chksum=Chksum XOR ASC(MID$(Txt$,n,1))
 NEXT n
 CRC$=Txt$+"*"+HEX$(Chksum,2)
END FUNCTION