Robot v1

'*****************************Ustawienie Procesora i Kwarcu*********************
$regfile = "m16def.dat"
$crystal = 16000000


'*****************************Konfiguracja bluetootha***************************
Config Com1 = 9600 , Parity = None , Stopbits = 1 , Databits = 8
On Urxc Odbior
Enable Urxc


'*****************************Konfiguracja Timera*******************************
Config Timer1 = Pwm , Compare A Pwm = Clear_up , Compare B Pwm =
Clear_up , Prescale = 1
Enable Timer1
Start Timer1

Enable Interrupts


'*****************************Konfiguracja Portów*******************************
Config Portb.3 = Output
Portb.3 = 0
Config Portb.4 = Output
Portb.4 = 0

Config Portb.0 = Output
   Portb.0 = 0 1
Dioda_lewa Alias Portb.0

Config Porta.1 = Output
   Porta.1 = 0
Dioda_prawa Alias Porta.1

Config Portd.2 = Output
   Portd.2 = 0
Stby Alias Portd.2

Config Portd.3 = Output
   Portd.3 = 0
Ain1 Alias Portd.3

Config Portd.6 = Output
   Portd.6 = 0
Ain2 Alias Portd.6

Config Portd.7 = Output
   Portd.7 = 0
Bin2 Alias Portd.7

Config Portc.0 = Output
   Portc.0 = 0
Bin1 Alias Portc.0


'*****************************Funkcje*******************************************
Declare Sub Jedz(byref Kierunek As String * 1 , Byref Szybkosc As Byte)

    If Kierunek = "P" Or Kierunek = "p" Then        'Przód
     Ain1 = 0
     Bin1 = 0
     Ain2 = 1
     Bin2 = 1
     Stby = 1
   Elseif Kierunek = "T" Or Kierunek = "t" Then      'Tył
     Ain1 = 1
     Bin1 = 1
     Ain2 = 0
     Bin2 = 0
     Stby = 1
   End If

   Pwm1a = Szybkosc
   Pwm1b = Szybkosc
End Sub


'*****************************Zmienne*******************************************
'Czujnik Odległości
Dim Pomiar1 As Word
Dim Odleglosc_srodek As Single
Odleglosc_srodek = 0

'Proporcjonalność
Dim Roznica As Byte
Dim Uchyb As Single
Dim Proporcjonalnosc As Single
Odleglosc_zadana = 100
Dim Odleglosc_zadana As Byte
Proporcjonalnosc = 2.26


'*****************************Do Loop*******************************************
Jedz "P" , 0

Do

   Portb.4 = 1
     Waitus 10
   Portb.4 = 0

'Pomiar Odległości
      Pulsein Pomiar1 , Pinb , 3 , 4
      Odleglosc_srodek = Pomiar1 * 340
      Odleglosc_srodek = Odleglosc_srodek / 200             'milimetry
      Waitms 10

'Liczenie Uchybu
            Uchyb = Odleglosc_srodek - Odleglosc_zadana
            Uchyb = Uchyb * Proporcjonalnosc

'Dojeżdzanie od Przeszkody - Przód
            If Uchyb > 0 Then
               If Uchyb > 255 Then Uchyb = 255
               Jedz "P" , Uchyb
            End If

'Dojeżdzanieod Przeszkody - Tył
            If Uchyb < 0 Then
               Uchyb = Uchyb * -1
               Jedz "P" , Uchyb
            End If
Loop


'*****************************Edykiety******************************************
Odbior:
   Inputbin C , 1
Return