|
BACK
Download Circuit
Source Code:
The program is divided into 3 sections - The main
program loop, The pwm isr ( pulse width modulation interrupt sub routine)
and The action routines. The program functions to turn on the transmitter
portion of the sensors in order of priority and see if a line is seen
by the receiver section. The outer sensors are tested first. If a line
is seen then one motor is reversed while the other continues forward.
If no line is een the next set in are checked. If a line is seen here
then one motor is stopped while the other continues forward. If no line
is seen here then the next set in is checked. If a line is seen here
then the speed of one motor is reduced while the others speed is maintained.If
no line is seen by these sensors then the centermost sensor is checked.
If a line is seen here then the motors are set form both full forward
(we reduced this from full speed for better reliability). If no line
is seen by this last sensor than the last motor setting is maintained
in hopes of finding the line again. Once a sensor detects a line and
sets the motor settings then the sensor routine is started again. In
this manor priority is given to the outermost sensors for the biggest
corrections with the more minor corrections being serviced last. More
details on this can be seen in the code listing.
device PIC16F84,WDT_ON
; techbot1.src
; ORIG 9/27/98 By Jerry Merrill
; Code for Simple line following robot for
; Embedded Systems Conference West, 1998
; Modified 11/27/98 By James R. Vroman
; Hardware consists of 2 reversible DC gear motors for propulsion
; and 7 IR reflective sensors.
; The gear motors are Cirrus CS70 servos that have been modified
; for full rotation and have had the servo control boards removed.
; The original wires are reused with the three wires being the
; 2 motor terminals and the case ground.
;
; The sensors are 7 QT Opto (QRB1114) IR emitter/detector pairs
; mounted under the nose of the robot.
; The sensors are arranged in an inverted "V" pattern as shown
below.
;
; Sensor arrangement:
;
; A <= Fast Forward
; B C <= Correct ( adjust speed )
; D E <= Spin
; F G <= Rotate
; PIN ASSIGNMENTS:
; PORTA.0 Left Motor Enable 0=Enable 1=Disable
; PORTA.1 Left Motor Direction 0=Forward 1=Reverse
; PORTA.2 Right Motor Enable 0=Enable 1=Disable
; PORTA.3 Right Motor Direction 0=Forward 1=Reverse
; PORTA.4 reserved for SPKR
; PORTB.0 Sensor INPUT ; 1 => BLACK sensed
; PORTB.1 -_ 000 - disable all
; PORTB.2 _- To 138 inputs 001 - LED A
; PORTB.3 - 010 - LED B
; 011 - LED C
; 100 - LED D
; 101 - LED E
; 110 - LED F
; 110 - LED G
; PORTB.4 reserved for future expansion
; PORTB.5 reserved for future expansion
; PORTB.6 reserved for future expansion
; PORTB.7 reserved for future expansion
org 0Ch ; start of RAM
setpnt1 ds 1
setpnt2 ds 1
upcount1 ds 1
dwncount1 ds 1
upcount2 ds 1
dwncount2 ds 1
wsave ds 1
statsave ds 1
dly ds 1 ;delay loop counter (forgnd)
;Sensor enable chart.
LEDS equ PORTB
DISABLEALL equ 11110001b
ENABLEA equ 11110011b
ENABLEB equ 11110101b
ENABLEC equ 11110111b
ENABLED equ 11111001b
ENABLEE equ 11111011b
ENABLEF equ 11111101b
ENABLEG equ 11111111b
org 0
nop
jmp start
org 4
; Interrupt service routine at 4
include 'hbridg2.src'
; Followed by the main action routines
include 'tech1act.src'
; Followed by the initialization code
start setb RP0; STATUS.5 ; select 2nd bank
mov TRISB,#01 ; all outputs except b0
clr TRISA ; all outputs
clrwdt ;
mov OPTION,#1 ; Set /4 prescaler to RTCC.
clrb STATUS.5 ; back to 1st bank
clr upcount1
clr dwncount1
clr upcount2
clr dwncount2
clr setpnt1
clr setpnt2
mov INTCON,#0A0h ; enable timer0 interrupts
; Followed by the MAIN SENSOR POLLING LOOP
; main loop
; We poll each sensor in order of priority.
; The FIRST active sensor is serviced and then
; the polling is started over. This creates a
; if..elseif..elseif priority type structure.
; For efficiency, we JMP to the service routine
; which then JMPs to MAIN when complete (rather
; than call..return..JMP MAIN).
main
mov w,#ENABLEG ; enable sensor G
call rdsensor ; read the sensor
sc ; skip next command if no line
jmp ROTRT ; rotate right
mov w,#ENABLEF ; enable sensor F
call rdsensor ; read the sensor
sc ; skip next command if no line
jmp ROTLT ; rotate left
mov w,#ENABLEE ; enable sensor E
call rdsensor ; read the sensor
sc ; skip next command if no line
jmp PIVRT ; pivot right
mov w,#ENABLED ; enable sensor D
call rdsensor ; read the sensor
sc ; skip next command if no line
jmp PIVLT ; pivot left
mov w,#ENABLEC ; enable sensor C
call rdsensor ; read the sensor
sc ; skip next command if no line
jmp SLOWRT ; slow right motor
mov w,#ENABLEB ; enable sensor B
call rdsensor ; read the sensor
sc ; skip next command if no line
jmp SLOWLT ; slow left motor
mov w,#ENABLEA ; enable sensor A
call rdsensor ; read the sensor
sc ; skip next command if no line
jmp FORWARD ; full speed forward
jmp HOLD ; if line has not been detected by now
; retain last motor setting,
rdsensor ; Read the sensors -all sensors are wired
; in parallel
clrwdt ; 'Kick the DOG'
clrb GIE ; NO IRQ while reading sensor
mov LEDS,w
call delay ; let sensor settle about 35us
rrf PORTB,0 ; get reading into C
setb GIE ; re-enable IRQ
mov LEDS,#DISABLEALL ; Disable LEDs and
call delay ; delay about 140us
call delay ; to reduce power
call delay ; LED duty cycle = 20%
call delay ; LED power = .2*25ma = 5ma
ret
; simple delay loop to delay ~35us
; This is used to allow the sensor time
; to settle after turning on its LED.
delay mov dly,#010
loop: decsz dly
jmp loop:
ret
BACK
|
