'usa esta lib para adequar o PULSEIN para 10MHz
$lib "Mymcs.lbx"

Declare Sub Frente(byval V As Byte)
Declare Sub Para()
Declare Sub Re(byval V As Byte)
Declare Sub Direita(byval V As Byte)
Declare Sub Esquerda(byval V As Byte)

Dim I As Byte
Dim W As Word
Dim W2 As Word


'configura os servos
Config Servos = 2 , Servo1 = Portb.0 , Servo2 = Portb.1 , Reload = 10
'os pinos 0 e 1 são saída, o resto entrada
Config Portb = &B00111011
Config Portd = Input

'precisamos habilitar as interrupções para os servos
Enable Interrupts

'corpo util do programa, que realiza a funcao desejada

    Do
         Pulsein W , Pinb , 2 , 1
         Waitms 100
         Pulsein W2 , Pinb , 2 , 1
      If W < 200 And W2 < 200 Then
      Call Re(20)
      End If
      If W > 400 And W2 > 400 Then
      Call Frente(20)
      End If
      If W < 400 And W > 200 And W2 < 400 And W2 > 200 Then
      Call Para()
      End If



Loop

'funcoes usadas no programa

Sub Para()
    Servo(1) = 0
    Servo(2) = 0
End Sub

Sub Frente(v As Byte)
    Servo(1) = 30 + V
    Servo(2) = 30 - V
End Sub

Sub Re(v As Byte)
    Servo(1) = 30 - V
    Servo(2) = 30 + V
End Sub

Sub Direita(v As Byte)
    Servo(1) = 30 - V
    Servo(2) = 30 - V
End Sub

Sub Esquerda(v As Byte)
    Servo(1) = 30 + V
    Servo(2) = 30 + V
End Sub

End