
'TODO - ADD watchdog, 

' FOV = 240 degrees
' 10 Hz Update Rate
' 1mm resolution
' 0.36 degrees angular resolution
' 682 Beams 

' This module makes use of only 640 beams and a  FOV of 225.22 degrees


'SCIP is a protocol used to communicate with URG. 
'Commands of SCIP protocols are used to interact with the URG. 

'SCIP commands are used to interact with URG sensor
'SCIP commands are classified as "Sensor information command", "Distance acquisition command" , and "Mode setting command".
'Setting of operation mode
'   "SCIP2.0" ... Change SCIP2.0 mode
'   "TM0", "TM1", "TM2" ... Time stamp mode
'   "SS" ... Change the baudrate
'   "BM" ... laser on.
'   "QT" ... laser off
'   "RS" ... Resets parameter, measurement suspends.
'   "CR" ... Change the rotation speed of the motor.
'Obtain status information
'   "VV" ... Acquisition of version information.
'   "PP" ... Acquisition of parameter information.
'   "II" ... Acquisition of status.
'Receive distance data
'   "GD", "GS" ... Acquisition of distance data one at a time.
'   "MD", "MS" ... Acquisition of distance data continuously.

Imports System.Threading
Imports System.IO.Ports
Imports System.Drawing
Imports System.Windows.Forms

