include "P16F628A.INC" ;include the defaults for the chip
list p=16f628a
__config (_CP_OFF & _CPD_OFF & _PWRTE_OFF & _WDT_OFF & _INTRC_OSC_NOCLKOUT & _LVP_OFF & _BOREN_OFF & _MCLRE_OFF)
; pin out list
; PORTA RA
; 0 comparator input - right line sensor
; 1 comparator input - left line sensor
; 2 comparator input - reference voltage
; 3 motor driver output A - right motor
; 4 motor driver output A - right motor
; - open collector - needs pull up resistor to 5V to go high
; 5 unused - input only
; 6 motor driver output B - left motor
; 7 motor driver output B - left motor
; PORTB RB
; 0 IR detector input - front - IR detectors go low on signal, normally 5V
; 1 unused
; 2 status led - blinks while code in main loop
; 3 digital output, PWM
; 4 IR detector input - left front - IR detectors go low on signal, normally 5V
; 5 IR detector input - right front
; 6 IR detector input - left side
; 7 IR detector input - right side
; general variables
cblock 0x20
count ; used in delay routine
count1 ; used in delay routine
counta ; used in delay routine
countb ; used in delay routine
compcount ; used in comparator compare
endc
#define W_TEMP 0x70 ; common memory area across all banks
#define STATUS_TEMP 0x71 ; temp vars for ISR
org 0x00
goto init
;
; ISR code
;
org 0x04
movwf W_TEMP ; Save W & STATUS, etc
swapf STATUS, W
bcf STATUS, RP0
movwf STATUS_TEMP ; end ISR save bits
btfsc PIR1, CMIF
call check_comp ;check comparators - line detection
btfsc INTCON,INTF
goto check_front_IR ;check single front IR detector, skip other IR decectors
btfsc INTCON,RBIF
call check_IR ;check other IR detectors
isr_exit
swapf STATUS_TEMP, W ; restore W & STATUS
movwf STATUS
swapf W_TEMP,F
swapf W_TEMP,W
retfie ; end ISR restore bits
; end ISR code
init
; init steps are
; setup PORTA and PORTB pins as input/output
; halt motors
; delay 5 seconds
; setup comparator mode
; setup ir detectors - done as enable interrupt
; setup rear ir sensor
; setup 38khz, 50% duty cycle PWM
; enable interrupts
banksel TRISA ; set all pins in appropriate input/output
movlw b'00100111' ; RA5 always input
movwf TRISA ; set PORTA pins
movlw b'11110001'
movwf TRISB ; set PORTB pins
call stop ; motors halt
banksel PORTB
bsf PORTB,2 ; turn on test led
call Long_Delay ; delay 5-6 secs
banksel CMCON ; init comparator mode
movlw 0x03
movwf CMCON ; CM<2:0> = 011
call Delay10 ; delay 10 microseconds
movf CMCON,F ; read cmcon to end change condition wrt interrupt setup
bcf PIR1, CMIF ; clear pending interrupts
banksel PIE1 ; select bank 1
bsf PIE1, CMIE ; enable comparator interrupts
; setup and run 38khz pwm
banksel PR2 ; set pwm period
movlw 0x19 ; 0x19 == 25 decimal. So via equation
movwf PR2 ; 25+1 * 4 * 0.25 * 1 = 26 and 1/26 == 38khz ish
; set pwm duty cycle to 50%
banksel CCP1CON ; so need duty cycle to be half pwm period
; ie half (25+1)*4 -> 52 = 0x34
movlw 0x0c ; since Tosc is in both pwm and duty cycle equation they cancel out
movwf CCP1CON ; remember split across bits 5/4 of ccp1con and ccpr1l
movlw 0x0d ; 0x 0 d 0
movwf CCPR1L ; duty cycle 0x34 - ie 52 decimal
banksel T2CON ; set tmr2 prescaler
movlw 0x00
movwf T2CON
bsf T2CON, TMR2ON ; turn on timer 2
banksel INTCON ; enable interrupts
bsf INTCON, RBIE ; enable RB port interrupts (RB4-7)
bsf INTCON, INTE ; enable RB0 port interrupt
bsf INTCON, PEIE ; enable peripheral interrupts
bsf INTCON, GIE ; enable global interrupts
main
nop
banksel PORTB
bcf PORTB,2 ; turn off status led
call forward
goto main
;
; check front IR detector, go forward if something there
;
check_front_IR
call Delay20 ; wait for signal to settle
banksel PORTB
btfss PORTB,0 ; check RB0, low means we detected something
call forward ; move ahead
banksel INTCON
bcf INTCON,INTF ; clear RB0 interrupt flag (maybe clear other IR interrupt flag?)
goto isr_exit
; end check_front_IR
;
; check what IR detetor went off and change motors as needed
; low means we detected something
;
check_IR
call Delay20 ; wait for signals to steady
banksel PORTB
btfss PORTB,6 ; check right side
goto turnright_IR ; turn right as something was there
btfss PORTB,7 ; check left side
goto turnleft_IR ; turn left as something was there
btfss PORTB,5 ; check left front
call frontleft_IR
btfss PORTB,4 ; check right front
call frontright_IR
checkirexit
banksel INTCON
bcf INTCON,RBIF ; clear interrupt flag
return
turnright_IR
call fastright
goto checkirexit
turnleft_IR
call fastleft
goto checkirexit
frontright_IR
call slowright
goto checkirexit
frontleft_IR
call slowleft
goto checkirexit
; end check_IR
;
; check comparators function (line sensors)
; reverse if both sensors are active
check_comp
call Delay20 ; wait 20ms for input to get stable
banksel CMCON
btfsc CMCON,C1OUT ; if c1out is 1
bsf compcount,0 ; got a right sensor reading
btfsc CMCON,C2OUT ; if c2out is 1
bsf compcount,1 ; got a left sensor
movlw d'3' ; compcount is 0000 0011 if both sensors active ie 3
subwf compcount,0
btfsc STATUS,Z ; was the result 0?
goto comp_reverse ; both sensors active, reverse, otherwise check again
btfsc CMCON,C1OUT ; right sensor active
goto comp_turnleft ; turn right
btfsc CMCON,C2OUT ; left sensor active
goto comp_turnright ; turn left
compexit
bcf PIR1, CMIF ; clear pending interrupts
return
comp_reverse
call reverse ; both sensors active, reverse
call fastleft
goto compexit
comp_turnleft
call fastleft
goto compexit
comp_turnright
call fastright
goto compexit
; end check_comp
; motor drive functions
;
; halt both motors by setting both outputs high
;
stop
banksel PORTA
movlw b'11011000'
movwf PORTA
return
;
; forward on both motors
;
forward
banksel PORTA
movlw b'01001000'
movwf PORTA
;call Delay255
call Delay50
return
;
; reverse on both motors
;
reverse
banksel PORTA
movlw b'10010000'
movwf PORTA
call Delay255
return
;
; turn right on spot
;
fastright
banksel PORTA
movlw b'10001000'
movwf PORTA
call Delay255
return
;
; turn right by stopping left motor
;
slowright
banksel PORTA
movlw b'11001000'
movwf PORTA
call Delay255
return
;
; turn left on the spot
;
fastleft
banksel PORTA
movlw b'01010000'
movwf PORTA
call Delay255
return
;
; turn left by stopping right motor
;
slowleft
banksel PORTA
movlw b'01011000'
movwf PORTA
call Delay255
return
; end motor functions
;
; Delay routines (based on 4MHz clock)
; http://www.piclist.com/techref/piclist/codegen/delay.htm
Long_Delay
movlw d'60' ;delay 6 seconds
call Delay100W
return
Delay100W movwf count ;delay W x 100mS
d2 call Delay100 ;maximum delay 25.5 seconds
decfsz count ,f
goto d2
return
Delay255 movlw 0xff ;delay 255 mS
goto d0
Delay100 movlw d'100' ;delay 100mS
goto d0
Delay50 movlw d'50' ;delay 50mS
goto d0
Delay20 movlw d'20' ;delay 20mS
goto d0
Delay10 movlw d'10' ;delay 10mS
goto d0
Delay1 movlw d'1' ;delay 1mS
goto d0
Delay5 movlw 0x05 ;delay 5.000 ms (4 MHz clock)
d0 movwf count1
d1 movlw 0xC7
movwf counta
movlw 0x01
movwf countb
Delay_0 decfsz counta, f
goto $+2
decfsz countb, f
goto Delay_0
decfsz count1 ,f
goto d1
return
;end of Delay routines
end
No comments:
Post a Comment