Attribute VB_Name = "ServoTest2"

''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
'    TAB Electronics Build Your Own Robot Kit BX24 Servo Test Module
'        by Dr. J (Juliano@csuChico.edu)
'        California State University, Chico
'        Intelligent Systems Lab
'        http://isl.ecst.csuchico.edu
'
''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''

Option Explicit

Private State As Byte

''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''

Public Sub Main()

    ' This is to generate a refresh rate of roughly 50 Hz.
    Const RefreshPeriod As Single = 0.05

    Dim i As Integer
    Dim C As Byte
    Dim D As Byte

    Debug.Print "Setting pins ..."

    Call SetSerialPins(SC,SD)  ' Emulate BS2 pin settings
'   Call SetSerialPins(19,20)  ' BX-24 specific settings?
    Call GetSerialPins(C ,D )
    Call InitSerialPins

    Debug.Print "C = " & CStr(C)
    Debug.Print "D = " & CStr(D)

    Debug.Print "Robot: " & CStr(GetRobotState())

    Call RobotSend(Behavior1)
    Call Delay(0.9)
    Debug.Print "Robot: " & CStr(GetRobotState())

    Debug.Print
    Debug.Print "START Servo Demo"
    Debug.Print

    Call RobotSend(RobotStop)
    Debug.Print "Robot: " & CStr(GetRobotState())

    Debug.Print "Blinkers ..."

    For i = 1 To 10
        Call RobotSend(RobotLEDon)
        Call Delay(RefreshPeriod)
    Debug.Print "Robot: " & CStr(GetRobotState())
        Call RobotSend(RobotLEDoff)
        Call Delay(RefreshPeriod)
    Debug.Print "Robot: " & CStr(GetRobotState())
    Next

    Debug.Print "Servos forward"

    For i = 1 To 25  ' For about 5 seconds.
        Call RobotSend(RobotForward)
        Call Delay(RefreshPeriod)
    Debug.Print "Robot: " & CStr(GetRobotState())
    Next

    Debug.Print "Servos backward"

    For i = 1 To 25  ' For about 5 seconds.
        Call RobotSend(RobotReverse)
        Call Delay(RefreshPeriod)
    Debug.Print "Robot: " & CStr(GetRobotState())
    Next

    Debug.Print
    Debug.Print "END Servo Demo"
    Debug.Print

End Sub

''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''

Public Function GetRobotState() As Byte

    State = RobotState
    Call RobotSendReceive(State)
    GetRobotState = State

End Function

''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''

Public Sub OldMain()

' This program exercises the BYORK servos by moving them back and forth through
' their position range, at about 1 cycle every 5 seconds.

    Const ServoPin As Byte  = 17

    Const NSteps As Integer = 100

    ' This is to generate a refresh rate of roughly 50 Hz.
    Const RefreshPeriod As Single = 0.02

    Dim i As Integer, Position As Single

    Debug.Print
    Debug.Print "BYORK Servo Demonstration"
    Debug.Print

    Debug.Print "Center servo"

    For i = 1 To 50  ' For about 1 second.
        Call MoveServo(ServoPin, 0.5)
        Call Delay(RefreshPeriod)
    Next

    Do
        Debug.Print "Forward"

        For i = 0 To NSteps

            ' Nondimensionalize position.
            Position = CSng(i) / CSng(NSteps)

            Call MoveServo(ServoPin, Position)

            Call Delay(RefreshPeriod)
        Next

        Debug.Print "Reverse"

        For i = NSteps To 0 Step -1

            ' Nondimensionalize position.
            Position = CSng(i) / CSng(NSteps)

            Call MoveServo(ServoPin, Position)

            Call Delay(RefreshPeriod)
        Next
    Loop

End Sub

''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''

Public Sub MoveServo( _
    ByVal ServoPin As Byte, _
    ByVal Position As Single)

' Moves a servo by sending a single pulse. The position is a nondimensional
' value in range 0.0 to 1.0.

    Dim PulseWidth As Single

    ' Translate position to pulse width. Resulting range is 1.0 to 2.0 ms,
    ' centered at 1.5 ms.
    PulseWidth = 0.001 + (0.001 * Position)

    ' Generate a high-going pulse on the servo pin.
    Call PulseOut(ServoPin, PulseWidth, 1)

End Sub

''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''