Module modIF_URG
    Friend WithEvents comURG As New SerialPort
    Dim strUrgDataReceived As String

 

    Public Function ftnURGInitializeTop() As Boolean
        Try
            ftnURGInitializeTop = False
            If Not boolURGConnected And Not boolShutDown Then
                boolUrgLaserOn = False
                strStatusDriving = "Detecting URG."
                For i = 1 To 2
                    Application.DoEvents()
                    For Each sp As String In My.Computer.Ports.SerialPortNames
                        Dim strER1ComPort As String = ""
                        If (comBot IsNot Nothing) Then strER1ComPort = comBot.PortName
                        If Not boolURGConnected And Not boolShutDown And Not (sp = comBot.PortName) _
                            And Not ((sp = ("COM" & frmArm.intArmComPort.ToString)) And frmArm.boolEnableArm) Then   ' 
                            frmDebug.subWriteLogs(ftnElapsedTimeS() & " ftnURG_SerialConfig: Checking " & sp)
                            If comURG IsNot Nothing Then ftnUrgClose()
                            comURG.PortName = sp  '
                            boolURGConnected = ftnURG_CheckConnection()
                            If Not boolURGConnected Then ftnUrgClose()
                        End If
                    Next sp
                Next i
                If boolURGConnected Then
                    frmDebug.subWriteLogs(" ftnURG_SerialConfig: Connected " & comURG.PortName)
                    strStatusDriving = "URG Connected."
                    sngScanFOV = 225.22
                    intScanVectorsMaxRangeInches = 560 / 2.54
                    intScanVectorsMinRangeInches = 0
                    subRecalculateVariables()
                    subCalculateScanBeamAngles()
                    subCalculateScanCalCoefficients()
                Else
                    If comURG IsNot Nothing Then comURG.PortName = "999"
                End If
            ElseIf boolShutDown Then
                ftnUrgClose()
            Else
                boolURGConnected = (ftnUrgXmit("BM", 8, 500) <> "")  ' Laser on
            End If
            strStatusDriving = ""
            ftnURGInitializeTop = boolURGConnected
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnURGInitializeTop :  " & ex.ToString)
            ftnURGInitializeTop = False
            ftnUrgClose()
        End Try
    End Function

    Public Function ftnURG_SerialOpen() As Boolean
        ' returns true if comBot is successfully opened
        Try
            ftnURG_SerialOpen = False
            If comURG IsNot Nothing And Not boolShutDown Then
                If Not boolURGConnected And Not comURG.IsOpen Then comURG.Open()
                If Not comURG.IsOpen Then frmDebug.subWriteLogs(" ftnURG_SerialOpen: could not open comURG = " & comURG.PortName)
            ElseIf boolShutDown Then
                ftnUrgClose()
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnURG_SerialOpen :  " & ex.ToString)
            ftnUrgClose()
        End Try
    End Function

    Function ftnURG_CheckConnection() As Boolean
        Try
            ftnURG_CheckConnection = False
            If comURG IsNot Nothing And Not boolShutDown Then
                ftnURG_SerialOpen()
                If comURG.IsOpen Then
                    subURGInitialize()
                    boolURGConnected = (ftnUrgXmit("BM", 8, 500) <> "")  ' Laser on
                End If
            ElseIf boolShutDown Then
                ftnUrgClose()
            End If
            If Not boolURGConnected Then frmDebug.subWriteLogs(" ftnURG_CheckConnection: ERROR  - boolURGConnected = False.   ")
            ftnURG_CheckConnection = boolURGConnected
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnURG_CheckConnection :  " & ex.ToString)
            ftnUrgClose()
        End Try
    End Function


    Sub subURGInitialize()
        Try
            If comURG IsNot Nothing And Not boolShutDown Then
                If Not boolURGConnected Then
                    ftnUrgXmit("SCIP2.0", 13, 500) 'protocol
                    ftnUrgXmit("RS", 8, 500) ' reset
                    boolUrgLaserOn = False
                End If
                If Not boolUrgLaserOn Then
                    boolURGConnected = (ftnUrgXmit("BM", 8, 500) <> "")  ' Laser on
                    boolUrgLaserOn = True
                End If
            ElseIf boolShutDown Then
                ftnUrgClose()
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subURGInitialize :  " & ex.ToString)
        End Try
    End Sub

    Private Sub subURGDataReceived(sender As System.Object, e As System.IO.Ports.SerialDataReceivedEventArgs) Handles comURG.DataReceived
        Try
            strUrgDataReceived &= comURG.ReadExisting()
            If boolDebugURG Then frmDebug.subWriteLogs(ftnElapsedTimeS() & " subURGDataReceived:  strUrgDataReceived =  " & strUrgDataReceived)
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subURGDataReceived :  " & ex.ToString)
        End Try
    End Sub

    Dim boolftnUrgXmitBusy As Boolean = False
    Function ftnUrgXmit(ByRef strCommand As String, ByVal intLength As Integer, ByVal intTimeOut_ms As Integer) As String
        Try
            ftnUrgXmit = ""
            If Not boolShutDown Then
                If boolDebugURG Then frmDebug.subWriteLogs(ftnElapsedTimeS() & " ftnUrgXmit:  " & strCommand)
                Dim timeOut As DateTime = Now().AddMilliseconds(150)
                While (Now() < timeOut) And boolftnUrgXmitBusy
                    subWait(10)
                End While
                boolftnUrgXmitBusy = True
                ftnUrgXmit = ""
                strUrgDataReceived = ""
                comURG.Write(strCommand & vbCrLf) ' get  data
                timeOut = Now().AddMilliseconds(intTimeOut_ms)
                Do Until ((strUrgDataReceived.StartsWith(strCommand) And (strUrgDataReceived.Length >= intLength)) Or boolShutDown Or (Now() > timeOut))
                    Thread.Sleep(10)
                Loop
                If (Now() > timeOut) Or (strUrgDataReceived.Length < intLength) Then
                    If boolDebugURG Then
                        frmDebug.subWriteLogs(" ftnUrgXmit: ERROR - strCommand  = " & strCommand)
                        frmDebug.subWriteLogs(" ftnUrgXmit: ERROR - strUrgDataReceived  = " & strUrgDataReceived)
                        If (Now() > timeOut) Then frmDebug.subWriteLogs(" ftnUrgXmit: ERROR - timeOut  /  boolURGConnected = " & boolURGConnected)
                        If (strUrgDataReceived.Length < intLength) Then frmDebug.subWriteLogs(" ftnUrgXmit: ERROR -  strUrgDataReceived.Length = " & strUrgDataReceived.Length)
                    End If
                    ftnUrgXmit = ""
                Else
                    ftnUrgXmit = strUrgDataReceived
                End If
            Else
                ftnUrgClose()
            End If
            boolftnUrgXmitBusy = False
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnUrgXmit :  " & ex.ToString)
            ftnUrgXmit = False
        End Try
    End Function




    Public Sub subUrgLaserOff()
        Try
            frmDebug.subWriteLogs(ftnElapsedTimeS() & " subUrgLaserOff:  Start")
            If comURG IsNot Nothing Then
                If boolURGConnected And Not boolShutDown And comURG.IsOpen Then
                    comURG.Write("QT" & vbLf) ' Laser off
                End If
            End If
            boolUrgLaserOn = False
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subUrgLaserOff " & ex.ToString)
        End Try
    End Sub

    Public Function ftnUrgClose() As Boolean
        Try
            If comURG IsNot Nothing Then comURG.Close()
            boolURGConnected = False
            ftnUrgClose = True
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnUrgClose :  " & ex.ToString)
            ftnUrgClose = False
        End Try
    End Function


End Module


