wschroeder
New Member
Yes, you can drive 33 servos with 256 steps of resolution with a single PIC18 (and PIC16 is possible too). Also, this program supports USART with standard baudrates up to 115K. The program presently uses RX only and you must send 33 bytes per frame. Also, there must be more than 20ms between the start of each servo update frame.
To achieve 33 outputs you must the use of the internal oscillator, which is 32MHz, 8MHz x 4PLL. There are several PIC18's that will work easily with this code, and some very inexpensive (PIC18F4510). The program is about 750 words compiled using mikroBasic 6.0, which is well under the 2K limitation of the free demo version of the compiler.
I will be adding the AutoBaud feature of the EUSART at a later time to this code. This is a great feature considering the possible variances of frequency using the internal oscillator.
I almost forgot to say what I consider a most important feature. You can use up to 5 different types of servos, such as those with different start delays. This makes this program extremely versatile for a complex servo project that requires servos from different manufacturers. At the beginning of the program you can set each port to drive a specified time frame, albeit with the assumption that 1500us is a standard for center.
Enjoy!
To achieve 33 outputs you must the use of the internal oscillator, which is 32MHz, 8MHz x 4PLL. There are several PIC18's that will work easily with this code, and some very inexpensive (PIC18F4510). The program is about 750 words compiled using mikroBasic 6.0, which is well under the 2K limitation of the free demo version of the compiler.
I will be adding the AutoBaud feature of the EUSART at a later time to this code. This is a great feature considering the possible variances of frequency using the internal oscillator.
I almost forgot to say what I consider a most important feature. You can use up to 5 different types of servos, such as those with different start delays. This makes this program extremely versatile for a complex servo project that requires servos from different manufacturers. At the beginning of the program you can set each port to drive a specified time frame, albeit with the assumption that 1500us is a standard for center.
Enjoy!
Code:
program SERVO33_USART
'***********************************************************************************
'***********************************************************************************
' SERVO_33 SINGLE PIC SERVO DRIVER WITH USART TO 115Kbaud *
' Warren Schroeder December 6, 2007 *
' Program tested using PIC18F4525 @ 32MHz (Internal Osc + PLL) *
' Source Code is mikroBasic 6.0 (Demo Version will work) *
' *
' This code will manage 33 50Hz Servos @ 8-bit resolution. Servo positions *
' can be updated with USART communication up to 115KBaud. 33 outputs are *
' available by using the internal oscillator feature of this PIC and the x4 *
' PLL feature. Only USART RX is required to use this driver program, but *
' TX is available to the user if needed. Only a single PIC is needed! *
'***********************************************************************************
'***********************************************************************************
'
' These are the port pin assignments for the servo array:
' servo[0] - PORTA.0
' servo[1] - PORTA.1
' servo[2] - PORTA.2
' servo[3] - PORTA.3
' servo[4] - PORTA.4
' servo[5] - PORTA.5
' servo[8] - PORTA.6
' servo[7] - PORTA.7
' servo[8] - PORTB.0
' servo[9] - PORTB.1
' servo[10]- PORTB.2
' servo[11]- PORTB.3
' servo[12]- PORTB.4
' servo[13]- PORTB.5
' servo[14]- PORTB.6
' servo[15]- PORTB.7
' servo[16]- PORTC.0
' servo[17]- PORTC.1
' servo[18]- PORTC.2
' servo[19]- PORTC.3
' servo[20]- PORTC.4
' servo[21]- PORTC.5
' PORTC.6 = USART TX
' PORTC.7 = USART RX
' servo[22]- PORTD.0
' servo[23]- PORTD.1
' servo[24]- PORTD.2
' servo[25]- PORTD.3
' servo[26]- PORTD.4
' servo[27]- PORTD.5
' servo[28]- PORTD.6
' servo[29]- PORTD.7
' servo[30]- PORTE.0
' servo[31]- PORTE.1
' servo[32]- PORTE.2
' PORTE.3 = MCLR or Input Only
'***********************************************************************************
symbol RXFlag = PIR1.RCIF
symbol T0Flag = INTCON.TMR0IF
symbol T1Flag = PIR1.TMR1IF
symbol T1Run = T1CON.TMR1ON
'***********************************************************************************
' Each port can be customized for initial start delay and 1/256 timing
'***********************************************************************************
const
T1_20 as word = 65536-40000+6 ' 20ms + timer1 stop compensation
portAdelay as word = 600 * 8 ' in microseconds * 8
portBdelay as word = 700 * 8
portCdelay as word = 800 * 8
portDdelay as word = 900 * 8
portEdelay as word = 1000 * 8
portAcycle as byte = (12000-portAdelay) >> 7 ' in microseconds * 8
portBcycle as byte = (12000-portBdelay) >> 7
portCcycle as byte = (12000-portCdelay) >> 7
portDcycle as byte = (12000-portDdelay) >> 7
portEcycle as byte = (12000-portEdelay) >> 7
'***********************************************************************************
' Variable Declarations
'***********************************************************************************
dim
T0 as word absolute $FD6 ' address of TMR0L
T1 as word absolute $FCE ' address of TMR1L
FSR_0 as word absolute $FE9 ' address of FSR0L
FSR_1 as word absolute $FE1 ' address of FSR1L
FSR_2 as word absolute $FD9 ' address of FSR2L
servo as byte[33] absolute $15 ' access ram & past system vars
rxbuffer as byte[33] ' usart rx array
endofbuffer as word
loops as byte
'***********************************************************************************
' Interrupt Service Routine
'***********************************************************************************
sub procedure interrupt
'**************
' PORTA
'**************
LATA = Not(LATA) ' turn on all servos on PortA (8)
loops = 0 ' 256 counts.. rollover to original value
TMR0H = Not(Hi(portAdelay))
TMR0L = Not(portAdelay) ' initial delay
T0FLag = 0 ' clear overflow flag
ASM
delayAloop:
btfsc PIR1, RCIF,0 ' check USART RX buffer
movff RCREG, POSTINC2 ' move RX byte into servobuffer
btfss INTCON, TMR0IF,0 ' check for overflow
bra delayAloop
loopAtimeout:
btfss INTCON, TMR0IF ' wait for 1/256 cycle to finsih
bra loopAtimeout
End ASM
TMR0H = Not(Hi(portAcycle))
TMR0L = Not(portAcycle) ' 1/256 cycle time
T0Flag = 0
ASM
portAmask: ' VERY efficient PWM masking routine
movlw 0
decfsz _servo+0, 1,0
iorlw 1
decfsz _servo+1, 1,0
iorlw 2
decfsz _servo+2, 1,0
iorlw 4
decfsz _servo+3, 1,0
iorlw 8
decfsz _servo+4, 1,0
iorlw 16
decfsz _servo+5, 1,0
iorlw 32
decfsz _servo+6, 1,0
iorlw 64
decfsz _servo+7, 1,0
iorlw 128
andwf LATA, 1,0
btfsc PIR1, RCIF,0 ' check USART RX buffer
movff RCREG, POSTINC2 ' move RX byte into servobuffer
decfsz _loops, 1,0 ' 256 loops yet?
bra loopAtimeout
End ASM
'**************
' PORTB
'**************
LATB = Not(LATB) ' turn on all servos on PortB (8)
loops = 0 ' 256 counts.. rollover to original value
TMR0H = Not(Hi(portBdelay))
TMR0L = Not(portBdelay) ' initial delay
T0Flag = 0 ' clear overflow flag
ASM
delayBloop:
btfsc PIR1, RCIF,0 ' check USART RX buffer
movff RCREG, POSTINC2 ' move RX byte into servobuffer
btfss INTCON, TMR0IF,0 ' check for overflow
bra delayBloop
loopBtimeout:
btfss INTCON, TMR0IF,0 ' wait for 1/256 cycle to finsih
bra loopBtimeout
End ASM
TMR0H = Not(Hi(portBcycle))
TMR0L = Not(portBcycle) ' 1/256 cycle time
T0Flag = 0
ASM
portBmask: ' VERY efficient PWM masking routine
movlw 0
decfsz _servo+8, 1,0
iorlw 1
decfsz _servo+9, 1,0
iorlw 2
decfsz _servo+10, 1,0
iorlw 4
decfsz _servo+11, 1,0
iorlw 8
decfsz _servo+12, 1,0
iorlw 16
decfsz _servo+13, 1,0
iorlw 32
decfsz _servo+14, 1,0
iorlw 64
decfsz _servo+15, 1,0
iorlw 128
andwf LATB, 1,0
btfsc PIR1, RCIF,0 ' check USART RX buffer
movff RCREG, POSTINC2 ' move RX byte into servobuffer
decfsz _loops, 1,0 ' 256 loops yet?
bra loopBtimeout
End ASM
'**************
' PORTC
'**************
LATC = Not(LATC) ' turn on all servos on PORTC (6)
loops = 0 ' 256 counts.. rollover to original value
TMR0H = Not(Hi(portCdelay))
TMR0L = Not(portCdelay) ' initial delay
T0Flag = 0 ' clear overflow flag
ASM
delayCloop:
btfsc PIR1, RCIF,0 ' check USART RX buffer
movff RCREG, POSTINC2 ' move RX byte into servobuffer
btfss INTCON, TMR0IF,0 ' check for overflow
bra delayCloop
loopCtimeout:
btfss INTCON, TMR0IF,0 ' wait for 1/256 cycle to finsih
bra loopCtimeout
End ASM
TMR0H = Not(Hi(portCcycle))
TMR0L = Not(portCcycle) ' 1/256 cycle time
T0Flag = 0
ASM
portCmask: ' VERY efficient PWM masking routine
movlw 192 ' mask out RX and TX
decfsz _servo+16, 1,0
iorlw 1
decfsz _servo+17, 1,0
iorlw 2
decfsz _servo+18, 1,0
iorlw 4
decfsz _servo+19, 1,0
iorlw 8
decfsz _servo+20, 1,0
iorlw 16
decfsz _servo+21, 1,0
iorlw 32
andwf LATC, 1,0
btfsc PIR1, RCIF,0 ' check USART RX buffer
movff RCREG, POSTINC2 ' move RX byte into servobuffer
decfsz _loops, 1,0 ' 256 loops yet?
bra loopCtimeout
End ASM
'**************
' PORTD
'**************
LATD = Not(LATD) ' turn on all servos on PortD (8)
loops = 0 ' 256 counts.. rollover to original value
TMR0H = Not(Hi(portDdelay))
TMR0L = Not(portDdelay) ' initial delay
T0Flag = 0 ' clear overflow flag
ASM
delayDloop:
btfsc PIR1, RCIF,0 ' check USART RX buffer
movff RCREG, POSTINC2 ' move RX byte into servobuffer
btfss INTCON, TMR0IF,0 ' check for overflow
bra delayDloop
loopDtimeout:
btfss INTCON, TMR0IF,0 ' wait for 1/256 cycle to finsih
bra loopDtimeout
End ASM
TMR0H = Not(Hi(portDcycle))
TMR0L = Not(portDcycle) ' 1/256 cycle time
T0Flag = 0
ASM
portDmask: ' VERY efficient PWM masking routine
movlw 0
decfsz _servo+22, 1,0
iorlw 1
decfsz _servo+23, 1,0
iorlw 2
decfsz _servo+24, 1,0
iorlw 4
decfsz _servo+25, 1,0
iorlw 8
decfsz _servo+26, 1,0
iorlw 16
decfsz _servo+27, 1,0
iorlw 32
decfsz _servo+28, 1,0
iorlw 64
decfsz _servo+29, 1,0
iorlw 128
andwf LATD, 1,0
btfsc PIR1, RCIF,0 ' check USART RX buffer
movff RCREG, POSTINC2 ' move RX byte into servobuffer
decfsz _loops, 1,0 ' 256 loops yet?
bra loopDtimeout
End ASM
'**************
' PORTE
'**************
LATE = Not(LATE) ' turn on all servos on PORTE (3)
loops = 0 ' 256 counts.. rollover to original value
TMR0H = Not(Hi(portEdelay))
TMR0L = Not(portEdelay) ' initial delay
T0Flag = 0 ' clear overflow flag
ASM
delayEloop:
btfsc PIR1, RCIF,0 ' check USART RX buffer
movff RCREG, POSTINC2 ' move RX byte into servobuffer
btfss INTCON, TMR0IF,0 ' check for overflow
bra delayEloop
loopEtimeout:
btfss INTCON, TMR0IF,0 ' wait for 1/256 cycle to finsih
bra loopEtimeout
End ASM
TMR0H = Not(Hi(portEcycle))
TMR0L = Not(portEcycle) ' 1/256 cycle time
T0Flag = 0
ASM
portEmask: ' VERY efficient PWM masking routine
movlw 0
decfsz _servo+30, 1,0
iorlw 1
decfsz _servo+31, 1,0
iorlw 2
decfsz _servo+32, 1,0
iorlw 4
andwf LATE, 1,0
btfsc PIR1, RCIF,0 ' check USART RX buffer
movff RCREG, POSTINC2 ' move RX byte into servobuffer
decfsz _loops, 1,0 ' 256 loops yet?
bra loopEtimeout
End ASM
T1Run = 0 ' stop timer1
T1 = T1 + T1_20 ' reload timer1
T1Run = 1 ' restart timer1
T1Flag = 0 ' clear interrupt flag
end sub
'***********************************************************************************
' Initialize Ports, Timers, and Other Registers
'***********************************************************************************
sub procedure general_setup
LATA = 0
LATB = 0
LATC = 0
LATD = 0
LATE = 0
TRISA = 0
TRISB = 0
TRISC = 0
TRISD = 0
TRISE = 0
ADCON1 = 15 ' disable ADC's
CMCON = 7 ' disable Comparators
endofbuffer = @rxbuffer + 33
FSR0ptr = @servo
While FSR0ptr < (@servo + 33)
POSTINC0 = 128 ' initialize all servo values to 0 position
Wend
end sub
sub procedure timers_setup
INTCON = 192 ' enable GIE & PEIE
T1CON = 32 ' prescaler=4, timer off
T1 = 0 ' clear Timer1
PIE1.TMR1IE = 1 ' enable Timer1 Interrupt
PIR1.TMR1IF = 0 ' clear Timer1 Interrupt Flag
T1CON.TMR1ON = 1 ' start Timer1
T0CON = 8 ' no prescaler
T0CON.TMR0ON = 1 ' start Timer0
end sub
'***********************************************************************************
' Main Program
'***********************************************************************************
main:
While OSCCON.OSTS = 0 ' wait for oscillator startup flag
Wend
OSCCON = $70 ' internal primary oscillator @ 8MHz
OSCTUNE.6 = 1 ' enable x4 PLL for internal oscillator
general_setup
timers_setup
USART_Init(115200) ' any baudrate up to 115K is allowed
FSR_2 = @rxbuffer
While true
While FSR_2 < endofbuffer ' FSR2 is RX servobuffer
While RXFlag = 0 ' wait for new RX byte
Wend
While TMR1H = 255 ' avoid trasfer if close to interrupt
Wend ' max possible delay is 256*.5us= 128us
If RXFlag = 1 Then
POSTINC2 = RCREG ' save RX byte in array
End If
Wend ' when 30 RX bytes move on
FSR_2 = @rxbuffer ' reset RX buffer
FSR_1 = @rxbuffer ' setup tranfer of RX array to
FSR_0 = @servo ' servo work array
While FSR_1 < endofbuffer
POSTINC0 = inc(POSTINC1) ' transfer RX buffer to work values.. add 1
Wend
Wend
end.