'
' LEGO Interface library
'
' All lego sesors connected to PORTA, which is the
' ADC input port.
'
'
Declare Function Readport(port As Byte) As Integer
Declare Sub Powerport(port As Byte)

'
Dim Enc_tmp As Byte , Light_tmp As Integer
'
' Initialize the ADC for fast conversion. If accuracy is important in your application
' don't call this routine and set up the ADC per Atmel specifications.  Each
' LEGO sensor function will take a little longer.
'
Sub Initlego()
   ' Note: atmel doesn't recommend running the ADC this fast
   ' but as far as I can tell it works fine.  The results will be noiser
   ' but only by 2-4 counts.
   Config Adc = Single , Prescaler = 8
   Start Adc
End Sub
'
' Read and decode the LEGO rotation sensor.
'
' The speed at which the encoder can be read is determined
' by the call rate.  LEGO specifies the encoder as capable of
' 300 rpm (4800 transitions/min or 80/sec) so call this routine
' at least 100 times/second.
'
' Parameters: Port bit #, Previous state, Encoder value
' Returns: Nothing
' Modifies: Previous state and Encoder Value
'
Sub Dolegoencoder(byval Port As Byte , State As Byte , Encoder As Integer)
   Light_tmp = Readport(port)
   Select Case Light_tmp
   Case Is > 850 :
         Enc_tmp = 2
         If State = 1 Then Goto Inc_encoder
         If State = 3 Then Goto Dec_encoder
   Case Is > 700 :
         Enc_tmp = 1
         If State = 0 Then Goto Inc_encoder
         If State = 2 Then Goto Dec_encoder
   Case Is > 450 :
         Enc_tmp = 3
         If State = 2 Then Goto Inc_encoder
         If State = 0 Then Goto Dec_encoder
   Case Else :
         Enc_tmp = 0
         If State = 3 Then Goto Inc_encoder
         If State = 1 Then Goto Dec_encoder
   End Select
   Goto No_encoder
Inc_encoder:
   Encoder = Encoder + 2
Dec_encoder:
   Encoder = Encoder - 1
No_encoder:
   State = Enc_tmp
   Call Powerport(port)
End Sub

'
' Read a LEGO light sensor.
'
' The raw output varies from ~600 to ~900, depending.  I clip the
' output and shift it & invert so low numbers == dark and high numbers
' equal light.  If you parallel a switch on this port a switch closure
' will read as 0.  If only a switch is on this port, it will read 20 (open)
' and 0 (closed)
'
' Parameters: Port Bit Value
' Returns: Cooked Sensor Reading
' Modifies: nothing (but port i/o values)
'
Function Light(byval Port As Byte) As Byte
   Light_tmp = Readport(port)
   If Light_tmp < 200 Then
      Light = 0
   Elseif Light_tmp < 620 Then
      Light = 255
   Elseif Light_tmp > 855 Then
      Light = 20
   Else
      Light = 875 - Light_tmp
   End If
   Call Powerport(port)
End Function

'
' Read LEGO switch
'
' functionally redundant with Light(), but
' consumes less power as the outputs are not driven.
' NB - calling this will mess up logic for Light().  Use
' one or the other but not both.  This routine returns
' either a 0 or a 1 (closed, open, respectively)
'
Function Switch(byval Port As Byte) As Byte
   Light_tmp = Readport(port)
   Switch = Pina.port
End Function
'
' Restore power to a port after reading it.
'
Sub Powerport(port As Byte)
   Porta.port = 1
   Ddra.port = 1
End Sub
'
' In order to keep code size to a minimum I put all common
' functions into this routine as I could.
'
Function Readport(port As Byte) As Integer
   Ddra.port = 0
   Porta.port = 0
   Waitus 10
   Readport = Getadc(port)
End Function

'end program