|
|
Sonar &
Infrared Obstacle Avoidance
 |
 |
|
Back To Index
|
|
This project combines infrared & sonar navigation
together. The hardware.inc include file used in previous code examples &
experiments has been changed & saved with the new name XFILES.INC.
Several modifications have been made to the new include file specifically
for this application.
The SRF04 is a relatively power hungry
device, and consumes [30-50mA]. Since we're running on batteries, we'll use PortB.6
to control a 2N3904 NPN transistor configured as a low-side driver to
turn power on or off to the sonar range-finder. When the SRF04 isn't being
used we can extend battery life significantly by turning it off. |
Figure #1: SRF04 Connections
Trigger VAR PortA.2 ' PortA.2 = SRF04 trigger pin
Echo VAR PortA.3 ' PortA.3 = SRF04 echo pin
As shown above,
two lines in the new include file are used to set up an alias for pins that
will control the SRF04. PortA.2 is the Trigger pin, and PortA.3 is the Echo pin. Make these connections between the
SRF04 & PIC PortA I/O-pins.
PortB.6 will be
used to control a 2N3904 NPN transistor. A logic 1 on the base of the NPN
turns the transistor on. This connects ground from the NPN emitter to the
collector providing ground to the SRF04. A logic 0 on the base of the NPN
turns the transistor off, and removes the ground connection from the SRF04.
Without the ground connection the SRF04 is turned off.
The new include
file defines an alias for PortB.6 called SonaControl. When SonaControl
= 1 the SRF04 will be turned on. When SonaControl = 0 the SRF04 is turned
off to conserve battery power.
We setup constants
to use for both on & off logic states making everything much easier to read.
Active = on, InActive=off. When we call the Ping sub-routine the SRF04 is
activated on entry, and deactivated on exit.
Active CON 1 ' Logic to turn on sonar drive transistor
InActive CON 0 ' Logic to turn off sonar drive transistor
SonaControl VAR PortB.6 ' PortB.6 = Sonar power control pin Ping:
SonaControl=Active
Trigger=0 ' Pulsout = compliment of initial value
PULSOUT Trigger, 6 ' Output >10 uS trigger pulse
PULSIN Echo, 1, Width ' Measure distance to target
Distance = Width / 74 ' Convert to inches
SonaControl=InActive
RETURN
|
|
Figure #2: Infrared LED Drive Circuit |
|
Figure #3: Infrared Detector
Circuit |
|
Figures #2 & #3 above show the
connections for the infrared LED drive & detector circuits. These are the
same as previous experiments.
Notice how we've set up a loop
counter. After 6 complete loops moving forward and checking for obstacles in
our path with infrared, we check for objects with the SRF04 sonar
range-finder. During the time we're using the infrared components for
obstacle detection we keep the SRF04 powered down to conserve batteries.
Once Micro-Bot has detected an
obstacle with infrared or the sonar range-finder, program execution will
jump to several sub-routines to determine where the obstacle was, and which
sensor type [IR or Sonar] found it.
Once we reach one of the obstacle
avoidance sub-routines BackUp, TurnRight or TurnLeft, we'll keep checking
for the obstacle that caused us to get into the obstacle avoidance
sub-routine before exiting. This will cause continuous right turns, left
turns or backing up until Micro-Bot has cleared the obstacle. |
'****************************************************************
'* Name : IR_Sonar.BAS *
'* Author : Bruce Reynolds *
'* Notes : Micro-Bot Infrared + Sonar Navigation Example *
'****************************************************************
INCLUDE "XFILES.INC"
Begin:
FOR X=1 TO 4
PULSOUT Left_Servo,ForwardLeft
PULSOUT Right_Servo,ForwardRight
PAUSE 20
NEXT
Loops=Loops+1
IF Loops >=6 THEN SonarCheck
GOSUB Check_Left ' Check left for obstacles
GOSUB Check_Right ' Check right for obstacles
' Check IR results
IF (Left_IR=No) AND (Right_IR=No) THEN Begin ' If no contact skip further checking
IF (Left_IR=Yes) AND (Right_IR=Yes) THEN GoBack ' If contact on both sides then reverse
IF (Left_IR=Yes) AND (Right_IR=No) THEN TurnRight ' If contact on left then turn right
IF (Left_IR=No) AND (Right_IR=Yes) THEN TurnLeft ' If contact on right then turn left
GOTO Begin
SonarCheck: ' Check sonar
Loops=0 ' Reset loop counter
GOSUB Ping ' Take sonar reading
IF (Distance > 9) THEN Begin ' Exit if no object within 9"
BackUp:
FOR X=1 TO 20
PULSOUT Left_Servo,ReverseLeft ' Back up first
PULSOUT Right_Servo,ReverseRight
PAUSE 20
NEXT
FOR X=1 TO 10
PULSOUT Left_Servo,ReverseLeft ' Now turn left
PULSOUT Right_Servo,ForwardRight
PAUSE 20
NEXT
GOSUB Ping ' Take another sonar reading to see if we're clear after the turn
IF Distance <= 9 THEN BackUp ' Check again before exiting right BackUp sub
GOTO Begin
GoBack: ' Go backwards
FOR X=1 TO 4
PULSOUT Left_Servo,ReverseLeft
PULSOUT Right_Servo,ReverseRight
PAUSE 20
NEXT
GOTO Begin
TurnRight: ' Turn right until object is no longer in our path
FOR X=1 TO 4
PULSOUT Left_Servo,ForwardLeft
PULSOUT Right_Servo,ReverseRight
PAUSE 20
NEXT
GOSUB Check_Right ' Check for obstacle again before exiting right turn sub
IF Right_IR=Yes THEN TurnRight
GOTO Begin
TurnLeft: ' Turn left until object is no longer in our path
FOR X=1 TO 4
PULSOUT Left_Servo,ReverseLeft
PULSOUT Right_Servo,ForwardRight
PAUSE 20
NEXT
GOSUB Check_Left ' Check for obstacle again before exiting left turn sub
IF Left_IR=Yes THEN TurnLeft
GOTO Begin
END
|
|
The Include File: |
'****************************************************************
'* Name : XFILES.INC *
'* Notes : Hardware Setup For IR_SONAR.BAS *
'****************************************************************
@ DEVICE HS_OSC,MCLR_OFF,LVP_OFF,WDT_OFF,PROTECT_OFF
DEFINE OSC 20 ' We're using a 20MHz crystal
DEFINE NO_CLRWDT 1 ' Do not auto insert watchdog timer reset code
'** Turn 16F62x Comparators OFF **'
CMCON = 7 ' Comparators = OFF, PortA = digital I/O
VRCON = 0 ' Internal A/D Vref disabled
'** Define Constants **'
Led_On CON 0 ' Led_On = logic 0 which turns IR LED on
Led_Off CON 1 ' Led_Off = logic 1 which turns IR LED off
No CON 1 ' If IR Detector Output = 1, then No object detected
Yes CON 0 ' If IR Detector Output = 0, then Yes object detected
ForwardLeft CON 800 ' Make adjustments to these motor constants
ReverseLeft CON 600 ' if Micro-Bot tends to veer left or right when
ForwardRight CON 600 ' moving in a straight line to compensate for
ReverseRight CON 800 ' different batteries or servo motors
Active CON 1 ' Logic to turn on sonar drive transistor
InActive CON 0 ' Logic to turn off sonar drive transistor
TRUE CON 12 ' CCP1CON value for PWM ON
FALSE CON 0 ' CCP1CON value for PWM OFF
'** Setup PortB **'
' PortB.3 = PWM output
TRISB = %00000100 ' Set PortB TRIS register
PORTB = %00000011 ' LEDs off, Servo pins=0, Sonar off, PWM pin=0
Left_Led VAR PortB.0 ' PortB.0 = left IRLED output
Right_Led VAR PortB.1 ' PortB.1 = right IRLED output
IR_Detector VAR PortB.2 ' PortB.2 = IR detector input
Left_Servo VAR PortB.4 ' PortB.4 = left servo output
Right_Servo VAR PortB.5 ' PortB.5 = right servo output
SonaControl VAR PortB.6 ' PortB.6 = Sonar power control pin
'** Setup PortA **'
TRISA = %00001000 ' Set PortA TRIS register
PORTA = %00000000 ' Initialize PortA
Trigger VAR PortA.2 ' PortA.2 = Output for SRF04 trigger pin
Echo VAR PortA.3 ' PortA.3 = Input for SRF04 echo pin
Width VAR WORD ' Pulse width from SRF04
Distance VAR WORD ' Converted distance value
'** IR Flag Bit Variables **'
Left_IR VAR BIT ' Flag bit for left IR object detection
Left_IR = No ' Initialize to 1 [no object detected] on power-up
Right_IR VAR BIT ' Flag bit for right IR object detection
Right_IR = No ' Initialize to 1 [no object detected] on power-up
'** G.P. Working Variables **'
X VAR BYTE ' GP working variable
Loops VAR BYTE ' GP working variable
Loops = 0 ' Start with Loops variable clear
'** Setup For 40KHz PWM IRLED Drive **'
CCPR1L = 25 ' 63=50%, 25=20%, 94=75% PWM Duty-Cycle
PR2 = 124 ' Set PWM for approximately 40KHz
T2CON = 4 ' Timer2 ON + 1:1 prescale
CCP1CON = FALSE ' Start with hardware PWM = OFF
GOTO Begin ' Jump to beginning routine
Check_Left: ' Call here to check left for obstacle
CCP1CON = TRUE ' ON 40KHz PWM
Right_Led=Led_Off : PAUSE 5 ' OFF right LED / pause
Left_Led=Led_On : PAUSE 5 ' ON left LED / pause
Left_IR = IR_Detector ' Read IR detector output
Left_Led=Led_Off ' OFF Left LED
CCP1CON = FALSE ' OFF 40KHz PWM
RETURN ' Return to calling routine
Check_Right: ' Call here to check for right obstacle
CCP1CON = TRUE ' ON 40KHz PWM
Left_Led=Led_Off : PAUSE 5 ' OFF left LED / pause
Right_Led=Led_On : PAUSE 5 ' ON right LED / pause
Right_IR = IR_Detector ' Read IR detector output
Right_Led=Led_Off' OFF Right LED
CCP1CON = FALSE ' OFF 40KHz PWM
RETURN ' Return to calling routine
Ping:
SonaControl=Active
Trigger=0 ' Pulsout = compliment of initial value
PULSOUT Trigger, 6 ' Output >10 uS trigger pulse
PULSIN Echo, 1, Width ' Measure distance to target
Distance = Width / 74 ' Convert to inches
SonaControl=InActive
RETURN
|
|
Back To Index
Copyright © 2007
Reynolds Electronics
http://www.rentron.com
|