Device = 18F2620
Clock = 32
Config OSC = INTIO67,
       MCLRE = OFF
 
#option LCD_DATA = PORTB.0
#option LCD_RS = PORTB.4
#option LCD_EN = PORTB.5
 
Include "IntOSC8PLL.bas"
Include "USART.bas"
Include "ADC.bas"
Include "CONVERT.bas"
Include "LCD.bas"
 
// define valid servo range
Const Servo_Min = 600
Const Servo_Max = 2400
Const InitialPosition = 1000
 
Dim Servo(8) As Word,
    curServo As Word,
    curPosition As Word,
    Errors As Byte,
    TotalErrors As LongWord,
    TotalPackets As LongWord,
    ScreenRefresh As Boolean
 
// function that samples the Mode potentiometer and returns the desired decoded value
Function GetServoIndex() As Word
    Dim Segments, i As Byte
 
    Result = ADC.ReadMedian(1)     
    // split the 10 bit ADC resolution into known segments for the array
    Segments = 1023 / 9    
    // determine where the ADC result lies throughout the segments & return the desired array value
    i = 0
    Repeat        
        If Result <= ((i+1) * Segments) Then
            Break
        EndIf        
        Inc(i)
    Until i = 8
    Result = i
End Function    
 
Function GetServoPosition() As Word
    Dim tmpLongWord As LongWord
    Result = ADC.ReadMedian(0)
    tmpLongWord = Result * (Servo_Max-Servo_Min)
    Result = tmpLongWord / 1023 + Servo_Min
End Function
 
Sub RefreshLCD()
    LCD.WriteAt(1,1,"S:  E:  ")
    LCD.WriteAt(2,1,"Pos:    ")
    ScreenRefresh = False
End Sub
 
 
Sub SendServoPositions()
    Dim CRC As Word
    Dim i As Byte
 
    // send header
    USART.Write(255,255)
    // send each servo position
    For i = 0 To 7
        USART.WriteByte(Servo(i).byte0)
        USART.WriteByte(Servo(i).byte1)    
    Next
    // calculate and send CRC
    CRC = 0
    For i = 0 To 7
        CRC = CRC Xor Servo(i)
    Next
    USART.WriteWord(CRC)  
    Inc(TotalPackets)
    If TotalPackets = 1000000 Then
        TotalPackets = 0        
    EndIf    
End Sub
 
// update all 8 servo positions
Sub InitialiseServos()
    Dim i As Byte
    For i = 0 To 7
        Servo(i) = InitialPosition
    Next  
End Sub
 
Sub CheckControls()
    curServo = GetServoIndex
    If curServo  8 Then
        curPosition = GetServoPosition            
        Servo(curServo) = curPosition
    EndIf        
End Sub
 
Sub UpdateDisplay()
    If curServo  8 Then
        If ScreenRefresh Then
            RefreshLCD
        EndIf
        LCD.WriteAt(1,3,DecToStr(curServo,1))
        LCD.WriteAt(2,5,DecToStr(curPosition,4))  
        LCD.WriteAt(1,7,DecToStr(Errors,2)) 
    Else
        ScreenRefresh = True
        LCD.WriteAt(1,1,"P:",DecToStr(TotalPackets,6))
        LCD.WriteAt(2,1,"E:",DecToStr(TotalErrors,6)) 
    EndIf       
End Sub
 
Sub CheckForConfirmation()
    If Not USART.WaitForTimeout(1,5) Then
        Inc(Errors)        
        If Errors = 100 Then
            Errors = 0
        EndIf
        Inc(TotalErrors)
        If TotalErrors = 1000000 Then
            TotalErrors = 0
        EndIf
    EndIf
End Sub
 
// Enable PLL
OSCTUNE.6 = 1
 
// initialize program
USART.SetBaudrate(br38400)
ADCON1 = %00001101
ADCON2 = %10101111 
 
LCD.WriteAt(1,1,"Servo   ")
LCD.WriteAt(2,1,"  Ranger")
DelayMS(2000)
LCD.WriteAt(1,1,"By      ")
LCD.WriteAt(2,1,"  Mitchy")
DelayMS(2000)
LCD.Cls
 
InitialiseServos
Errors = 0
TotalErrors = 0
TotalPackets = 0
RefreshLCD
 
// main program loop
While True   
    CheckControls
    SendServoPositions
    CheckForConfirmation
    UpdateDisplay       
Wend