Option Explicit

'*****
Public Const backward as byte = 5	'byork appmod pin 3 -activate with duty cycle for variable speed
public const forward as byte = 6 	'byork appmod pin 4 -activate with duty cycle for varable speed	
public const left as byte = 7		'byork appmod pin 5 -set HIGH to turn, set LOW to release
public const right as byte = 8		'byork appmod pin 6 -set HIGH to turn, set LOW to release
public const neutral as byte = 9	'byork appmod pin 7 -set LOW to use, set HIGH for autocenter of wheels

Const CmdReg  As Byte = 0        ' command register
Const GainReg As Byte = 1        ' Max Gain register
Const RngHigh As Byte = 2        ' High Range register
Const RngLow  As Byte = 3        ' LowRange register

Const Sensor1 As Byte = &He0     'Sensor 1 Hex Address

'*****





Public Sub Main()

    call putpin(25, 1)
    call putpin(26, 1)
    call sleep(0.5)
'Green LED to signal running
    call putpin(26, 0)
    call sleep(3.0)

'Code to move forward
    dim i as integer
'    Dim ExitFlag As Boolean
    Dim SensorOne As New UnsignedInteger
    Dim SDevOne As Byte   
    Dim SAddrOne As Byte
    Dim Counter As Byte

'    ExitFlag = True 
    SensorOne = 0
    SDevOne = 3
    SAddrOne = 224

    for i = 1 to 100
        call putpin(forward, 1)
        call sleep(0.0325)
        call putpin(forward, 0)
        call sleep(0.0325)

        Call I2cByteWrite(Sensor1, CmdReg, &H50)       
        Call Delay(0.065)        
        SensorOne = I2cWordRead(Sensor1, RngHigh)
        if (SensorOne < 12) then
            call PutPin(neutral, 0) 
            call PutPin(right, 1)
        else
            call PutPin(neutral, 1) 
            call PutPin(right, 0)
        end if
        if (SensorOne > 14) then
            call PutPin(neutral, 0)
            call PutPin(left, 1)
        else
            call PutPin(neutral, 0)
            call PutPin(left, 0)
        end if

    next
'stop code
'    for i = 1 to 50
'        call putpin(backward, 1)
'        call sleep(0.001)
'        call putpin(backward, 0)
'        call sleep(0.01)
'    next



'Code to work with sensor

'    Do While ExitFlag
'        Call I2cByteWrite(Sensor1, CmdReg, &H50)       
'        Call Delay(0.065)
'        SensorOne = I2cWordRead(Sensor1, RngHigh)
'        Debug.Print CStr(Counter) ;" Range = " ; CStr(SensorOne) 
'        Counter = Counter + 1
'    Loop



'Red LED to signal done
    call putpin(26, 1)
    call putpin(25, 0)

End Sub