$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


Config Servos = 2 , Servo1 = Portb.0 , Servo2 = Portb.1 , Reload = 10
Config Portb = &B00111011
Config Portd = Input
Verm Alias Portb.3
Verde Alias Portb.4

Enable Interrupts

Do
    Verde = 1
    Verde = 0
    Call Para()
Loop

Sub Para()
    Servo(1) = 30
    Servo(2) = 30
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