Subversion Repositories svnkaklik

'robot

'#pragma DATA _CONFIG2H, _WDT_OFF_2H & _WDTPS_128_2H


#pragma DATA _CONFIG1H, _OSCS_OFF_1H & _HSPLL_OSC_1H
#pragma DATA _CONFIG2L, _BOR_ON_2L & _BORV_42_2L & _PWRT_ON_2L
#pragma DATA _CONFIG2H, _WDT_OFF_2H & _WDTPS_128_2H
#pragma DATA _CONFIG3H, _CCP2MX_OFF_3H
#pragma DATA _CONFIG4L, _STVR_ON_4L & _LVP_OFF_4L & _DEBUG_OFF_4L
#pragma DATA _CONFIG5L, _CP0_ON_5L & _CP1_ON_5L & _CP2_ON_5L & _CP3_ON_5L
#pragma DATA _CONFIG5H, _CPB_ON_5H & _CPD_ON_5H
#pragma DATA _CONFIG6L, _WRT0_OFF_6L & _WRT1_OFF_6L & _WRT2_OFF_6L & _WRT3_OFF_6L
#pragma DATA _CONFIG6H, _WRTC_ON_6H & _WRTB_ON_6H & _WRTD_OFF_6H
#pragma DATA _CONFIG7L, _EBTR0_OFF_7L & _EBTR1_OFF_7L & _EBTR2_OFF_7L & _EBTR3_OFF_7L
#pragma DATA _CONFIG7H, _EBTRB_ON_7H

#pragma CLOCK_FREQ 40000000

Function receive() As Byte
        If ( (rcsta.OERR = 1) ) Then
                rcsta.CREN = 0
        rcsta.CREN = 1
        End If
    Do While (pir1.RCIF = 0)
        Loop
        receive = rcreg
        
End Function

Sub send(c As Byte)
        Do while (pir1 & (1 << TXIF)) = 0 Loop
        txreg = c
End Sub

Sub main()
        Dim motors As Byte
        Dim sensors As Byte
        Dim buttons As Byte
        Dim leds As Byte
        Dim count As Long
        Dim cas As Long
        Dim i As Long

        'NASTAVENI IO
        lata=0xFF
        latb=0
        latc=0xFF
        trisa=0xF0
        trisb=0x3F
        trisc=0x9F

        'VYPNOUT ADC
        adcon1=0xC6
        adcon0=0xC0

        'UART 9600bps
        spbrg=64
        txsta = txsta & ~(1 << BRGH)
        txsta = txsta | (1 << TXEN)
        rcsta = rcsta | (1 << CREN)
        rcsta = rcsta | (1 << SPEN)
                                
        motors = 0x00
                
    do while (portb.0 = 1)
        loop

        leds = 0xF0
        count = 400000
        
        do while (count > 0)
                count = count -1
                
                if count < 300000 then
                        leds = leds | 1
                end if
                if count < 200000 then
                        leds = leds | 2
                end if
                if count < 100000 then
                        leds = leds | 4
                end if
                lata = leds
        loop
        lata = 0xFF

                
Do while 1
  cas = 0
  sensors =0xFF 
  sensors = Call receive()      'prijmi stav cidel senzoru
                
  If (!((sensors.0) and (sensors.1))) Then
    motors = 0x77
  End If
                        
  If (!(sensors.2)) Then
    If cas< 400000 Then
      cas = cas + 1
    End If
    cas = 0
    motors = 0xF7
  End If
                
  If (!(sensors.3)) Then
    If cas< 400000 Then
      cas = cas + 1
    End If
    cas = 0
    motors = 0x7F
  End If 
        
  cas = cas+1                   
  lata = sensors
  call send(motors)
                
Loop
        
End Sub