Imports System.Math
Imports System.Threading
Imports System.Windows.Forms

' This code is loosely based upon MotionLib and RSlib.dll, taken from http:'web.ing.puc.cl/~asoto/IIC-3684/codes_en.htm

Module modIF_ER1_Top

    Const sngTurnFudgeFactor As Single = 60   ' 30 on carpet, 99999 on linoleum
    Const constER1FwdHysterises As Integer = 1
    Const constER1TurnHysterises As Integer = 1

    Public Function ftnER1_Command(ByRef strCommand As String) As String
        ftnER1_Command = ""
        Try
            If boolDebugER1IF Then
                frmDebug.subWriteLogs(" }}}}}}}}}}==== ")
                frmDebug.subWriteLogs(" }}}}}}}}}}====     Sent to Bot:    " & strCommand)
            End If
            Dim strParsedText As String() = strCommand.Split(" "c)
            If Not boolShutDown And (Not boolSimulatedMovement Or (strParsedText(0) = "play")) Then ' 
                Select Case strParsedText(0)
                    Case "move" : If Not boolStop Then _
                            ftnER1_Command = ftnMoveCommand(strParsedText, strCommand)
                    Case "battery" : ftnER1_Command = ftnGetBattery(strParsedText, strCommand)
                    Case "input" : If (UBound(strParsedText) > 0) Then
                            If (strParsedText(1) = "analog") Then  ' get  voltage
                                ftnER1_Command = ftnGetAnalog(strParsedText, strCommand)
                            End If
                        End If
                    Case "play" : ftnER1_Command = ftnSpeak(strCommand)
                    Case "pose" : ftnER1_Command = ftnGetPoseFromOdometryTop() ' get odometry derived pose
                    Case "moving" : ftnER1_Command = ftnMoving() ' get moving status
                    Case "bumper" : ftnER1_Command = ftnGetBumper(strParsedText, strCommand) ' get bumper switch status
                    Case "stop" : ftnER1_Command = ftnStop(strParsedText, strCommand) 'stop
                        'Case Else : ftnER1_Command = ftnER1_Serial_Communicate(strCommand, "OK")
                End Select
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnER1_Command :  " & ex.ToString)
        End Try
    End Function


    Function ftnMoving() As String
        Try
            If tmiMoveInfo.boolMoveCommandInProgress Then
                ftnMoving = boolBotConnected And tmiMoveInfo.boolBotIsMoving
            Else
                Dim bStatus0 As Byte = ftnSendER1_CommandTop(4, 0, PilotSet.GetEventStatus, 0, 0, 0, 0, 0, 0, 2) And &H1
                Dim bStatus1 As Byte = ftnSendER1_CommandTop(4, 1, PilotSet.GetEventStatus, 0, 0, 0, 0, 0, 0, 2) And &H1
                ftnMoving = (boolBotConnected And ((bStatus0 = 0) Or (bStatus1 = 0) Or tmiMoveInfo.boolMoveCommandInProgress)).ToString
            End If
        Catch ex As Exception
            ftnMoving = "False"
            frmDebug.subWriteLogs(" EXCEPTION in ftnMoving :  " & ex.ToString)
        End Try
    End Function

    Private Function ftnMoveCommand(ByRef strParsedText As String(), ByVal strCommand As String) As String
        ftnMoveCommand = "OK"
        Try
            If boolDebugDrive Then frmDebug.subWriteLogs(ftnElapsedTimeS() & " ftnMoveCommand: " & strCommand)
            Dim boolMoveCorrectionThread As Boolean = tmiMoveInfo.boolBotIsMoving
            If (tomTypeOfMovePlanned = TypeOfMove.TURN) Then
                ftnMoveTurn(strParsedText)
            ElseIf (tomTypeOfMovePlanned = TypeOfMove.FWD) Then
                ftnMoveFwd(strParsedText)
            ElseIf (tomTypeOfMovePlanned = TypeOfMove.CURVE) Then
                ftnMoveCurved()
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnMoveCommand :  " & ex.ToString)
        End Try
    End Function

    Function ftnMoveTurn(ByRef strParsedText As String()) As String
        ftnMoveTurn = "OK"
        Try
            Dim intVelocity As Integer = (4100 * sngBotAngularVelocity)
            Dim sngAngle As Single = Convert.ToSingle(Val(strParsedText(1)))
            If Not tmiMoveInfo.boolCourseCorrection Then
                If (sngAngle > 0) Then
                    sngAngle += constER1TurnHysterises + sngAngle / sngTurnFudgeFactor
                Else
                    sngAngle -= constER1TurnHysterises + sngAngle / sngTurnFudgeFactor
                End If
            End If
            Dim sngArcDistanceCm As Single = -Math.PI * sngBotWheelSeparation / 360.0 * sngAngle ' 
            Dim intSteps As Integer = (ftnCmsToSteps(sngArcDistanceCm))
            Dim intAcceleration As Integer = ER1Default.Acceleration * sngBotAngularAcceleration / 200  ' 200 = guesstimate
            subResetWheels(PLProfile.Trapezoidal, intAcceleration, intAcceleration)
            subMoveCommand(-intSteps, -intSteps, intVelocity, intVelocity)
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnMoveTurn :  " & ex.ToString)
        End Try
    End Function

    Function ftnMoveFwd(ByRef strParsedText As String()) As String
        ftnMoveFwd = "OK"
        Try
            Dim intVelocity As Integer = (18700 * sngBotLinearVelocity)
            Dim sngMoveDistance As Single = Convert.ToSingle(Val(strParsedText(1)))
            If Not tmiMoveInfo.boolCourseCorrection Then
                If (sngMoveDistance > 0) Then
                    sngMoveDistance += constER1FwdHysterises
                Else
                    sngMoveDistance -= constER1FwdHysterises
                End If
            End If
            Dim intSteps As Integer = (ftnCmsToSteps(ftnInchesToCm(sngMoveDistance)) * 80.0 / 77.0)
            Dim intAcceleration As Integer = ER1Default.Acceleration * sngBotLinearAcceleration / 70
            subResetWheels(PLProfile.Trapezoidal, intAcceleration, intAcceleration)
            subMoveCommand(-intSteps, intSteps, intVelocity, intVelocity)
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnMoveFwd :  " & ex.ToString)
        End Try
    End Function

    Function ftnMoveCurved() As String ' 
        ftnMoveCurved = "OK"
        Try
            Dim sngRadius, sngCentralAngle, sngArcLength As Single
            Dim boolClockwise As Boolean
            Dim tmoCenterOfRotation As typeMapOffset
            subDeriveCurveVariables(tbpStartingPosePlanned, tbpTargetPosePlanned, sngRadius, boolClockwise, tmoCenterOfRotation, sngCentralAngle, sngArcLength)

            Dim sngHalfBotWheelSeparationInches As Single = ftnCmToInches(sngBotWheelSeparation) / 2
            Dim sngArcLengthOuter As Single = ftnCurveArcLength(sngRadius + sngHalfBotWheelSeparationInches, sngCentralAngle) ' Outer wheel
            Dim sngArcLengthInner As Single = ftnCurveArcLength(sngRadius - sngHalfBotWheelSeparationInches, sngCentralAngle) ' Inner wheel

            Dim intStepsOuterWheel As Integer = Round(ftnCmsToSteps(ftnInchesToCm(sngArcLengthOuter)) * 80.0 / 77.0)
            Dim intStepsInnerWheel As Integer = Round(ftnCmsToSteps(ftnInchesToCm(sngArcLengthInner)) * 80.0 / 77.0)

            Dim sngVelocityFactor As Single = (sngArcLengthOuter / sngArcLengthInner - 1) / 2 + 1
            Dim sngMaxStepVelocity As Single = 18700 * sngBotLinearVelocityMax 'approx
            If (intDistanceFromWallSquaredArray(intBestParticle, tbpBotPose.Location.X, tbpBotPose.Location.Y) < intSafeDistanceFromWallSquared) Then
                sngMaxStepVelocity *= 0.75 ' too close to wall, slow down
            End If
            Dim intVelocityInnerWheel As Integer = Round(sngMaxStepVelocity / sngVelocityFactor)    ' an approximation
            Dim intVelocityOuterWheel As Integer = Round(sngMaxStepVelocity * sngVelocityFactor)  ' an approximation
            intVelocityInnerWheel = Round(intVelocityInnerWheel * Min(intVelocityOuterWheel, 1.3 * sngMaxStepVelocity) / intVelocityOuterWheel) ' Limit the max velocity
            intVelocityOuterWheel = Min(intVelocityOuterWheel, 1.3 * sngMaxStepVelocity)
            sngBotLinearVelocity = (intVelocityOuterWheel + intVelocityInnerWheel) / 18700 ' Average velocity

            Dim intOuterAcceleration As Integer
            Dim intInnerAcceleration As Integer
            If tmiMoveInfo.boolBotIsMoving Then
                intOuterAcceleration = Round(ER1Default.Acceleration * sngBotLinearAcceleration / 70 * sngVelocityFactor * 1.5)
                intInnerAcceleration = Round(ER1Default.Acceleration * sngBotLinearAcceleration / 70 / sngVelocityFactor * 1.5)
            Else
                intOuterAcceleration = Round(ER1Default.Acceleration * sngBotLinearAcceleration / 70 * sngVelocityFactor)
                intInnerAcceleration = Round(ER1Default.Acceleration * sngBotLinearAcceleration / 70 / sngVelocityFactor)
            End If
            subRecalculateBotSpeeds()
            ftnSendCommand("set power stopped " & Min(3 * intPowerStopped, 100))
            If boolClockwise Then
                subResetWheels(PLProfile.Trapezoidal, intInnerAcceleration, intOuterAcceleration)
                subMoveCommand(-intStepsOuterWheel, intStepsInnerWheel, intVelocityInnerWheel, intVelocityOuterWheel)
            Else
                subResetWheels(PLProfile.Trapezoidal, intOuterAcceleration, intInnerAcceleration)
                subMoveCommand(-intStepsInnerWheel, intStepsOuterWheel, intVelocityOuterWheel, intVelocityInnerWheel)
            End If
            sngBotLinearVelocity = sngBotLinearVelocityMax
            subRecalculateBotSpeeds()
            ftnSendCommand("set power stopped " & intPowerStopped)
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnMoveCurved :  " & ex.ToString)
        End Try
    End Function

    Public Sub subResetWheels(ByRef byteProfile As Byte, ByRef intAccelL As Integer, ByRef intAccelR As Integer)
        Try
            subResetWheel(0, byteProfile, intAccelL)
            subResetWheel(1, byteProfile, intAccelR)
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subResetWheels :  " & ex.ToString)
        End Try
    End Sub

    Sub subResetWheel(ByRef byteWheel As Byte, ByRef byteProfile As Byte, ByRef intAcceleration As Integer)  ' 
        Try
            If Not tmiMoveInfo.boolBotIsMoving Then
                ftnSendER1_CommandTop(4, byteWheel, PilotSet.Reset, 0, 0, 0, 0, 0, 0, 0)
                ftnSendER1_CommandTop(6, byteWheel, PilotSet.SetLimitSwitchMode, 0, 0, 0, 0, 0, 0, 0)  'LimitSwitch=Off
                ftnSendER1_CommandTop(6, byteWheel, PilotSet.SetProfileMode, 0, byteProfile, 0, 0, 0, 0, 0)
                ftnSendER1_CommandTop(8, byteWheel, PilotSet.SetAcceleration, ftnHiByte(ftnHiWord(intAcceleration)), ftnLoByte(ftnHiWord(intAcceleration)), ftnHiByte(ftnLoWord(intAcceleration)), ftnLoByte(ftnLoWord(intAcceleration)), 0, 0, 0)
                ftnSendER1_CommandTop(8, byteWheel, PilotSet.SetDeceleration, ftnHiByte(ftnHiWord(intAcceleration)), ftnLoByte(ftnHiWord(intAcceleration)), ftnHiByte(ftnLoWord(intAcceleration)), ftnLoByte(ftnLoWord(intAcceleration)), 0, 0, 0)
                ftnSendER1_CommandTop(8, byteWheel, PilotSet.SetJerk, &H3, &H29, &HA, &HBB, 0, 0, 0)
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subResetWheel :  " & ex.ToString)
        End Try
    End Sub

    Sub subMoveCommand(ByVal intStepsR As Integer, ByVal intStepsL As Integer, ByVal intVelocityL As Integer, ByVal intVelocityR As Integer)
        Try
            ' set distance to travel

            Dim boolCourseCorrection As Boolean = tmiMoveInfo.boolBotIsMoving
            If Not boolCourseCorrection Then
                ftnSendER1_CommandTop(6, 0, PilotSet.SetMotionCompleteMode, 0, PLMotionCompleteMode.Commanded, 0, 0, 0, 0, 0) ' motion complete when velocity ramps down to 0
                ftnSendER1_CommandTop(6, 1, PilotSet.SetMotionCompleteMode, 0, PLMotionCompleteMode.Commanded, 0, 0, 0, 0, 0)
                ftnSendER1_CommandTop(6, 0, PilotSet.ResetEventStatus, &HFF, &HFE, 0, 0, 0, 0, 0) ' Event = motion complete
                ftnSendER1_CommandTop(6, 1, PilotSet.ResetEventStatus, &HFF, &HFE, 0, 0, 0, 0, 0)
                subSetPower(intPowerMoving * 32768 / 100)
            Else
                ftnSendER1_CommandTop(8, 0, PilotSet.SetActualPostion, 0, 0, 0, 0, 0, 0, 0)
                ftnSendER1_CommandTop(8, 1, PilotSet.SetActualPostion, 0, 0, 0, 0, 0, 0, 0)
            End If

            ftnSendER1_CommandTop(8, 0, PilotSet.SetPosition, ftnHiByte(ftnHiWord(intStepsL)), ftnLoByte(ftnHiWord(intStepsL)), ftnHiByte(ftnLoWord(intStepsL)), ftnLoByte(ftnLoWord(intStepsL)), 0, 0, 0)
            ftnSendER1_CommandTop(8, 1, PilotSet.SetPosition, ftnHiByte(ftnHiWord(intStepsR)), ftnLoByte(ftnHiWord(intStepsR)), ftnHiByte(ftnLoWord(intStepsR)), ftnLoByte(ftnLoWord(intStepsR)), 0, 0, 0)
            subSetWheelVelocity(intVelocityL, intVelocityR)

            ftnSetMoveInfo(tomTypeOfMovePlanned, tbpStartingPosePlanned, tbpTargetPosePlanned, True)
            subUpdateAll()
            tbpBotPose = tbpStartingPosePlanned
            tmiMoveInfo.boolMoveCommandInProgress = False
            If Not boolCourseCorrection Then
                While tmiMoveInfo.boolBotIsMoving And Not boolShutDown And Not boolStop
                    subWait(45)
                End While
                subWait(100)
                subSetPower(intPowerStopped * 32768 / 100) ' 
                subUpdateAll()
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subMoveCommand :  " & ex.ToString)
            tmiMoveInfo.boolMoveCommandInProgress = False
        End Try
    End Sub

    Private Function ftnGetPoseFromOdometryTop() As String
        'Return pose as x y orientation 
        Try
            Dim tbpPose As typeBotPose = ftnGetPoseFromOdometry()
            ftnGetPoseFromOdometryTop = "OK " & tbpPose.Location.X.ToString & " " & tbpPose.Location.Y.ToString & " " & tbpPose.Orientation
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnGetPoseFromOdometry :  " & ex.ToString)
            ftnGetPoseFromOdometryTop = "ERROR"
        End Try
    End Function

    Private Function ftnGetPoseFromOdometry() As typeBotPose
        ftnGetPoseFromOdometry = tbpBotPose
        Try
            If Not tmiMoveInfo.boolMoveCommandInProgress Then
                Dim sngCms0, sngCms1, sngAngleTraveled, sngDistanceFromStart As Single '
                Dim tmiMoveInfoLocal As typeMoveInfo = tmiMoveInfo     'ftnGetMoveInfo()
                subGetRawOdometryER1(sngCms0, sngCms1)
                Select Case tmiMoveInfoLocal.tomTypeOfMove
                    Case TypeOfMove.TURN
                        sngAngleTraveled = ftnHysterisesCorrectedAngle((sngCms0 + sngCms1) * 180 / sngBotWheelSeparation / Math.PI)
                        ftnGetPoseFromOdometry = ftnTurnDR(tmiMoveInfoLocal.tbpStartingPose, sngAngleTraveled, False)
                    Case TypeOfMove.FWD
                        sngDistanceFromStart = ftnHysterisesCorrectedDistance(ftnCmToInches((sngCms0 - sngCms1) / 2))
                        ftnGetPoseFromOdometry = ftnFwdDR(tmiMoveInfoLocal.tbpStartingPose, sngDistanceFromStart, False)
                    Case TypeOfMove.CURVE
                        sngDistanceFromStart = ftnHysterisesCorrectedDistance(ftnCmToInches((sngCms0 - sngCms1) / 2))
                        ftnGetPoseFromOdometry = ftnCurve_DR(tmiMoveInfoLocal.tbpStartingPose, sngDistanceFromStart, tmiMoveInfoLocal.tbpTargetPose, True)
                    Case Else
                        ftnGetPoseFromOdometry = tbpBotPose
                End Select
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnGetPoseFromOdometry :  " & ex.ToString)
            ftnGetPoseFromOdometry = tbpBotPose
        End Try
    End Function

    Public Sub subGetRawOdometryER1(ByRef sngCms0 As Single, ByRef sngCms1 As Single)
        Try
            sngCms0 = ftnStepsToCms(ftnSendER1_CommandTop(4, 0, PilotSet.GetCommandedPosition, 0, 0, 0, 0, 0, 0, 4))  ' 
            If tmiMoveInfo.boolBotIsMoving Then
                If (tmiMoveInfo.tomTypeOfMove = TypeOfMove.TURN) Then
                    sngCms1 = sngCms0
                ElseIf (tmiMoveInfo.tomTypeOfMove = TypeOfMove.CURVE) Then
                    sngCms1 = ftnStepsToCms(ftnSendER1_CommandTop(4, 1, PilotSet.GetCommandedPosition, 0, 0, 0, 0, 0, 0, 4))
                Else
                    sngCms1 = -sngCms0
                End If
            Else
                sngCms1 = ftnStepsToCms(ftnSendER1_CommandTop(4, 1, PilotSet.GetCommandedPosition, 0, 0, 0, 0, 0, 0, 4))
            End If
            If boolDebugOdometry Then frmDebug.subWriteLogs(ftnElapsedTimeS() & " subGetRawOdometryER1:  sngCms0 raw       = " & sngCms0 & ":   sngCms1 raw      = " & sngCms1)
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subGetRawOdometry :  " & ex.ToString)
        End Try
    End Sub

    Public Function ftnHysterisesCorrectedDistance(ByRef sngDistanceFromStart As Single) As Single
        Try
            If tmiMoveInfo.boolCourseCorrection Then
                ftnHysterisesCorrectedDistance = sngDistanceFromStart
            Else
                If (sngDistanceFromStart > 0) Then
                    ftnHysterisesCorrectedDistance = Max(0, sngDistanceFromStart - constER1FwdHysterises)
                Else
                    ftnHysterisesCorrectedDistance = Min(0, sngDistanceFromStart + constER1FwdHysterises)
                End If
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnHysterisesCorrectedDistance :  " & ex.ToString)
        End Try
    End Function

    Public Function ftnHysterisesCorrectedAngle(ByRef sngAngleTraveled As Single) As Single
        Try
            If (Abs(sngAngleTraveled) < constER1TurnHysterises) Then ftnHysterisesCorrectedAngle = 0
            If (sngAngleTraveled > 0) Then
                ftnHysterisesCorrectedAngle = Max(0, sngAngleTraveled - constER1TurnHysterises - sngAngleTraveled / sngTurnFudgeFactor)
            Else
                ftnHysterisesCorrectedAngle = Min(0, sngAngleTraveled + constER1TurnHysterises + sngAngleTraveled / sngTurnFudgeFactor)
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnHysterisesCorrectedAngle :  " & ex.ToString)
        End Try
    End Function


    Private Function ftnGetBumper(ByRef strParsedText As String(), ByRef strCommand As String) As String
        ftnGetBumper = "7"
        Try
            If boolEnableBumperSwitches_ER1 Then
                Dim byteGetDigital As Byte = ftnSendER1_CommandTop(6, 0, PilotSet.ReadIO, 0, 0, 0, 0, 0, 0, 2) And 7
                If (byteGetDigital = 0) Then
                    ftnGetBumper = "7"  '  probably a COM error, ignore
                Else
                    ftnGetBumper = byteGetDigital.ToString
                End If
            End If
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnGetBumper")
        End Try
    End Function



    Private Function ftnStop(ByVal strParsedText As String(), ByVal strCommand As String) As String
        ftnStop = "OK"
        Try
            If boolDebugER1IF Then frmDebug.subWriteLogs(" ftnStop: ")
            Dim tmiMoveInfoLocal As typeMoveInfo = ftnGetMoveInfo()
            ftnSendER1_CommandTop(6, 0, PilotSet.SetStopMode, 0, 1, 0, 0, 0, 0, 0)
            ftnSendER1_CommandTop(6, 1, PilotSet.SetStopMode, 0, 1, 0, 0, 0, 0, 0)
            ftnSendER1_CommandTop(6, 1, PilotSet.SetMotorCommand, 0, 0, 0, 0, 0, 0, 0)
            ftnSendER1_CommandTop(6, 0, PilotSet.SetMotorCommand, 0, 0, 0, 0, 0, 0, 0)
            subSetPower(intPowerStopped * 32768 / 100)
            ftnSetMoveInfo(TypeOfMove.HALT, tbpBotPose, tbpBotPose, True)
            subUpdateAll()
            ftnSetMoveInfo(TypeOfMove.HALT, tbpBotPose, tbpBotPose, False)
            tmiMoveInfo.boolMoveCommandInProgress = False
            If tmiMoveInfoLocal.boolBotIsMoving Then subWait(1000)
            subSetPower(intPowerStopped * 32768 / 100)
            subUpdateAll()
        Catch ex As Exception
            ftnStop = ""
            tmiMoveInfo.boolMoveCommandInProgress = False
            frmDebug.subWriteLogs(" EXCEPTION in ftnStop :  " & ex.ToString)
        End Try
    End Function



    Private Function ftnGetBattery(ByVal strParsedText As String(), ByRef strCommand As String) As String
        Try
            If tmiMoveInfo.boolMoveCommandInProgress Then
                strStatusBotVoltage = "Bot: " & sngBotBatteryVoltage.ToString("N1") & "V"
            Else
                sngBotBatteryVoltage = Val(ftnSendER1_CommandTop(6, 1, PilotSet.ReadAnalog, 0, 7, 0, 0, 0, 0, 2)) / 65536.0 * 16.4
                If (sngBotBatteryVoltage = 0) Then
                    Thread.Sleep(15)   ' 
                    sngBotBatteryVoltage = Val(ftnSendER1_CommandTop(6, 1, PilotSet.ReadAnalog, 0, 7, 0, 0, 0, 0, 2)) / 65536.0 * 16.4
                End If
                If (sngBotBatteryVoltage = 0) Then
                    strStatusBotVoltage = "   ??   "
                    If boolBotConnected Then ftnER1_Command("play phrase 'No power'")
                Else
                    strStatusBotVoltage = "Bot: " & sngBotBatteryVoltage.ToString("N1") & "V"
                End If
            End If
            ftnGetBattery = strStatusBotVoltage
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnGetBattery :  " & ex.ToString)
            ftnGetBattery = ""
        End Try
    End Function


    Private Function ftnGetAnalog(ByVal strParsedText As String(), ByRef strCommand As String) As String
        ftnGetAnalog = "OK "
        Try
            Dim intInput As Byte
            For i = 2 To UBound(strParsedText)
                intInput = Val(strParsedText(i))
                If (intInput < 8) Then
                    ftnGetAnalog &= " " & ftnSendER1_CommandTop(6, 0, PilotSet.ReadAnalog, 0, intInput, 0, 0, 0, 0, 2).ToString
                Else
                    ftnGetAnalog &= " " & ftnSendER1_CommandTop(6, 1, PilotSet.ReadAnalog, 0, intInput - 8, 0, 0, 0, 0, 2).ToString
                End If
            Next
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnGetAnalog")
        End Try
    End Function


    Private Function ftnSpeak(ByVal strCommand As String) As String
        Try
            ftnSpeak = "OK"
            subSpeak(strCommand)
        Catch ex As Exception
            ftnSpeak = ""
            frmDebug.subWriteLogs(" EXCEPTION in ftnSpeak")
        End Try
    End Function

    Sub subSetWheelVelocity(ByRef intVelocityL As UInteger, ByRef intVelocityR As UInteger)
        Try
            ftnSendER1_CommandTop(8, 0, PilotSet.SetVelocity, ftnHiByte(ftnHiWord(intVelocityL)), ftnLoByte(ftnHiWord(intVelocityL)), ftnHiByte(ftnLoWord(intVelocityL)), ftnLoByte(ftnLoWord(intVelocityL)), 0, 0, 0)
            ftnSendER1_CommandTop(8, 1, PilotSet.SetVelocity, ftnHiByte(ftnHiWord(intVelocityR)), ftnLoByte(ftnHiWord(intVelocityR)), ftnHiByte(ftnLoWord(intVelocityR)), ftnLoByte(ftnLoWord(intVelocityR)), 0, 0, 0)
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subSetWheelVelocity :  " & ex.ToString)
        End Try
    End Sub

    Sub subSetPower(ByVal intPower As Integer)
        Try
            ftnSendER1_CommandTop(6, 1, PilotSet.SetMotorCommand, ftnHiByte(intPower), ftnLoByte(intPower), 0, 0, 0, 0, 0) ' ex: 13.7% = 13.7 *32,768/100 = 4,489
            ftnSendER1_CommandTop(6, 0, PilotSet.SetMotorCommand, ftnHiByte(intPower), ftnLoByte(intPower), 0, 0, 0, 0, 0)
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subSetPower :  " & ex.ToString)
        End Try
    End Sub

    Sub subUpdateAll()
        Try
            ftnSendER1_CommandTop(4, 1, PilotSet.Update, 0, 0, 0, 0, 0, 0, 0)
            ftnSendER1_CommandTop(4, 0, PilotSet.Update, 0, 0, 0, 0, 0, 0, 0)
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in subUpdateAll :  " & ex.ToString)
        End Try
    End Sub

    Function ftnGetTime(ByVal byteWheel As Byte) As UInt32    'unused
        Try
            ftnGetTime = ftnSendER1_CommandTop(4, byteWheel, PilotSet.GetTime, 0, 0, 0, 0, 0, 0, 4)
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnGetTime :  " & ex.ToString)
        End Try
    End Function



    Function ftnCmsToSteps(ByVal sngCms As Single) As Single
        Try
            ftnCmsToSteps = sngCms * ER1Metrics.CountsPerRev / ER1Metrics.WheelPer
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnCmsToSteps :  " & ex.ToString)
        End Try
    End Function

    Function ftnStepsToCms(ByVal intSteps As Integer) As Single
        Try
            ftnStepsToCms = intSteps * ER1Metrics.WheelPer / ER1Metrics.CountsPerRev
        Catch ex As Exception
            frmDebug.subWriteLogs(" EXCEPTION in ftnCmsToSteps :  " & ex.ToString)
        End Try
    End Function




    Function ftnLoByte(ByRef intValue As Integer) As Byte
        ftnLoByte = intValue And &HFF
    End Function

    Function ftnHiByte(ByRef intValue As Integer) As Byte
        ftnHiByte = intValue >> 8
    End Function

    Function ftnLoWord(ByRef intValue As UInt32) As Integer
        ftnLoWord = intValue And &HFFFF
    End Function

    Function ftnHiWord(ByRef intValue As UInt32) As Integer
        ftnHiWord = intValue >> 16
    End Function






    Public Class ER1Default
        Public Const Acceleration As Integer = &HF5
        Public Const Deceleration As Integer = &HF5
        Public Const Cycle As Integer = 204 ' Cycle time in microsecond
    End Class

    Public Class ER1Metrics
        Public Const CountsPerRev As Integer = 52000
        Public Const WheelPer As Double = 31.3
    End Class

    Public Class PLProfile
        Public Const Trapezoidal As Byte = 0
        Public Const Contouring As Byte = 1
        Public Const SCurve As Byte = 2
    End Class

    Public Class PLMotionCompleteMode
        Public Const Commanded As Integer = 0
        Public Const Actual As Integer = 1
    End Class


    Public Class PilotSet
        Public Const ClearInterrupt As Byte = &HAC
        Public Const AdjustActualPostion As Byte = &HF5
        Public Const ClearPositionError As Byte = &H47
        Public Const GetActivityStatus As Byte = &HA6
        Public Const GetActualVelocity As Byte = &HAD
        Public Const GetCaptureValue As Byte = &H36
        Public Const GetChecksum As Byte = &HF8
        Public Const GetCommandedAcceleration As Byte = &HA7
        Public Const GetCommandedPosition As Byte = &H1D
        Public Const GetCommandedVelocity As Byte = &H1E
        Public Const GetCurrentMotorCommand As Byte = &H3A
        Public Const GetDerivative As Byte = &H9B
        Public Const GetEventStatus As Byte = &H31
        Public Const GetHostIOError As Byte = &HA5
        Public Const GetIntegral As Byte = &H9A
        Public Const GetInterruptAxis As Byte = &HE1
        Public Const GetPhaseCommand As Byte = &HEA
        Public Const GetPositionError As Byte = &H99
        Public Const GetSignalStatus As Byte = &HA4
        Public Const GetTime As Byte = &H3E
        Public Const GetTraceCount As Byte = &HBB
        Public Const GetTraceStatus As Byte = &HBA
        Public Const GetVersion As Byte = &H8F
        Public Const InitializPhase As Byte = &H7A
        Public Const NoOperation As Byte = &H0
        Public Const ReadAnalog As Byte = &HEF
        Public Const ReadBuffer As Byte = &HC9
        Public Const ReadIO As Byte = &H83
        Public Const Reset As Byte = &H39
        Public Const ResetEventStatus As Byte = &H34
        Public Const SetAcceleration As Byte = &H90
        Public Const GetAcceleration As Byte = &H4C
        Public Const SetActualPostion As Byte = &H4D
        Public Const GetActualPosition = &H37
        Public Const SetActualPositionUnits As Byte = &HBE
        Public Const GetActualPositionUnits As Byte = &HBF
        Public Const SetAutoStopMode As Byte = &HD2
        Public Const GetAutoStopMode As Byte = &HD3
        Public Const SetAxisMode As Byte = &H87
        Public Const GetAxisMode As Byte = &H88
        Public Const SetAxisOutSource As Byte = &HED
        Public Const GetAxisOutSource As Byte = &HEE
        Public Const SetBreakpoint As Byte = &HD4
        Public Const GetBreakpoint As Byte = &HD5
        Public Const SetBreakpointValue As Byte = &HD6
        Public Const GetBreakpointValue As Byte = &HD7
        Public Const SetBufferFunction As Byte = &HCA
        Public Const GetBufferFunction As Byte = &HCB
        Public Const SetbufferLength As Byte = &HC2
        Public Const GetBufferLength As Byte = &HC3
        Public Const SetBufferReadIndex As Byte = &HC6
        Public Const GetBufferReadIndex As Byte = &HC7
        Public Const SetBufferStart As Byte = &HC0
        Public Const GetBufferStart As Byte = &HC1
        Public Const SetBufferWriteIndex As Byte = &HC4
        Public Const GetBufferWriteIndex As Byte = &HC5
        Public Const SetCaptureSource As Byte = &HD8
        Public Const GetCaptureSource As Byte = &HD9
        Public Const SetCommutationMode As Byte = &HE2
        Public Const GetCommutationMode As Byte = &HE3
        Public Const SetDeceleration As Byte = &H91
        Public Const GetDeceleration As Byte = &H92
        Public Const SetDerivativeTime As Byte = &H9C
        Public Const GetDerivativeTime As Byte = &H9D
        Public Const SetDiagnosticPortMode As Byte = &H89
        Public Const GetDiagnosticPortMode As Byte = &H8A
        Public Const SetEncoderModulus As Byte = &H8D
        Public Const GetEncoderModulus As Byte = &H8E
        Public Const SetEncoderSource As Byte = &HDA
        Public Const GetEncoderSource As Byte = &HDB
        Public Const SetEncoderToStepRatio As Byte = &HDE
        Public Const GetEncoderToStepRatio As Byte = &HDF
        Public Const SetIntegrationLimit As Byte = &H95
        Public Const GetIntegrationLimit As Byte = &H96
        Public Const SetInterruptMask As Byte = &H2F
        Public Const GetInterruptMask As Byte = &H56
        Public Const SetJerk As Byte = &H13
        Public Const GetJerk As Byte = &H58
        Public Const SetKaff As Byte = &H93
        Public Const GetKaff As Byte = &H94
        Public Const SetKd As Byte = &H27
        Public Const GetKd As Byte = &H52
        Public Const SetKi As Byte = &H26
        Public Const GetKi As Byte = &H51
        Public Const SetKout As Byte = &H9E
        Public Const GetKout As Byte = &H9F
        Public Const SetKp As Byte = &H25
        Public Const GetKp As Byte = &H50
        Public Const SetKvff As Byte = &H2B
        Public Const GetKvff As Byte = &H54
        Public Const SetLimitSwitchMode As Byte = &H80
        Public Const GetLimitSwitchMode As Byte = &H81
        Public Const SetMotionCompleteMode As Byte = &HEB
        Public Const GetMotionCompleteMode As Byte = &HEC
        Public Const SetMotorBias As Byte = &HF
        Public Const GetMotorBias As Byte = &H2D
        Public Const SetMotorCommand As Byte = &H77
        Public Const GetMotorCommand As Byte = &H69
        Public Const SetMotorLimit As Byte = &H6
        Public Const GetMotorLimit As Byte = &H7
        Public Const SetMotorMode As Byte = &HDC
        Public Const GetMotorMode As Byte = &HDD
        Public Const SetNumberPhases As Byte = &H85
        Public Const GetNumberPhases As Byte = &H86
        Public Const SetOutputMode As Byte = &HE0
        Public Const GetOutputMode As Byte = &H6E
        Public Const SetPhaseAngle As Byte = &H84
        Public Const GetPhaseAngle As Byte = &H2C
        Public Const SetPhaseCorrectionMode As Byte = &HE8
        Public Const GetPhaseCorrectionMode As Byte = &HE9
        Public Const SetPhaseCounts As Byte = &H75
        Public Const GetPhaseCounts As Byte = &H7D
        Public Const SetPhaseInitializeMode As Byte = &HE4
        Public Const GetPhaseInitializeMode As Byte = &HE5
        Public Const SetPhaseInitializeTime As Byte = &H72
        Public Const GetPhaseInitializeTime As Byte = &H7C
        Public Const SetPhaseOffset As Byte = &H76
        Public Const GetPhaseOffset As Byte = &H7B
        Public Const SetPhasePrescale As Byte = &HE6
        Public Const GetPhasePrescale As Byte = &HE7
        Public Const SetPosition As Byte = &H10
        Public Const GetPosition As Byte = &H4A
        Public Const SetPostionErrorLimit As Byte = &H97
        Public Const GetPostionErrorLimit As Byte = &H98
        Public Const SetProfileMode As Byte = &HA0
        Public Const GetProfileMode As Byte = &HA1
        Public Const SetSampleTime As Byte = &H38
        Public Const GetSampleTime As Byte = &H61
        Public Const SetSerialPortMode As Byte = &H8B
        Public Const GetSerialPortMode As Byte = &H8C
        Public Const SetSettleTime As Byte = &HAA
        Public Const GetSettleTime As Byte = &HAB
        Public Const SetSettleWindow As Byte = &HBC
        Public Const GetSettleWindow As Byte = &HBD
        Public Const SetSignalSense As Byte = &HA2
        Public Const GetSignalSense As Byte = &HA3
        Public Const SetStartMode As Byte = &HCC
        Public Const GetStartMode As Byte = &HCD
        Public Const SetStartVelocity As Byte = &H6A
        Public Const GetStartVelocity As Byte = &H6B
        Public Const SetStopMode As Byte = &HD0
        Public Const GetStopMode As Byte = &HD1
        Public Const SetSynchronizationMode As Byte = &HF2
        Public Const GetSynchronizationMode As Byte = &HF3
        Public Const SetTraceMode As Byte = &HB0
        Public Const GetTraceMode As Byte = &HB1
        Public Const SetTracePeriod As Byte = &HB8
        Public Const GetTracePeriod As Byte = &HB9
        Public Const SetTraceStart As Byte = &HB2
        Public Const GetTraceStart As Byte = &HB3
        Public Const SetTraceStop As Byte = &HB4
        Public Const GetTraceStop As Byte = &HB5
        Public Const SetTraceVariable As Byte = &HB6
        Public Const GetTraceVariable As Byte = &HB7
        Public Const SetTrackingWindow As Byte = &HA8
        Public Const GetTrackingWindow As Byte = &HA9
        Public Const SetVelocity As Byte = &H11
        Public Const GetVelocity As Byte = &H4B
        Public Const Update As Byte = &H1A
        Public Const WriteBuffer As Byte = &HC8
        Public Const WriteIO As Byte = &H82
    End Class




End Module
