' 
' Programmation du microcontrôleur PIC 16F887
' Robot Sumo  
' Lycée Louis Payen de St Paul
'
program Timer1_Interrupt
dim counter,n1,n2,compteur as word
dim i,j as word
dim  distance,distancemoy as longint
dim tableau as longint [10]
dim sens,sensTourelle as longint
dim ref as word [4]

sub procedure Interrupt()  ' Pour gérer la tourelle du SONAR
                
  PORTA.4 = 1		" On génère un signal largeur 1 à 2ms sur le PORT A4
  for i = 1 to n1
  next i
  PORTA.4 = 0
  PORTA.5 = 1           ' idem sur le PORT A5
  for i = 1 to n2
  next i
  PORTA.5 = 0
  PIR1.TMR1IF = 0            ' clear TMR1IF
  TMR1H = 0x50
  TMR1L = 0x00
end sub

sub procedure rouler
'sens: 1 avancer , -1 reculer, 2 tourner à droite ,-2 tourner à gauche
  if sens= 0 then
    PORTA.0=0
  else
    PORTA.0=1
  end if
  if sens =1 then
    PORTA.1=0
    PORTA.2=1
  end if
  if sens=-1 then
    PORTA.1=1
    PORTA.2=0
  end if
  if sens=2 then
    PORTA.1=0
    PORTA.2=0
  end if
  if sens=-2 then
    PORTA.1=1
    PORTA.2=1
  end if
end sub

sub procedure sonar
    do
    loop until (PORTB.0=0)
    do
    loop until PORTB.0=1     ' detection front montant
    distance=0
    do
      distance= distance +1
    loop until (PORTB.0=0)
end sub

sub procedure sonar2
  j=0
  for j=0 to 2
    sonar()
    tableau[j]=distance
  next j
    distancemoy=0
  for j=0 to 2
    distancemoy=distancemoy+tableau[j]
  next j
    distancemoy=distancemoy/3
end sub
sub procedure tourelle
  if n2>255 then
    sensTourelle=-20
  end if
  if n2<100 then
    sensTourelle=20
  end if
  n2=n2+sensTourelle
end sub
sub procedure test
  while TRUE
    j=0
    for j=0 to 2
      sonar()
      tableau[j]=distance
    next j
    distancemoy=0
    for j=0 to 2
      distancemoy=distancemoy+tableau[j]
    next j
    distancemoy=distancemoy/3
    if distancemoy < 150 then
       PORTC = 1
    else
       PORTC = 3
    end if
  wend
end sub

main:
  n1 = 340
  n2 = 200
  ANSEL  = 0                 ' Configure AN pins as digital I/O
  ANSELH = 0x0F
  TRISB = 255                  ' TRISB = 0 :PORTB en Sortie   ' TRISB = 255 : PORTB en entrée
  TRISA = 0                    ' Port A en sortie
  TRISC = 0                    ' Port C en Sortie
  PORTA = 0
  PORTB = 0
  PORTC = 255                  '
  
  T1CON = 1                  ' Timer1 settings
  PIR1.TMR1IF = 0            ' clear TMR1IF
  TMR1H = 0x50               ' Initialize Timer1 register
  TMR1L = 0x00
  PIE1.TMR1IE  = 1           ' enable Timer1 interrupt
  'Mise en place valeur de reference cptaer lignes
  ref[0]=ADC_Read(10)
  ref[1]=ADC_Read(8)
  ref[2]=ADC_Read(9)
  ref[3]=ADC_Read(11)
  counter =   0              ' initialize counter
  INTCON = 0xC0              ' Set GIE, PEIE : %11000000
  compteur=0
  sensTourelle=20
  
  while TRUE
    ref[2]=ADC_Read(9)
    ref[3]=ADC_Read(11)
    while (ref[3]>380) and (ref[2]>230)
      sens=1
      rouler()
      ref[2]=ADC_Read(9)
      ref[3]=ADC_Read(11)
      ref[0]=ADC_Read(10)
      ref[1]=ADC_Read(8)
      if ((ref[0]>460) or (ref[1]>400)) and (sens=1) then
        PORTA.3=1
        delay_ms(2000)
        PORTA.3=0
      end if
      compteur = compteur + 1
      if compteur >100 then
        compteur=0
        sonar2()
        if distancemoy < 150 then
          if n2>190 then
            sens=2
          end if
          if n2 < 160 then
            sens=-2
          end if
          if (n2>=160) and (n2<=190) then
            sens=1
            PORTA.3=1
          end if
          rouler()
          delay_ms(800)
          PORTA.3=0
        end if
        tourelle()
      end if

    wend
    sens=-1
    rouler()
    n2=185
    delay_ms(200)
    if ref[3]<380 then
      sens=2
    else
      sens=-2
    end if
    rouler()
    delay_ms(1200)
  wend
end.