program SERVO30_USART
'**********************************************************************
'****** ******
'****** 30 Servo Driver @ 256 Positions using P18 @ 40MHz ******
'****** by W. Schroeder on July 22, 2006 ******
'****** ******
'****** REVISED on Sept 19, 2006..added USART & code tweaks ******
'****** Compiled with mikroBASIC 5.0.0.1 & Tested on 18F452 ******
'****** ******
'****** REVISED on Dec 5, 2007.. added USART for any Baud ******
'****** Compiled with mikroBASIC 6.0 & Tested on 18F452 ******
'****** ******
'****** Servo routines use only ~2.5ms of 20ms cycle ******
'****** USART at any baudrate to change servo values ******
'****** ******
'**********************************************************************
' 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.5
' servo[5]- PORTB.0
' servo[6]- PORTB.1
' servo[7]- PORTB.2
' servo[8]- PORTB.3
' servo[9]- PORTB.4
' servo[10]-PORTB.5
' servo[11]-PORTB.6
' servo[12]-PORTB.7
' servo[13]-PORTC.0
' servo[14]-PORTC.1
' servo[15]-PORTC.2
' servo[16]-PORTC.3
' servo[17]-PORTC.4
' servo[18]-PORTC.5
' servo[19]-PORTD.0
' servo[20]-PORTD.1
' servo[21]-PORTD.2
' servo[22]-PORTD.3
' servo[23]-PORTD.4
' servo[24]-PORTD.5
' servo[25]-PORTD.6
' servo[26]-PORTD.7
' servo[27]-PORTE.0
' servo[28]-PORTE.1
' servo[29]-PORTE.2
'********************************************************************
const T1_20ms as word = 65536-50000+6 ' 20ms + stop timer1 stop compensation
dim pos as byte
dim servo as byte[30]
dim servobuffer as byte[30]
dim lastservobufaddr as word
dim T1 As word absolute $FCE
sub procedure interrupt
LATA = 63 ' turn on all servos on PortA
LATB = 255 ' turn on all servos on PortB
LATC = 63 ' turn on all servos on PORTC
LATD = 255 ' turn on all servos on PortD
LATE = 7 ' turn on all servos on PORTE
pos = 0 ' 256 counts.. rollover to original value
delay_us(500) ' 0 position delay; adjust to suit needs
ASM ' 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 32
andwf LATA, 1,0
movlw 0
decfsz _servo+5, 1,0
iorlw 1
decfsz _servo+6, 1,0
iorlw 2
decfsz _servo+7, 1,0
iorlw 4
decfsz _servo+8, 1,0
iorlw 8
decfsz _servo+9, 1,0
iorlw 16
decfsz _servo+10, 1,0
iorlw 32
decfsz _servo+11, 1,0
iorlw 64
decfsz _servo+12, 1,0
iorlw 128
andwf LATB, 1,0
movlw 0
decfsz _servo+13, 1,0
iorlw 1
decfsz _servo+14, 1,0
iorlw 2
decfsz _servo+15, 1,0
iorlw 4
decfsz _servo+16, 1,0
iorlw 8
decfsz _servo+17, 1,0
iorlw 16
decfsz _servo+18, 1,0
iorlw 32
andwf LATC, 1,0
movlw 0
decfsz _servo+19, 1,0
iorlw 1
decfsz _servo+20, 1,0
iorlw 2
decfsz _servo+21, 1,0
iorlw 4
decfsz _servo+22, 1,0
iorlw 8
decfsz _servo+23, 1,0
iorlw 16
decfsz _servo+24, 1,0
iorlw 32
decfsz _servo+25, 1,0
iorlw 64
decfsz _servo+26, 1,0
iorlw 128
andwf LATD, 1,0
movlw 0
decfsz _servo+27, 1,0
iorlw 1
decfsz _servo+28, 1,0
iorlw 2
decfsz _servo+29, 1,0
iorlw 4
andwf LATE, 1,0
nop
btfsc PIR1, 5,0 ' check USART RX buffer
movff RCREG, POSTINC2 ' move RX byte into servobuffer
nop
incfsz _pos, 1,0
bra $-76 ' loop total of 20224 cycles = 2.02ms
END ASM
T1CON.TMR1ON = 0 ' stop timer1
T1 = T1 + T1_20ms ' reload timer1
T1CON.TMR1ON = 1 ' restart timer1
PIR1.TMR1IF = 0 ' clear interrupt flag
end sub
sub procedure _init
LATA = 0
LATB = 0
LATC = 0
LATD = 0
LATE = 0
TRISA = 0
TRISB = 0
TRISC = 0
TRISD = 0
TRISE = 0
ADCON1 = 7 ' disable ADC's
lastservobufaddr = @servobuffer + 30
INTCON = 192 ' enable GIE & PEIE
T1CON = 32 ' prescaler=4, timer off
T1 = 0 ' load Timer1
PIE1.TMR1IE = 1 ' enable Timer1 Interrupt
PIR1.TMR1IF = 0 ' clear Timer1 Interrupt Flag
T1CON.TMR1ON = 1 ' start Timer1
end sub
main:
_init
USART_Init(115200) ' any baudrate is permissible
FSR0ptr = @servo
While FSR0ptr < (@servo + 30) ' initialize all servo values to 0 position
POSTINC0 = 1
Wend
FSR2ptr = @servobuffer
While true
While FSR2ptr < lastservobufaddr ' FSR2 is RX servobuffer
Do
Loop Until PIR1.RCIF = 1 ' wait for new RX byte
POSTINC2 = RCREG ' load RX byte.. when 30 bytes move on
Wend
FSR2ptr = @servobuffer ' reset RX buffer
FSR1ptr = @servobuffer ' setup tranfer of RX array to
FSR0ptr = @servo ' servo work array
While FSR1ptr < lastservobufaddr
POSTINC0 = inc(POSTINC1) ' transfer RX buffer to work values.. add 1
Wend
Wend
end.