The full pennybot asm code..
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> = 0112:0>
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