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 delay(cas As Byte)
Dim i As Byte
Dim x As Byte
    Do while i <= cas
     Do While x <= 100
      Call receive()
      x=x+1
     loop
    i=i+1
        loop    
End Sub

Sub main()
        Dim motors As Byte
        Dim sensors As Byte
        Dim buttons As Byte
        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
        
        lata = 0x0E 
    Call delay(200)
        lata = 0x0D
    Call delay(200)
        lata = 0x0B
    Call delay(200)
        lata = 0x07
    Call delay(200)

        motors = 0x77

    Call delay(100)

                
Do while 1

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