'commade servo code 16 et 17

Define CONF_WORD = 0x33e4
ADCON0 = %00000111
ANSEL = %00000000
WPU = %00000111

OSCCON = %01100000
OPTION_REG = %00000000
TRISIO = %11111011

AllDigital
Dim positoa As Word
Dim positob As Word

Dim positfa As Word
Dim positfb As Word

Dim posita As Word
Dim positb As Word

Dim ecart As Word
Dim pas As Word
Dim cpt As Word

Dim test As Word
Dim vara As Byte
Dim varb As Byte
Dim rega As Byte
Dim ir As Byte
Dim code As Byte
GPIO = 0

posita = 1500
positb = 1500

pas = 15
ecart = 70
positoa = 1500 - ecart
positfa = 1500 + ecart
positob = 1500 - ecart
positfb = 1500 + ecart


vara = 0
varb = 0

rega = 0
WaitMs 2000

Gosub basvershauta
WaitMs 3000
Gosub hautversbasa
WaitMs 3000
Gosub basvershauta
WaitMs 3000
Gosub hautversbasa
WaitMs 3000
debut:
ir = 0
test = 0
cpt = 0
code = 0
test = 0
'attende signal
While GPIO.5 = 1
Wend
'detection code
test = 1
WaitUs 700
If GPIO.5 = 0 Then
test = 1
cpt = cpt + 1
Else
test = 0
Endif
WaitUs 600
If GPIO.5 = 0 Then
cpt = cpt + 1
test = test
Else
test = 0
Endif
WaitUs 600
If GPIO.5 = 0 Then
test = test
cpt = cpt + 1
Else
test = 0
Endif
Break
WaitUs 600
If GPIO.5 = 1 Then
test = test
cpt = cpt + 1
Else
test = 0
Endif
Break
suite:  'si ok cpt=4
If cpt <> 4 Then
WaitMs 35
Goto debut
Endif

'If cpt = 4 Then
WaitUs 1200

If GPIO.5 = 1 Then
code.0 = 0
WaitUs 1200
Else
code.0 = 1
WaitUs 1800
Endif


If GPIO.5 = 1 Then
code.1 = 0
WaitUs 1200
Else
code.1 = 1
WaitUs 1800
Endif


If GPIO.5 = 1 Then
code.2 = 0
WaitUs 1200
Else
code.2 = 1
WaitUs 1800
Endif

If GPIO.5 = 1 Then
code.3 = 0
WaitUs 1200
Else
code.3 = 1
WaitUs 1800
Endif

If GPIO.5 = 1 Then
code.4 = 0
WaitUs 1800
Else
code.4 = 1
WaitUs 1200
Endif
'traitement code reçu************

'commande

If code = 16 Then
Gosub basvershauta
Endif


If code = 17 Then
Gosub hautversbasa
Endif


Goto debut
End                                               
basvershauta:
test = 0
positfa = 1500 + ecart  'a2
vara.2 = 0
GPIO = vara
varb.1 = 1

'Break
While posita < positfa
posita = posita + pas
For cpt = 1 To 3
vara.2 = 1
GPIO = vara
WaitUs posita
vara.2 = 0
GPIO = vara
WaitMs 19
Next cpt
Wend
positfa = posita
For cpt = 1 To 3
vara.2 = 1
GPIO = vara
WaitUs posita
vara.2 = 0
GPIO = vara
WaitMs 19
Next cpt
varb.1 = 1
vara.2 = 0
GPIO = vara

Return                                            
hautversbasa:  'a2
test = 0
positoa = 1500 - ecart
varb.1 = 0

'Break
While posita > positoa
posita = posita - pas

For cpt = 1 To 3
vara.2 = 1
GPIO = vara
WaitUs posita
vara.2 = 0
GPIO = vara
WaitMs 19
Next cpt

Wend

positoa = posita
For cpt = 1 To 3

vara.2 = 1
GPIO = vara
WaitUs posita

vara.2 = 0
GPIO = vara
WaitMs 19
Next cpt

vara.2 = 0
GPIO = vara

Return