#include "p16f15323.inc"
; Config BITS
;#######################################################################################################################
;#######################################################################################################################
__CONFIG _CONFIG1, _FEXTOSC_OFF & _RSTOSC_HFINT32 & _CLKOUTEN_OFF & _CSWEN_ON & _FCMEN_ON
__CONFIG _CONFIG2, _MCLRE_OFF & _PWRTE_OFF & _LPBOREN_OFF & _BOREN_ON & _BORV_LO & _ZCD_OFF & _PPS1WAY_OFF & _STVREN_ON
__CONFIG _CONFIG3, _WDTCPS_WDTCPS_31 & _WDTE_OFF & _WDTCWS_WDTCWS_7 & _WDTCCS_SC
__CONFIG _CONFIG4, _BBSIZE_BB512 & _BBEN_OFF & _SAFEN_OFF & _WRTAPP_OFF & _WRTB_OFF & _WRTC_OFF & _WRTSAF_OFF & _LVP_ON
__CONFIG _CONFIG5, _CP_ON
; Servo I/O Pin definitions
;#######################################################################################################################
;#######################################################################################################################
#define Servo_0 LATC, RC0
#define Servo_1 LATC, RC1
#define Servo_2 LATC, RC2
#define Servo_3 LATC, RC3
#define Servo_4 LATC, RC4
#define Servo_5 LATC, RC5
#define Servo_6 LATA, RA0
#define Servo_7 LATA, RA1
; Variable Definitions
;#######################################################################################################################
;#######################################################################################################################
ServoIndex equ h'40'
ServoTempH equ h'41'
ServoTempL equ h'42'
; Interrupt and Program Vector
;#######################################################################################################################
;#######################################################################################################################
RES_VECT CODE 0x0000 ; processor reset vector
GOTO Initial_START ; go to beginning of program
ISR CODE 0x0004 ; interrupt vector location
; <Search the device datasheet for 'context' and copy interrupt
; context saving code here. Older devices need context saving code,
; but newer devices like the 16F#### don't need context saving code.>
;
; START Interrupt Program
;#######################################################################################################################
;#######################################################################################################################
banksel PIR6
btfss PIR6, 0
goto PeriodInterval
PulseInterval:
banksel PIR6
bcf PIR6, 0
banksel ServoIndex
movf ServoIndex,W
banksel PCL
addwf PCL,F
goto Handle_SERVO_OFF_0
goto Handle_SERVO_OFF_1
goto Handle_SERVO_OFF_2
goto Handle_SERVO_OFF_3
goto Handle_SERVO_OFF_4
goto Handle_SERVO_OFF_5
goto Handle_SERVO_OFF_6
goto Handle_SERVO_OFF_7
Handle_SERVO_OFF_0:
banksel LATC
bcf Servo_0
Handle_SERVO_OFF_1:
banksel LATC
bcf Servo_1
Handle_SERVO_OFF_2:
banksel LATC
bcf Servo_2
Handle_SERVO_OFF_3:
banksel LATC
bcf Servo_3
Handle_SERVO_OFF_4:
banksel LATC
bcf Servo_4
Handle_SERVO_OFF_5:
banksel LATC
bcf Servo_5
Handle_SERVO_OFF_6:
banksel LATA
bcf Servo_6
Handle_SERVO_OFF_7:
banksel LATA
bcf Servo_7
goto Interrupt_Done ; Should never reach this line
PeriodInterval:
banksel PIR4
btfss PIR4, 0
goto Interrupt_Done
bcf PIR4, 0
banksel T1CON ; Dissable Timer 1
bcf T1CON, 0
banksel TMR1H ; Reset Timer 1 Period Value
movlw HIGH TMR1_Period_value
movwf TMR1H
banksel TMR1L
movlw LOW TMR1_Period_value
movwf TMR1L
banksel T1CON ; Enable Timer 1
bsf T1CON, 0
;...............................................................................
banksel ServoIndex ; Increment ServoIndex and keep range from 0 to 7
incf ServoIndex,W
andlw b'00000111'
movwf ServoIndex
banksel ServoIndex
movf ServoIndex,W
banksel PCL
addwf PCL,F
goto Handle_SERVO_ON_0
goto Handle_SERVO_ON_1
goto Handle_SERVO_ON_2
goto Handle_SERVO_ON_3
goto Handle_SERVO_ON_4
goto Handle_SERVO_ON_5
goto Handle_SERVO_ON_6
goto Handle_SERVO_ON_7
Handle_SERVO_ON_0:
banksel LATC ; Set Servo 0 I/O HIGH
bsf Servo_0
movlw HIGH Servo0_value ; Load Servo 0 Time ON Data
banksel ServoTempH
movwf ServoTempH
movlw LOW Servo0_value
banksel ServoTempL
movwf ServoTempL
call Process_Data
goto Interrupt_Done
Handle_SERVO_ON_1:
banksel LATC ; Set Servo 1 I/O HIGH
bsf Servo_1
movlw HIGH Servo1_value ; Load Servo 1 Time ON Data
banksel ServoTempH
movwf ServoTempH
movlw LOW Servo1_value
banksel ServoTempL
movwf ServoTempL
call Process_Data
goto Interrupt_Done
Handle_SERVO_ON_2:
banksel LATC ; Set Servo 2 I/O HIGH
bsf Servo_2
movlw HIGH Servo2_value ; Load Servo 2 Time ON Data
banksel ServoTempH
movwf ServoTempH
movlw LOW Servo2_value
banksel ServoTempL
movwf ServoTempL
call Process_Data
goto Interrupt_Done
Handle_SERVO_ON_3:
banksel LATC ; Set Servo 3 I/O HIGH
bsf Servo_3
movlw HIGH Servo3_value ; Load Servo 3 Time ON Data
banksel ServoTempH
movwf ServoTempH
movlw LOW Servo3_value
banksel ServoTempL
movwf ServoTempL
call Process_Data
goto Interrupt_Done
Handle_SERVO_ON_4:
banksel LATC ; Set Servo 4 I/O HIGH
bsf Servo_4
movlw HIGH Servo4_value ; Load Servo 4 Time ON Data
banksel ServoTempH
movwf ServoTempH
movlw LOW Servo4_value
banksel ServoTempL
movwf ServoTempL
call Process_Data
goto Interrupt_Done
Handle_SERVO_ON_5:
banksel LATC ; Set Servo 5 I/O HIGH
bsf Servo_5
movlw HIGH Servo5_value ; Load Servo 5 Time ON Data
banksel ServoTempH
movwf ServoTempH
movlw LOW Servo5_value
banksel ServoTempL
movwf ServoTempL
call Process_Data
goto Interrupt_Done
Handle_SERVO_ON_6:
banksel LATA ; Set Servo 6 I/O HIGH
bsf Servo_6
movlw HIGH Servo6_value ; Load Servo 6 Time ON Data
banksel ServoTempH
movwf ServoTempH
movlw LOW Servo6_value
banksel ServoTempL
movwf ServoTempL
call Process_Data
goto Interrupt_Done
Handle_SERVO_ON_7:
banksel LATA ; Set Servo 7 I/O HIGH
bsf Servo_7
movlw HIGH Servo7_value ; Load Servo 7 Time ON Data
banksel ServoTempH
movwf ServoTempH
movlw LOW Servo7_value
banksel ServoTempL
movwf ServoTempL
call Process_Data
goto Interrupt_Done
;banksel CCPR1H ; Set Compare 1 Value
;movlw h'00'
;movwf CCPR1H
;banksel CCPR1L
;movlw h'00'
;movwf CCPR1L
Interrupt_Done:
RETFIE
Process_Data:
banksel ServoTempL ; Multiply Servo Time by 8
rlf ServoTempL,F
banksel ServoTempH ; Note: your mileage may vary here...
rlf ServoTempH,F ; I used a processor running at 32MHz.
banksel ServoTempL ; That means that the instruction time was 125ns
rlf ServoTempL,F ; To get 1 micro second Resolution I multiplied
banksel ServoTempH ; 125ns by 8 which equals 1 micro second.
rlf ServoTempH,F ; So a processor running at 8MHz could still
banksel ServoTempL ; achieve a 1 microsecond resolution by omitting
rlf ServoTempL,F ; this section.
banksel ServoTempH
rlf ServoTempH,F
banksel ServoTempL
movf ServoTempL,W
andlw b'11111000'
movwf ServoTempL
movlw LOW TMR1_Period_value ; Add TMR1_Period_value to ServoTemp
banksel ServoTempL
addwf ServoTempL,F
btfsc STATUS,C
banksel ServoTempH
incf ServoTempH,F
movlw HIGH TMR1_Period_value
banksel ServoTempH
addwf ServoTempH,F
banksel ServoTempL ; Set Compare Value
movf ServoTempL,W
banksel CCPR1L
movwf CCPR1L
banksel ServoTempH
movf ServoTempH,W
banksel CCPR1H
movwf CCPR1H
return
MAIN_PROG CODE ; let linker place main program
Initial_START:
; START Main Program Initialization - Define PORTs
;#######################################################################################################################
;#######################################################################################################################
banksel LATA ; Preset PORTA to all LOW
clrf LATA
banksel TRISA
movlw b'11111100'
movwf TRISA ; DEFINE ALL pins as INPUT
banksel LATC ; Preset PORTA to all LOW
clrf LATC
banksel TRISC
movlw b'11000000'
movwf TRISC ; DEFINE RC0 to RC5 as OUTPUT
; START Main Program Initialization - Define Analog vs Digital operation Mode
;#######################################################################################################################
;#######################################################################################################################
banksel ANSELA
movlw b'00000000'
; XX...... Unimplemented
; ..XX.... RA5 RA4 anaolg or digital select
; ....X... Unimplemented
; .....XXX RA2 RA1 RA0 anaolg or digital select
movwf ANSELA
banksel ANSELC
movlw b'00000000'
; XX...... Unimplemented
; ..XXXXXX RC5 RC0 anaolg or digital select
movwf ANSELC
; TIMER and Interrupt Setup
;#######################################################################################################################
;#######################################################################################################################
banksel INTCON
movlw b'11000000' ;DEBUG DISABLE
; X....... Global Interrupt Enable
; .X...... Peripheral Interrupt Enable
; ..XXXXX. Unimplemented
; .......X Interrupt Edge Select
movwf INTCON
banksel T1CON
movlw b'00000011'
; XX...... Unimplemented: Read as '0'
; ..XX.... CKPS<1:0>: Timer1 Input Clock Prescale Select bits
; ....X... Unimplemented: Read as '0'
; .....X.. SYNC: Timer1 Synchronization Control bit
; ......X. RD16: 16-bit Read/Write Mode Enable bit
; .......X ON: Timer1 On bit
movwf T1CON
banksel T1CLK
movlw b'00000001'
; XXXX.... Unimplemented: Read as '0'
; ....XXXX CS<3:0>: Timer1 Clock Select bits
movwf T1CLK
banksel PIE4
movlw b'00000001'
; XXXXXX.. Unimplemented: Read as '0'
; ......X. TMR2 to PR2 Match Interrupt Enable
; .......X Timer1 Interrupt Enable
movwf PIE4
; Compare Setup
;#######################################################################################################################
;#######################################################################################################################
banksel CCP1CON
movlw b'10000010'
; X....... EN: CCPx Module Enable bit
; .X...... Unimplemented: Read as '0'
; ..X..... OUT: CCPx Output Data bit (read-only)
; ...X.... FMT: CCPW (Pulse Width) Alignment bit
; ....XXXX MODE<3:0>: CCPx Mode Select bits(1)
movwf CCP1CON
banksel PIE6
movlw b'00000001'
; XXXXXX.. Unimplemented: Read as '0'
; ......X. CCP2IE: CCP2 Interrupt Enable bit
; .......X CCP1IE: CCP1 Interrupt Enable bit
movwf PIE6
; How to set Period interval
;#######################################################################################################################
;#######################################################################################################################
; TMR1_Period_value = 65535 - ( ( Period * Fosc/4 ) / PreScale )
;
; Period ... Normal 20ms Period for ONE channel is 0.02 ... divide .02 by 8 for 8 Channels to obtain 0.0025 Period Value
;
; TMR1_Period_value = 65535 - ( ( 0.0025 * 32000000/4 ) / 1 )
;
; TMR1_Period_value = 65535 - ( ( 0.0025 * 8000000 ) / 1 )
;
; TMR1_Period_value = 65535 - ( 20000 / 1 )
;
; TMR1_Period_value = 45535
TMR1_Period_value equ d'45535'
; Note: Due to overhead requirements, running at 32MHz, the shortest pulse is 133 us and the longest pulse is 2495 us
Servo0_value equ d'500' ;micro seconds
Servo1_value equ d'787' ;micro seconds
Servo2_value equ d'1071' ;micro seconds
Servo3_value equ d'1357' ;micro seconds
Servo4_value equ d'1643' ;micro seconds
Servo5_value equ d'1929' ;micro seconds
Servo6_value equ d'2214' ;micro seconds
Servo7_value equ d'2495' ;micro seconds <--- Note cannot exceed Period Value PLUS some overhead
Main_Program:
; START Main Program
;#######################################################################################################################
;#######################################################################################################################
GOTO Main_Program
END