Development Board: Dedicated 8 Channel Servo Controller

{
Dedicated Servo Controller
By Graham (Servo controller code developed by XOR)
 
Servo positional information is received in the following manner;
 
$FF,$FF,Servo(0),Servo(1),Servo(2),Servo(3),Servo(4),Servo(5),Servo(6),Servo(7),CRC
 
Note 1: Both Servo(x) and CRC are WORD type variables.
Note 2: CRC is calculated by XOR'ing all Servo positions together, for example;
 
Servo(0) = 1000
Servo(1) = 1100
Servo(2) = 1200
Servo(3) = 1300
Servo(4) = 1400
Servo(5) = 1500
Servo(6) = 1600
Servo(7) = 1700
 
     CRC = 1600
 
Note 3: Program is designed to work with an 16Mhz External Oscillator
Note 4: Ensure common ground connection between all circuits
}
 
Device = 18F2620
Clock = 16
Config MCLRE = OFF
 
Include "USART.bas"
 
// define valid servo range
Const Servo_Min = 600
Const Servo_Max = 2400
// default servo starting position
Const DefaultPos = 1500
 
Dim tmpServo(8) As Word   
Dim ServoPeriodValue As CCPR1L.asWord
Dim ServoCompareValue As CCPR2L.asword
Dim ServoOnIntEnable As PIE1.bits(2)
Dim ServoOnFlag As PIR1.bits(2)
Dim ServoOffIntEnable As PIE2.bits(0)
Dim ServoOffFlag As PIR2.bits(0)
Dim Servo(8) As Word
Dim Servo_Port As Byte
Dim n As Byte
 
// interrupt to service 8 servo channels
Interrupt ServoManager()
    Save(FSR0, FSR1)                 // context save
    If ServoOnFlag = 1 Then          // CCP1 Special Compare Event Interrupt
        LATB = Servo_Port            // Turn On Next Servo
        ServoCompareValue = Servo(n) // Active Servo Value loaded in CCPR2   
        ASM
            rlncf Servo_Port, F, 0
        End ASM
        Inc(n)
        n = n And 7       
        ServoOnFlag = 0   
    Else                             // CCP2 Normal Compare Event Interrupt
        LATB = 0                     // Turn Off Active Servo
        ServoOffFlag = 0
    EndIf 
    Restore                          // context restore
End Interrupt
 
// initialize servos and interrupt settings
Sub InitializeServos()
    Dim i As Byte
    LATB = 0                     // servo port
    TRISB = 0
    T1CON = %00100000            // prescaler = 1:4 = 1us ticks @16Mhz ; timer1 off
    TMR1L = 0                    // Timer1 starts at 0
    TMR1H = 0
    CCP1CON = %00001011          // CCP1 triggers special compare event on Timer1 match
    CCP2CON = %00001010          // CCP2 normal compare event on Timer1 match
    ServoPeriodValue = 2500      // special event interrupt every 2500us; jumps to ServoON ISR
    Servo_Port = 1               // servo outputs start at PORTB.0, then shift and rotate
    n = 0                        // start with servo(0)
    ServoOnFlag = 0
    ServoOffFlag = 0
    ServoOnIntEnable = 1
    ServoOffIntEnable = 1
    INTCON = %11000000            // enable GIE and PEIE
 
    For i = 0 To 7
        Servo(i) = DefaultPos
    Next    
 
    Enable(ServoManager)
    T1CON.0 = 1                   // start Timer1
End Sub
 
// sub that waits for the header ($FF,$FF) to be received
Sub WaitForHeader()
    Repeat
    Until USART.ReadByte = 255
    Repeat
    Until USART.ReadByte = 255
End Sub
 
// collect each servo position, and return false if non-valid data is received
Function GetServoPos() As Boolean
    Dim i As Byte
    Result = True
    For i = 0 To 7
        tmpServo(i).byte0 = USART.ReadByte
        tmpServo(i).byte1 = USART.ReadByte
        If tmpServo(i) < Servo_Min Or tmpServo(i) > Servo_Max Then
            Result = False
            Break
        EndIf
    Next 
End Function
 
// Receive the CRC, and validate data. Return false if non-valid.
Function CheckCRC() As Boolean
    Dim i As Byte
    Dim CRC As Word
    Dim tmpCRC As Word
 
    Result = True
    tmpCRC = USART.ReadWord
 
    CRC = 0
    For i = 0 To 7
        CRC = CRC Xor tmpServo(i)
    Next    
 
    If tmpCRC <> CRC Then
        Result = False
    EndIf
End Function
 
// update all 8 servo positions
Sub UpdateServos()
    Dim i As Byte
    For i = 0 To 7
        Servo(i) = tmpServo(i)
    Next  
End Sub
 
// initialize program
InitializeServos   
USART.SetBaudrate(br38400)
 
// main program loop
While True    
    WaitForHeader                             // wait for valid data header ($FF,$FF)
    If GetServoPos Then                       // attempt to receive servo positions
        If CheckCRC Then                      // attempt to validate CRC
            UpdateServos                      // if all good, then update servo positions
            USART.Write(1)                    // send complete response (1)
        Else
            USART.Write(0)                    // send error response (0)
        EndIf
    EndIf
Wend

Device = 18F2620
Clock = 40
Config OSC = HSPLL,
       MCLRE = OFF
 
Include "USART.bas"
 
Dim Servo(8) As Word,
    i As Byte,
    tmpPos As Word
 
 
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)        
End Sub
 
// start of main program
USART.SetBaudrate(br38400)
 
While True 
    // rotate all servos in one direction
    For tmpPos = 1000 To 2000
        For i = 0 To 7
            Servo(i) = tmpPos
        Next        
        SendServoPositions        
    Next 
    // rotate all servos in the other direction    
    For tmpPos = 2000 To 1000 Step -1
        For i = 0 To 7
            Servo(i) = tmpPos
        Next        
        SendServoPositions        
    Next 
Wend