Sie sind auf Seite 1von 93

VEHICLE FOLLOWING AND COLLISION AVOIDANCE MOBILE ROBOT

BOON KHANG HUA

UNIVERSITI TEKNOLOGI MALAYSIA


PSZ 19:16 (Pind. 1/07)

UNIVERSITI TEKNOLOGI MALAYSIA

DECLARATION OF THESIS/ UNDERGRADUATE PROJECT PAPER AND COPYRIGHT

Author’s full name : BOON KHANG HUA

Date of birth : 13 DECEMBER 1987

Title : VEHICLE FOLLOWING AND COLLISION AVOIDANCE


ROBOT

Academic Session : 2009/2010

I declare that this thesis is classified as:

CONFIDENTIAL (Contains confidential information under the


Official Secret Act 1972)*

RESTRICTED (Contains restricted information as specified by


the organization where research was done) *

√ OPEN ACCESS I agree that my thesis to be published as online


open access (full text)

I acknowledged that Universiti Teknologi Malaysia reserves the right as follows :


1. The thesis is the property of Universiti Teknologi Malaysia.
2. The Library of Universiti Teknologi Malaysia has the right to make copies
for the purpose of research only.
3. The Library has the right to make copies of the thesis for academic
exchange.

yf
Certified by :

…………………………………….. ……………………………………..
SIGNATURE SIGNATURE OF SUPERVISOR

871213-13-6475 DR. JOHARI HALIM SHAH OSMAN


(NEW IC NO. PASSPORT NO.) NAME OF SUPERVISOR

Date: MAY 2010 Date: MAY 2010


“I hereby declare that I have read this thesis and in my opinion this thesis is
sufficient in terms of scope and quality for the award of the degree of Bachelor of
Mechatronics Engineering

Signature : __________________________

Name of Supervisor Dr. Johari Halim Shah Osman


: __________________________

Date 04 MAY 2010


: __________________________
VEHICLE FOLLOWING AND COLLISION AVOIDANCE MOBILE ROBOT

BOON KHANG HUA

A thesis submitted in partial fulfillment of


the requirements for the award of the degree
of Bachelor of Mechatronics Engineering

Faculty of Electrical Engineering

Universiti Teknologi Malaysia

2010
ii

“I declare that this thesis entitled “Vehicle Following and Collision Avoidance
Mobile Robot” is the result of my own research except as cited in the references. The
thesis has not been accepted for any degree and is not currently submitted in
candidature of ant other degree.”

Signature : __________________________

Name of Author BOON KHANG HUA


: __________________________

Date 04 MAY 2010


: __________________________
iii

Special dedicated to
My beloved family and friends
iv

ACKNOWLEDGEMENT

I would like to take this opportunity to express my deepest gratitude to my


supervisor, Dr. Johari Halim Shan Osman who had been guided me along the project.
He has been motivating and inspired me to successfully complete my project. His
guidance, advice, encouragement, patient and support given throughout the project
are greatly appreciated.

Sincere thanks also to my lecturers who had given me valuable suggestions


and helpful discussions to ensure successfulness of the project.

My appreciations also extend to my parents and friends for their caring and
support. Last but not least, I am thankful for those who directly or indirectly lent me
a hand in this project.
v

ABSTRACT

Nowadays, various robots are built to perform multiple tasks. Multiple robots
working together to perform a single task becomes important. One of the key
elements for multiple robots to work together is the robot need to able to follow
another robot. This project is mainly concerned on the design and construction of the
robot that can follow another robot. In this project, the follower robot is the robot that
follows the robot and the leader robot is the robot being followed. The follower robot
can follow leader robot using ultrasonic sensors. Three ultrasonic sensors are
installed in the follower robot to obtain the relative position of the leader robot.
Besides, the follower robot also can avoid the collision with the leader robot when
perform the task following. Follower robot always maintains a distance of 15 cm
from the leader robot to avoid the collision with leader robot. Furthermore, the
follower robot is also equipped with infrared sensors to detect and avoid obstacles
around the environment when perform the task following. The follower robot can
perform the following task better with the obstacle avoidance feature.
vi

ABSTRAK

Pada masa kini, pelbagai robot dibuat untuk melakukan pelbagai tugas.
Keperluan untuk pelbagai robot bekerjasama untuk melakukan satu tugas menjadi
penting. Salah satu elemen yang penting bagi robot-robot untuk bekerja bersama
adalah kemampuan untuk mengikut robot lain. Projek ini fokus kepada reka bentuk
dan pembinaan sebuah robot yang boleh mengikut robot yang lain. Dalam projek ini,
robot pengikut adalah robot yang mengikut robot lain dan robot pemimpin adalah
robot yang diikuti. Robot pengikut dapat mengikuti robot pemimpin dengan
menggunakan sensor ultrasonik. Tiga sensor ultrasonik dipasang pada robot pengikut
untuk mendapatkan kedudukan relatif robot pemimpin dari robot pengikut. Selain itu,
robot pengikut juga boleh mengelakkan pelanggaran dengan robot pemimpin ketika
mengikuti robot pemimpin. Robot pengikut selalu akan berada pada jarak 15 cm
daripada robot pemimpin untuk mengelakkan pelanggaran dengan robot pemimpin.
Selain itu, robot pengikut juga dilengkapi dengan sensor infra-merah untuk
mengesan dan mengelak pelanggaran dengan halangan di sekitar laluan ketika
mengikuti robot pemimpin.
vii

TABLE OF CONTENTS

CHAPTER TITLE PAGE


DECLARATION ii
DEDICATION iii
ACKNOWLEDGEMENT iv
ABSTRACT v
ABSTRAK vi
TABLE OF CONTENTS vii
LIST OF TABLES x
LIST OF FIGURES xi
LIST OF SYMBOLS xiii
LIST OF APPENDICES xiv

1 INTRODUCTION 1
1.1 Background of Study 1
1.2 Statement of Problem 2
1.3 Objective 3
1.4 Significant of Study 3
1.5 Scope of Study 3
1.6 Outline of Thesis 4

2 LITERATURE REVIEW 5
2.1 Introduction 5
2.2 Development of Road Vehicle Convoy
System 5
2.3 Low Cost Sensing for Autonomous Car
Driving on Road 6
viii

2.4 Raccoon 7
2.5 Scale Invariant Feature Transform (SIFT)
Algorithm 9
2.6 The X80 Robot 10
2.6.1 Leader/Follower Behaviour Using
Ultrasonic Transmitter and Receiver 11
2.7 Conclusion 12

3 METHODOLOGY 13
3.1 Introduction 13
3.2 Electrical Design 13
3.2.1 Electrical Design for the Follower
Robot 13
3.2.1.1 Power Supply Circuit 14
3.2.1.2 DsPIC30F4013 Circuit 14
3.2.1.3 Push Button Circuit 16
3.2.1.4 Motor Driver Circuit 16
3.2.1.5 Ultrasonic Receiver Circuit 17
3.2.1.6 Infrared Circuit 19
3.2.2 Electrical Design for the Leader Robot 20
3.2.2.1 DsPIC30F4013 Circuit 20
3.2.2.2 Ultrasonic Transmitter Circuit 21
3.2.3 Radio Frequency(RF) Module 21
3.3 Mechanical Design 22
3.3.1 Mechanical Design for the Follower
Robot 22
3.3.2 Mechanical Design for the Leader
Robot 27
3.4 Software Implementation 30
3.4.1 Interaction between the Leader Robot
and the Follower Robot 30
3.4.2 Software Implementation for the Leader
Robot 31
ix

3.4.3 Software Implementation for the


Follower Robot 33
3.5 Positioning the Leader Robot 37

4 RESULT AND DISCUSSION 39


4.1 Reliability of the Positioning System for the
Follower Robot 39
4.2 Ability of the Follower Robot 41

5 CONCLUSION 45
5.1 Conclusion 45
5.2 Limitation 46
5.3 Future Improvement 46

REFERENCES 48

Appendices A-B 50-76


x

LIST OF TABLES

TABLE NO. TITLE PAGE

4.1 Actual distance and measured distance 39

4.2 Motion performed if obstacle detected 44


xi

LIST OF FIGURES

FIGURE NO. TITLE PAGE

2.1 HANS vehicle (ARTV robot) 6

2.2 Occupancy grid 7

2.3 Robot Follower using SIFT algorithm 9

2.4 X80 robot 10

2.5 IR sensor on X80 robot 10

2.6 Maxelbot 11

3.1 Power supply schematic 14

3.2 DsPIC30F4014 circuit 15

3.3 Push button circuit 16

3.4 Motor driver circuit 17

3.5 DC motor 17

3.6 Ultrasonic receiver circuit 18

3.7 IR receiver and emitter 19

3.8 LM324 comparator circuit 19

3.9 DsPIC30F4013 circuit 20

3.10 Ultrasonic transmitter circuit 21

3.11 Radio frequency transmitter module 22

3.12 Radio frequency receiver module 22

3.13 Top view for the follower robot 23


xii

3.14 Front view for the follower robot 24

3.15 Side view for the follower robot 25

3.16 Coupling Design 25

3.17 Actual picture of the follower robot 26

3.18 Top view for the leader robot 27

3.19 Side view of the leader robot 28

3.20 Front view of the leader robot 28

3.21 Actual picture of the leader robot 29

3.22 Flow Chart of interaction between the leader robot 31


and follower robot

3.23 Flow chart for the algorithm of the leader robot 32

3.24 Flow chart for the general algorithm of the leader 33


robot

3.25 Reception of RF signal 34

3.26 Reception of ultrasonic signal 35

3.27 Algorihtm for movement of the follower robot 36

3.28 Relative distance and angle of the leader robot 37

4.1 Actual distance and measured distance 40

4.2 Move forward 41

4.3 Turn right 42

4.4 Turn left 42

4.5 Reverse 43

4.6 Reverse and turn right sharply 43

4.7 Arrangement of the IR sensor from top view 44

5.1 Analog sensor from SHARP 46

5.2 Aluminum Cone 47


xiii

LIST OF SYMBOLS

IR - Infrared
RF - Radio Frequency
cm - Centimeter
DC - Direct Current
Θ - Angle
D - Distance
xiv

LIST OF APPENDICES

APPENDIX TITLE PAGE


A Program in DsPIC30F4013 Microcontroller for The 50
Follwer Robot

B Program in DsPIC30F4013 Microcontroller for The 72


Leader Robot
CHAPTER 1

INTRODUCTION

1.1 Background of Study

Nowadays, robots are built to perform multiple tasks with different level of
complexity. There are some situations that require multiple robots to perform a
single task. When these situations are required, the robots are required to cooperate
with each other. One of the key elements for the multiple robots to work together is
that the robot need to able to follow other robot or human. This element leads to the
study of the leader and follower behavior.

Basically, the leader in leader/follower behavior is the target that is followed


by the other robot (follower). The leader (target) can be either human or robot. The
follower, which is a robot, need to follow the leader autonomously. The task of this
behavior is called task following.

When a robot (follower) performs a task following, the most important thing
for the follower is to be able to obtain the position of the target (Leader). The
follower must be able to obtain its current position relative to the target before it
makes decision on how to follow the target. “Localization” or knowledge of its
current location is calculated by one or more means, using sensors such as motor
encoder, vision, laser or sonar sensors. There are many vision systems being built to
track and follow the target effectively. The method can be seen in [2], [3] and [4].
2

The visual tracking system installed on the follower enables the follower to
follow the leader robustly in a variety of environment. Despite this advantage, there
are quite a number of disadvantages for using vision system. One of the main
disadvantages of the vision system is that it is costly to build. This is due to the high
computational power required to process the raw data obtained from vision sensor.
This reason makes the vision tracking system less preferable to be used.

Another main method is using an optical system which utilizes the optical
range sensor such as IR sensor or laser sensor. This can be seen in [5]. The optical
system using IR sensor is simple and cheap. However, the follower solely depends
on the IR sensor to perform the task following is quite inconvenient. It is because the
IR sensor can give the information in a single axis or straight line only. This will
cause the follower to lose sight of the target followed easily during turning.

1.2 Statement of Problem

It is an increased in research interest in systems composed of multiple


autonomous mobile robots exhibiting cooperative behavior. Groups of mobile robots
are constructed, with the aim to studying such issues as group architecture, resource
conflict, origin of cooperation, learning, and geometric problems. As yet, few
applications of cooperative robotics have been reported, and supporting theory is still
in its formative stages [7]. One of the main tasks for cooperative wheeled mobile
robots [8] is the object (target) following task, which usually represents to follow
another robot. Also, the following task is important for the wheeled mobile robots in
[9], [10] and the target can be static or a dynamic object.

There are many problems arise when designing a robot to perform a


following task. These problems include robot may looses or hits the target being
followed. Besides, the robot may also choose not to follow the desired object and
goes after another detected object from the environment in a dynamic environment.
3

1.3 Objective

The objectives of this report are:


a) To design and construct an autonomous robot.
b) To integrate the concept of collision avoidance into the leader/follower behavior.
c) To improve the capability of the robot in performing the following task smoothly.

1.4 Significance of Study

The following task, which performs by the follower, is very important and
has large number of application. The application includes car convoy system which
is very important in adaptive cruise control. The follower can follow the leader
effectively. Besides, the follower can avoid the collision with the leader when the
distance between them is too close. This collision avoidance is crucial for the
following task since the collision will damage both leader and follower.

1.5 Scope of Study

The scope of this project is to design a mobile robot that functions


autonomously. Implementing the sonar sensor and IR sensor is included within the
scope. The sensor provides capabilities to the robot for navigation and prevents the
collision between the follower and leader. The DC motor and the sensor are
interfaced with DsPIC30F4013 microprocessor is essential for the proper function of
the robot.

The scope for the follower robot designed is that it can follow the leader
within the indoor environment with simple obstacle. Furthermore, the follower can
have the collision avoidance capability with the leader.
4

1.6 Outline of Thesis

This thesis consists Five chapters. In chapter 1, it discuss about the objective
and scope of this project. While Chapter 2 will discuss literature reviews that have
been done. It well discuss about various type of robot, advantages and limitation for
each of the robots.

In Chapter 3, the discussion will be on the methodology hardware and


software implementation of this project. The result and discussion will be presented
in Chapter 4. Last but not least, Chapter 5 discusses the conclusion of this project and
future work that can be done.
CHAPTER 2

LITERATURE REVIEW

2.1 Introduction

This chapter review some of the robots that were built to perform the task
following. The advantages and disadvantages for each of the robot are also included
in the review.

2.2 Development of road vehicle convoy system

A final year project entitled “Development of a road vehicle model for road
vehicle convoy system” [1] was conducted at Faculty Electrical Engineering (FKE),
University Teknologi Malaysia in 2007. In this project, two small scale car-like
robots were developed. One is the leading vehicle and another is the following
vehicle. The following vehicle could follow the leading vehicle in straight line.

Both vehicles in this project utilize PIC 18F454 microcontroller (MCU) as


the “brain” for both vehicles. The following vehicle utilizes ultrasonic sensor (R40-
16 & T40-16) to detect and measure the distance between the leading and the
following vehicle. When the ultrasonic sensor sends a wave with certain frequency
and received it back through reflection after hit the obstacle, the information was sent
to the PIC microcontroller. The Microcontroller then perform calculation to obtain
the distance between the vehicles and follow the vehicle. The C programming
language was used for the programming part in this project. The MPLAB IDE
version 7.43 with C18 compiler support C language programming.
6

The robot follower in [1] has a numbers of limitations. Firstly, the following
vehicle cannot follow the leading vehicle when the leading vehicle turn left or right
from the straight line. Besides, the following vehicle could not avoid any obstacle or
collision. The following vehicle also could not vary its speed in accordance to the
distance between the vehicles. It can travel at a constant speed only.

2.3 Low Cost Sensing for Autonomous Car Driving on Road

Figure 2.1 HANS Vehicle [2]

According to [2], a car-like robot equipped with a system called HANS, is


able to navigate in an autonomous and safe manner, performing trajectories similar to
the ones carried out by human drivers. The system was successfully tested in both
simulations and in a laboratory environment using a mobile robot to emulate the car-
like vehicle. As a result, this autonomous car can follow the front vehicle in curve
road. Besides, this mobile robot also can follow the road, keeping the car in the right
lane, maintaining safe distances between vehicles, and avoiding collision. For this
mobile robot, it is assumed that there are no cars driving faster than the HANS
vehicle which means that no cars will appear from behind.

HANS in [2] uses a low resolution web camera located in the centre of the
vehicle behind the rear-view mirror and a set of sixteen sonar sensor. The key role of
the camera is to act as a vision system. It is used to detect the side lines that bound
7

the traffic lanes, the position and orientation of the robot relative to these lines, and
the vehicles driving ahead and determining their lane and distance to the robot.

The sixteen sonar sensor was arranged to build up a occupancy grid as shown
in figure 2.2. This strategy is to reduce the influence of sonar reflections. Each sonar
sensor will form up one cone and each cone is divided into zones. The distance of
each zone is defined from the robot. Obstacles lying over a region of the occupancy
grid contribute to the voting of the cells. The zone with the highest number of
measurements (votes in a sense) is considered as being occupied by obstacle. Sonar
sensors are also used to detect emergency stopping conditions. With combinational
of camera and sonar sensor, the perception of environment also can be mapped for
the robot making autonomous decision.

Figure 2.2 Occupancy Grid [2]

2.4 Raccoon

RACCOON is a vision-based system that tracks car taillights at night as


described in [3]. The RACCOON system was developed at Carnegie Mellon
University. The prototype was built and integrated with RACCOON system. This
system enables the autonomous vehicle to chase the leading car effectively under low
light condition.

According to [3], this project was inspired by following reason:

a) The road cannot be seen clearly at night.


8

b) Unlit landmarks cannot be detected so corners and intersections have to be


negotiated based solely on the observed actions of the lead vehicle at night.

Problems above make the following vehicle cannot detect the leading vehicle
clearly at night using normal vision system and algorithm. In normal algorithm, the
taillights can be easily extracted from a dark background. After the extraction, the
autonomous car steers toward the taillight of lead vehicle. According to [2], the
autonomous vehicle may follow the lead vehicle successfully with this algorithm.
However, this algorithm fails when lead vehicles turns to follow winding road. When
this scenario occurs, the computer controlled may steer towards taillights of lead
vehicle and then results in corner cutting. These problems can be solved by using
RACCOON system.

Under RACCOON system autonomous car, the image sensor can build a
global map in real time that contains the position of lead vehicle based on the
location and separation of the taillights (taillights of car) in a sequence of video
images. Only one color camera was used as sensor. According to [2], RACCOON is
creating an intermediate map structure which records the lead vehicle’s trajectory.
The path is represented by points in a global reference frame. After that, the
computer controlled vehicle is steered from point to point to follow lead vehicle’s
trajectory. The autonomous vehicles can follow the lead vehicle at any desired speed
to keep the lead vehicle’s taillights in sight. Using this approach, the autonomous
vehicle steers around corners and obstacles rather than through them.
9

2.4 Scale Invariant Feature Transform (SIFT) algorithm

In [4], a robot was constructed to follow human or another robot using the
vision system. The vision system in this project utilized the SIFT algorithm as shown
in figure 2.3. In this algorithm, the robot uses the feature extracted from the training
image of target to track the target. Firstly, it uses the SIFT algorithm to recognize the
target. After the target is recognized, it estimates the position of the target. Then it
uses the PID controller to control the motor to maintain the minimum distance
between the follower and the target.

Figure 2.3 Robot follower using SIFT algorithm [4]

There are a number of limitations of this method for following behavior. The
3.5 meter effective distance for the recognition system is adequate for a small robots
operating indoors, but would not be adequate for larger outdoor platforms.
Additionally, direct pursuit of the leader’s current position is quite impropriate and
does not work well in complex environments. PID control loops were time
consuming to properly tune, and the performance of the simple robot platform limits
the applicability of the system as implemented to wider applications.

The advantages of this system are: Firstly, the immunity to orientation and
occlusion problems made the system easy to use. Object recognition also allows for
the implementation of a wide variety of different behaviors based upon a set of
different trained objects, opening the way for new avenues of human robot
cooperation. And despite the image recognition software iterating at only 2Hz, the
controllers were able to perform adequately for the task. With the application of
10

increased processing power to the problem, robot control performance would


improve.

2.5 The X80 Robot

Figure 2.4 shows the image of the X80 robot. According to [5], the X80 robot
has 4 Infrared sensors and 3 sonar sensors to perform the following task. It is used to
pursuit the direct position of leader robot. The position of each sensor is arranged as
shown in figure 2.5. The following task is heavily depending on the IR sensors only.
The other 3 sonar sensors are used only to perform the checking on the measurement
of distance return by the IR sensors.

Figure 2.4 X80 ROBOT [5] Figure 2.5 IR sensor on X80 robot [5]

The limitation of the robot is that this robot can only perform the following
task in free space. The follower will lost the target if it is operating in the space full
of the obstacles. Besides, it may also easily lost track of the leader robot. Another
limitation is that the follower robot needs to stop during turning direction when the
leader robot turns direction.

Despite its limitation, the X80 robot can follow the leader robot at low speed
in free space.
11

2.6 Leader/Follower Behavior using ultrasonic transmitter and receiver

Figure 2.6 shows two robots that are performing the leader and follower
behavior. The robot is called Maxelbot [6]. In figure 2.6, one of the robots act as the
leader and another robot act as follower in the following behavior. The leader uses
the ultrasonic transmitter to transmit a signal through the parabolic cone. The
purpose of the parabolic cone is to transmit the signal at 360 degree to the
surrounding. The three receivers of the following car will catch the signal transmitted
from the transmitter. Then the following car will perform mathematical calculation
based on the distance of three receivers from the transmitter. Base on the calculation
result, the follower predicts the distance and angle of leader relative to the follower.

Figure 2.6 Maxelbot [6]

Limitation of this project is that the effective distance that can be measured
by the follower from the leader is about one meter only. Once the distance of the
leader is more than 1 meter from the follower, the follower cannot follow the leader.

The advantage of this method is that the cost of the hardware is relatively low
when compared to the vision system base method. Beside, the robot is more robust
when performing the following task in multiple obstacles environment compared to
following system utilizing IR sensors only.
12

2.7 Conclusion

Various robots were built to perform the task following. The advantages and
disadvantages of robots reviewed are discussed in this chapter. These reviews
provide some guidance for this project when choosing the sensor. In conclusion, the
ultrasonic sensor and infrared sensor are selected to be implemented in the project.

The ultrasonic sensor is chosen over image sensor because ultrasonic sensor
is unaffected by lighting condition of environment. Hence, ultrasonic sensor can
work well under different lighting condition. Besides, the ultrasonic sensor is chosen
because an ultrasonic positioning system modified from Maxelbot [6] is built to
perform the task following in this project. The system that implemented in Maxelbot
[6] is not suitable for task following of this project. However, the positioning system
is quite useful when positioning the leader robot. Besides, the cost of the ultrasonic
sensor is lower compared to image sensor used in SIFT robot [4] and RACCOON
robot [3].

The infrared sensor is used to perform the obstacle avoidance. The infrared
sensor had been used in X80 robot [5] and HANS vehicle [2] for obstacle avoidance.
This feature is useful since the follower robot of this project need to avoid the
collision with the obstacles around the environment.
CHAPTER 3

METHODOLOGY

3.1 Introduction

The mechanical design, electrical design and software design of the robot will
be discussed in this chapter.

3.2 Electrical Design

The electrical design for the leader robot and the follower robot will be
discussed in this section.

3.2.1 Electrical Design for the Follower Robot

The electrical design for the follower includes the power supply, the
DsPIC30F4013 circuit, the push button circuit, the motor driver circuit, the ultrasonic
receiver circuit, and the infrared sensor circuit.
14

3.2.1.1 Power Supply Circuit

Figure 3.1 shows the schematic design for the power supply in the follower
robot. The LM7805 regulator IC is utilized in the circuit to produce a constant 5V
voltage to all circuits in the follower robot. This 5V voltage will become the voltage
source for all chips which include the DsPIC30F4013, LM324 comparator, the
L293D motor driver, the LMC6032 OP-Amp and the LM311 comparator.

Figure 3.1 Power Supply Schematic

3.2.1.2 DsPIC30F4013 Circuit

Figure 3.2 shows the DsPIC30F4013 circuit used in the follower robot. The
microcontroller used in the circuit is DsPIC30F4013, which is a 16 bit
microcontroller. The main reason this microcontroller is chosen is because of its
input capture feature. There are four channels of the input capture pins which are use
as the trigger source from the ultrasonic circuit. The input capture pins is IC1, IC2,
IC7 and IC8 as shown in figure 3.2. The PIC18F452 had been considered as the
microcontroller for the project but the number of channel for the input capture
feature is not adequate for the project.

The main function of the DsPIC30F4013 circuit is to act as the controller for
the robot. The chip is programmed to receive the digital information from the
ultrasonic receiver circuit and infrared circuit. Then the microcontroller will drive the
DC motor via the motor driver circuit based on the information received.
15

(a)

(b)

Figure 3.2 DsPIC30F4013 circuit (a) Picture (b) Schematic Circuit


16

3.2.1.3 Push Button Circuit

Figure 3.3 shows the push button circuit used in the follower robot. It is used
to select and chose the mode of the program in the DsPIC30F4013. With this
configuration, the number of the discrete program can be programmed into
microcontroller and choose by the button Mode Select in figure 3.3. After the
program selected, the Mode Execute button is pressed to implement the program.

Figure 3.3 Push Button Circuit

3.2.1.4 Motor Driver Circuit

Two DC motors are chosen to be used in the follower robot. The L293D
motor driver is used to control the left and right DC motors. The PWM signal is
generated from microcontroller and connected to EN1 and EN2 in figure 3.4.
Another 4 pins from microcontroller (RB9, RB10, RB11 and RB12) are connected to
control the direction of the motor as shown in figure 3.4. Figure 3.5 shows the DC
motor used in the follower robot. The maximum speed is 26 revolution per minute
while the maximum torque that can be handled by the motor is 0.588 N m.
17

(a) (b)
Figure 3.4 Motor Driver Circuit (a) Schematic (b) Picture

Figure 3.5 DC motor

3.2.1.5 Ultrasonic Receiver Circuit

Figure 3.6 show the ultrasonic receiver circuit for the follower robot. The
function of the ultrasonic receiver circuit is to amplify the electrical signal from the
ultrasonic transducer and convert the analog electrical signal into digital electrical
signal. The ultrasonic sensor is also used to convert the ultrasonic wave in the air into
the electrical signal. However, this electrical signal is too weak and need to be
amplified by the ultrasonic receiver circuit.

Figure 3.6(a) shows the schematic of the amplifier circuit part of the
ultrasonic receiver circuit. The LMC6032 Op-Amp is used as stage amplifier to
obtain the gain of 5032. The gain of 5032 is needed because the electrical signal
generated by ultrasonic sensor around 80 mV. It is not sufficient to be compared with
the reference voltage in amplifier circuit. As a result, the electrical signal needed to
be amplified to 4 to 5 volt before it was sent to amplifier circuit. There are 3 stages of
amplifier. The first two stages are high pass filter with the gain of 18 for each stage.
18

The third stage is band pass filter with the gain of 15. Figure 3.6(b) shows the
schematic of the comparator part of the ultrasonic receiver circuit. The LM311 IC is
used as the comparator for the circuit. The reference voltage for both comparators is
2V and -2V respectively. The comparator part of the ultrasonic circuit will convert
the analog signal from amplifier part of ultrasonic circuit into the digital form, which
is either logic 1(5V) or logic 0(0V). This digital signal will be used to trigger the
input capture pins of the DsPIC30F4013 microcontroller. Figure 3.6(d) shows the
ultrasonic sensor used.

(a)

(b)

(c) (d)
Figure 3.6 Ultrasonic receiver circuit (a) Schematic for amplifier part
(b) Schematic for comparator Part (c) Picture for ultrasonic receiver circuit
(d) Ultrasonic sensor
19

3.2.1.6 Infrared Circuit

Figure 3.7 and figure 3.8 show the circuit for the IR sensor that being used in
the follower robot. Figure 3.7 shows the IR emitter and receiver. The voltage from
the IR receiver is fed to the LM324 comparator for the comparison with the reference
voltage as shown in figure 3.8. The digital output (logic 1 and logic 0) of the
comparator circuit will be sent to the DsPIC30F4013 microcontroller. The output of
the comparator circuit is logic 1 (5V) if the IR sensor detects the obstacle and logic 0
(0V) if no obstacle is detected. The maximum distance that the IR sensor can sense is
5-10 cm.

(a) (b)

Figure 3.7 IR receiver and emitter (a)schematic (b) Picture

(a) (b)
Figure 3.8 LM324 Comparator Circuit (a) schematic (b) Picture
20

3.2.2 Electrical Design for the Leader Robot

This section shows the electrical design for the leader robot. The circuits are
the DsPIC30F4013 and the ultrasonic transmitter circuit.

3.2.2.1 DsPIC30F4013 circuit

Figure 3.9 shows the DsPIC30F4013 circuit for the leader robot. The main
function of the circuit is to generate a 40 kHz pulse width modulation (PWM) signal
to the ultrasonic transmitter circuit. This circuit also generates radio frequency signal
through the radio frequency transmitter.

(a) (c)
Figure 3.9 DsPIC30F4013 circuit (a) Schematic (b) Picture
21

3.2.2.2 Ultrasonic Transmitter Circuit

Figure 3.10 shows the ultrasonic transmitter circuit in the leader robot. This
circuit utilizes the MAX 232 chip to convert the PWM signal from 0 to 5V to 0 to
10V. It is to increase the amplitude of the 40 kHz electrical signal from the
microcontroller. After the conversion, this signal will be sent to the ultrasonic sensor.
The ultrasonic sensor will convert the electrical signal to ultrasonic wave.

(a) (b)
Figure 3.10 Ultrasonic transmitter circuit (a) Schematic (b) Picture

3.2.3 Radio Frequency (RF) module

Figure 3.11 and figure 3.12 shows the radio frequency modules used in leader
robot and follower robot. The module in figure 3.11 is the radio frequency
transmitter while the module in figure 3.12 is the radio frequency receiver. The
function of the module is to transmit and receive the data in the form of radio
frequency signal.
22

Figure 3.11 Radio frequency transmitter module

Figure 3.12 Radio frequency receiver module

3.3 Mechanical Design

The mechanical design for the follower robot and the leader robot will be
discussed in this sub-subsection.

3.3.1 Mechanical Design for the Follower Robot

The body of the robot follower was built with the Perspex. It is because the
Perspex is light, rigid and low cost. The wheel of the robot was bought from Cytron
Sdn Bhd.

Figure 3.13 shows the top view of the follower robot. 3 ultrasonic sensors
were placed on the top of the Perspex as shown in figure 3.13. The dimension in the
figure is in millimeter (mm).
23

Ultrasonic
Sensors

Wheel
Coupling DsPIC Circuit
Board
Wheel
Figure 3.13 Top View for the Follower Robot
24

Figure 3.14 shows the front view of the follower robot. From the figure, there
are three levels for robot follower. Circuits such as infrared sensor circuit, motor
driver circuit, power supply circuit and motor driver circuit were placed on the first
level. The DC motors are also placed on the first level. Circuits such as the ultrasonic
receiver circuit and the DsPIC30F4013 circuit were placed on the second level. The
three ultrasonic sensor receiver circuits were put on the third level, which is the top
level of the follower robot.

Ultrasonic
Sensor
Copper PCB
Stand

Third
Level

Second
Level

First
Level

Coupling
DC Motor

Figure 3.14 Front View of the Follower Robot


25

Figure 3.15 shows the side view of the follower robot. The Wheel of the
robot follower is 40mm in radius.

Ultrasonic
Sensor

Third
Level

Second
Level

First
Level

Wheel Copper PCB


Stand

Figure 3.15 Side View of the Follower Robot

Figure 3.16 shows the design of the coupling used in the follower robot. The
material used to make the coupling is aluminum.

Figure 3.16 Coupling Design


26

Figure 3.17 shows the actual picture of the follower robot.

Figure 3.17 Actual Picture of The Follower Robot


27

3.3.2 Mechanical Design for the Leader Robot

Figure 3.18 shows the top view for the leader robot. The body of the leader
robot is made from the Perspex. The DsPIC30F4014 circuit and the ultrasonic sensor
circuit were placed on the top of the robot.

DsPIC30F4013
Circuit

Ultrasonic
Sensor
Ultrasonic
Sensor

105mm

75mm

Perspex
Ultrasonic
Sensor

Figure 3.18 Top View for Leader Robot

Figure 3.19 and figure 3.20 show the side view and front view respectively.
There are two levels for the leader robot. Power supply circuit and ultrasonic
transmitter circuit were placed on the first level. The second level is the top level as
shown in figure 3.18. The L-Shape Aluminum is attached to a string. The string will
be pulled by hand to move the leader robot.
28

Ultrasonic
Sensor

TOP LEVEL

First Level

Figure 3.19 Side View of the Leader Robot

Ultrasonic
Sensors

L-Shape Aluminum Copper PCB


Stand Perspex

Figure 3.20 Front View of the Leader Robot


29

Figure 3.21 shows the actual picture of the leader robot.

Figure 3.21 Actual Picture of Leader Robot


30

3.4 Software Implementation

This sub-section introduces the software implementation for the leader robot
and follower robot. Besides, the interaction between the leader robot and follower
robot also will be discussed in this section.

3.4.1 Interaction between the Leader Robot and the Follower Robot

Figure 3.22 shows the flow chart of the interaction between the leader robot
and the follower robot. Firstly, the leader robot transmits the radio frequency signal
to the follower robot by using radio frequency module. This signal carries the data in
byte. The value of the data is 99(hexadecimal value). The function of this signal is
not to tell the robot follower the direction of the robot. Instead, its function is to
synchronize the time for transmission of ultrasonic signal in the leader robot and the
reception of the ultrasonic signal in the follower robot.

After the follower robot received the radio frequency signal, the
microcontroller in the follower robot will start the timer. This process is called
synchronization of time for both the leader robot and the follower robot. The timer is
used to calculate the time of flight, which is the time required for the ultrasonic
signal transmit from leader robot until it is being received by the follower robot.
Once the follower start the timer, the leader robot will send the ultrasonic signal
immediately. Finally, after the follower obtain the time of flight, it will calculate the
position of the leader by using mathematical equation as given in section 3.5.
31

Start

Leader transmits RF signal to


Follower

Follower starts the timer

Leader robot transmits


ultrasonic signal

Follower robot receives the


signal and position the leader
robot

Figure 3.22 Flow Chart of Interaction between Leader Robot and Follower Robot

3.4.2 Software Implementation for the Leader Robot

Figure 3.23 shows the flow chart for the algorithm of the leader robot. The
algorithm is programmed in the DsPIC30F4013 microcontroller.

The initialization part of the algorithm is used to initialize the Universal


Synchronous Asynchronous Receiver Transmitter (USART), Pulse Width
Modulation (PWM), digital input and the output pin.

The microcontroller communicates with the radio frequency (RF) module


through the USART interface. The data 100110012 will be sent to the radio
frequency module through the USART. The baud rate for the USART used in the
microcontroller is set to 1200 bit per second. This baud rate is the maximum rate that
the RF module can communicate with the microcontroller. Higher baud rate will
cause the RF module unstable. After the radio frequency module received the data
from the microcontroller, the module will send the radio frequency signal that
contains the data.
32

After transmitting the RF signal, the microcontroller is doing nothing for


0.1634025 second before it generates a 40 kHz PWM signal to the ultrasonic
transmitter circuit. The frequency of the PWM signal is set to 40 kHz because this
signal will be used by the ultrasonic sensor (transmitter). The ultrasonic sensor is
sensitive to 40 kHz signal. It is able to transmit the 40 kHz wave in the air once the
40 kHz PWM signal is received.
Before transmitting another RF signal, the microcontroller will do nothing for
the next 0.01 second. After the 0.01 second, the whole process will repeat again.

Start

Initialization

Transmit RF signal

Delay for 0.1634025 seconds

Transmit Ultrasonic Signal

Delay for 0.01 second

Figure 3.23 Flow Chart for the algorithm of the Leader Robot
33

3.4.3 Software Implementation for the Follower Robot

Figure 3.24 shows the flow chart for the algorithm in the follower robot. The
feature of the microcontroller such as the input capture, USART and PWM are used.

Start

No Search for RF signal by 360


Get RF signal?
degree turning.

Yes

No
Get ultrasonic Search for ultrasonic signal
signal? by 360 degree turning.

Yes

Calculate the position of the


leader robot.

Yes
Obstacle Detected? Avoid the Obstacle.

No

Drive toward Leader


Robot.

Figure 3.24 Flow chart for the general algorithm of the Leader Robot

Figure 3.25 shows the flow chart for the reception of the RF signal. Initially,
the timer 4 will be switched on. The microcontroller will wait the RF signal that was
transmitted from the leader robot. If the RF signal is not received within 0.2 second,
the timer 4 will interrupt the program. Then the follower robot will rotate 360 degree
on the spot to search the RF signal. If the RF signal is received, it will check whether
34

the data carry by the RF signal is equal to 100110012. If the data is correct, it will
proceed to next instruction. Otherwise, the follower robot will wait for another RF
signal.

Start

Initialize and start the timer 4.

No Search for RF signal by


Get RF signal 360 degree turning.
within 0.2 second?

Yes

No
Data==10011001? Wait for the ultrasonic signal.

Yes

Wait for the ultrasonic signal.

Figure 3.25 Reception of the RF Signal

Figure 3.26 shows the flow chart of the reception of the ultrasonic signal.
After the RF signal is received, the input capture, timer 3 and timer 1 will start. Then
the follower robot will wait for the ultrasonic signal. If the ultrasonic signal is not
received within 2.9 ms, the timer 1 will interrupt the program. The follower robot
will search for the ultrasonic signal. If the ultrasonic signal is received by the
follower robot, this will trigger the input capture pin of the microcontroller and
capture the value of timer 3. This timer value will be used in the formula to calculate
the distance of each ultrasonic sensor (receiver) in the follower robot from the leader
robot. By using the information of distance obtained, the position of the leader will
be calculated.
35

Start

Initialize and start the input


capture, timer 3 and timer 1.

Get Ultrasonic Search for RF signal by 360


No
within 2.9 ms? degree turning.

Yes

Position the
Leader Robot?

Yes

End

Figure 3.26 Reception of the Ultrasonic Signal

Figure 3.27 shows the behavior of the follower robot after locating the leader
robot. After locating the leader robot, the follower robot will check whether there is
obstacle around the environment. If obstacles are detected, avoiding the obstacle is
the priority before moving toward the leader. If no obstacle is detected, the follower
robot will drive forward, backward, turn left or turn right. The minimum distance
between the leader robot and follower robot is 15 cm. If the distance is less than 15
cm, the follower will drive backward. If the distance is greater than 15 cm, the
follower robot will drive forward. This is to avoid the collision between the follower
robot and the leader robot.

After that, the algorithm will start again as shown in figure 3.24.
36

Start

Position the Leader.

Obstacle Detected? Yes


Avoid the obstacle

No

Minimum distance No
between Leader and Drive Backward
Follower, D >= 20 cm?

Yes

Relative angle of
No Follower robot turn left to
Leader from Follower,
right while moving forward
Θ == 90 degree?

Yes

Drive toward the leader robot


in straight line

End

Figure 3.27 Algorithm for Movement of the Follower Robot


37

3.5 Positioning the Leader Robot

This sub-section will discuss the way that the location of the leader robot can
be located. Figure 3.28 shows the relative angle of the leader robot from the follower
robot. The rectangle represents the leader robot while the circle represents the
ultrasonic sensors that are placed on the follower robot. The (X, Y) represent the
coordinate of the leader robot with reference to the origin of the follower robot.

Origin

Figure 3.28 Relative Distance and Angle of Leader Robot

Distance A, distance B and distance C are the distances between the


ultrasonic sensors and the leader robot. The unit of the distance is in centimeter (cm).
These distances can be calculated based on the formula:

Distance = Vsound * Timer_value* 0.00000005*100 (3.1)


Where,
Vsound = Velocity of sound, 340 m/s
Timer_value = Value of timer captured by input capture in
microcontroller
38

The coordinate of the leader robot can be calculated by using the equation 3.2
and the equation 3.3 after the distance A, B and C are obtained. The Unit of X and Y
are in centimeter (cm).

X = ((B+C)*(B-C)+100)/20 (3.2)
Y = ((B+A)*(B-A)+100)/20 3.3)

After the coordinate (X,Y) of the leader robot is obtained, the angle Θ can be
calculated by using equation 3.4. The unit of the angle is in degree (°).

Angle, Θ = tan (3.4)

The distance, D is used to indicate the distance leader robot and follower
robot. The distance D can be calculated by using equation 3.5. The unit of D is in
centimeter (cm).

D= (X-10) (3.5)

The distance, D and angle, Θ are used in the algorithm of the follower robot
to obtain the position the leader robot.
CHAPTER 4

RESULT AND DISCUSSION

4.1 Reliability of the Positioning System for the Follower Robot

10 datas for the measured distance between leader robot and follower robot
using the positioning system on the follower robot was collected and recorded in
table 4.1. Table 4.1 shows the average measured distance for the corresponding
actual distance.

Figure 4.1 shows the graph about the actual distance and the average
measured distance versus the number of data. The dash line represents the measured
distance while solid line represents the actual distance. Based on figure 4.1, the dash
line and the solid line almost overlap with minor difference when measured from 40
cm to 99 cm. It shows that the follower robot can locates the leader robot accurately.

Number, i Actual Measured Distance (cm)


Distance (cm) 1 2 3 Average
1 10 9.0 9.5 9.8 9.4
2 20 19.5 20.2 21.3 20.3
3 30 29.7 30.2 30.7 30.2
4 40 39.5 40.1 41.2 40.3
5 50 50.9 51.0 51.2 51.0
6 60 60.5 60.2 61.1 60.6
7 70 70.5 70.1 71.2 70.6
8 80 79.5 81.5 80.3 80.4
9 90 90.5 92.6 91.2 91.4
10 99 98.1 99.6 99.3 99.0

Table 4.1 Actual Distance and Measured Distance


40

100
90 Average
Measured
80 Distance
Actual
70
Distance
Distance (cm)

60
50
40
30
20
10
0 Number, i
1 2 3 4 5 6 7 8 9 10
Figure 4.1 Actual Distance and Measured Distance

The difference between average measured distance and the actual distance
between the follower robot and leader robot may due to quantization error and
random noise in the robots.

Quantization error results from the fact that the measurement of time of flight
is using the digital counter in the microcontroller. The counter counts the integer
value rather than the arbitrary real number. For example, object that is actually 12.3
units of count will be measured to be only 14 units of count. It is because the counter
will stop to count before count 15.

Random noise is the error result from the environment. For example, the
speed of the sound is affected by humidity, temperature, barometric pressure in the
air. The velocity of sound is assumed to 340 m/s regardless the change in velocity of
sound of the environment factor. As a result, the distance calculated using equation
3.1 may not accurate due the value of the actual velocity of sound is not 340 m/s.
41

4.2 Ability of the Follower Robot

The follower robot can follow the leader robot to reverse, forward, turns left
and turns right. When following the leader robot, the follower robot can maintain a
safety distance, approximately 15 cm with the leader robot, to avoid the collision
with leader robot. Besides, the follower robot can also perform some basic obstacle
avoidance.

Figure 4.2 shows the follower robot follows the leader robot in straight line
path when the leader robot is moving in straight line. Figure 4.3 shows the follower
robot can turn right while moving forward when the leader robot turns right. Figure
4.4 shows the follower robot can turn left while moving forward when the leader
robot turn left. The forward, turn left and turn right motions of leader and follower
behavior are shown in the video NUMBER_1 in the disc that accompanied with this
thesis.

Leader
Robot

Follower
Robot

Figure 4.2 Move Forward



42

Leader
Robot

Follower
Robot

Figure 4.3 Turn Right

Figure 4.4 Turn Left

Figure 4.5 shows the follower robot reverses when the leader robot reverse
and the minimum distance between the leader robot and follower robot is less than
15 cm. This motion is shown in the video NUMBER_2 in the disc that accompanied
in this thesis. Figure 4.6 shows the follower robot able to reverse and turn right
sharply when the leader robot reverses more than 3 second. This motion is shown in
the video NUMBER_3 in the disc that accompanied with this thesis.
43

Figure 4.5 Reverse

Figure 4.6 Reverse and Turn right Sharply

The follower robot can also avoid some obstacle around the environment. Six
infrared (IR) sensors are arranged at the base of follower robot as shown in figure 4.7.
The bold arrow in figure 4.7 represents the path and direction that each IR sensor
will sense to determine the existence of the obstacle. The reliable maximum distance
that the IR sensor can sense is around 5 cm. Table 4.2 shows the action performed by
the follower robot if IR sensor detects the obstacle. The various motions performed
by the follower robot when obstacle is detected are designed to aid the follower robot
to perform the task following. As a result, the algorithm for the obstacle avoidance is
not complicated.
44

Position of IR sensor Motion


Position 1 Reverse
Position 2 Turn Right
Position 3 Turn Left
Position 4 Turn Right
Position 5 Turn Left
Position 6 Stop

Table 4.2 Motion Performed if Obstacle Detected

Position 1
Position 2 Position 3

IR Sensor
Board

Position 4

Position 5

IR Sensor
Board

Position 6

Figure 4.7 Arrangement of the IR Sensor from Top View


CHAPTER 5

CONCLUSION

5.1 Conclusion

The main objective of this project is to design and construct a robot that can
follow another robot autonomously. In this project, a follower robot is design and
built to follow the leader robot successfully in reverse, forward, turn left and turn
right motions. Besides, if the leader robot is in reverse motion continuously for 3
second, the follower robot can turn right backward sharply to allow the leader robot
the pass through the follower robot. As a result, the main objective is achieved.

The second objective is to integrate the concept of collision avoidance into


the leader/follower behavior. In the project, the follower robot constructed can avoid
from the collision with the leader robot. The follower robot can maintain a 15 cm
distance with the leader robot whether the leader robot is in reverse, forward, turn
left or turn right motions. Therefore, the follower robot has the ability the avoid
collision with the leader robot.

The Last objective of the robot is to construct the follower robot that can
perform the task following around the environment with obstacle. The follower robot
is installed with infrared (IR) sensors to avoid obstacle in its path. As a result, the last
objective is achieved.

In conclusion, Follower robot can perform the task following successfully.


All objectives of this project have been successfully achieved.
46

5.2 Limitation

Although all the objectives of the project are successfully fulfilled, there are a
few limitations for the follower robot.

One of the limitations is about the infrared (IR) sensor. The infrared sensor is
very sensitive to the sun light. The infrared will give random output if the IR sensor
exposed to the sun light. As a result, the follower robot can only perform the obstacle
avoidance in the environment without any sun light.

Besides, the follower robot also may lose the leader robot if the environment
has too many obstacles. It is because the obstacle may contribute the false reading to
the ultrasonic sensor. This is because the ultrasonic sensor may capture the ultrasonic
signal reflected by the obstacle in the environment.

5.3 Future Improvement

The limitation of the IR sensor can be solved by using a higher cost analog
sensor, for example, the analog sensor manufactured by SHARP as shown in figure
5.1. It has high immunity to the different lightning condition. As a result, the
follower robot can perform the task following with obstacle avoidance in the
environment where there is sun light.

Figure 5.1 Analog Sensor from SHARP


47

The ability of the follower robot when performing the following task in
environment with various obstacles can be improved by modified the positioning
system in the follower robot. The ultrasonic signal can be captured by the aluminum
cone as shown in figure 5.2 [6]. The ultrasonic sensor can have higher immunity to
the ultrasonic wave reflected by an obstacle. As a result, this can improve the ability
of the follower robot when perform the following task within the complex
environment.

Figure 5.2 Aluminum Cone


48

REFERENCES

1. Gan Hui Wen. Development of a road vehicle model for road vehicle convoy
System. 2007

2. Jo˜ao Sequeira and Andr´e Gonc¸alves, Andr´e Godinho. Low cost Sensing For
Autonomous Car Driving In Highways. 2007.

3. Rahul Sukthankar. RACCOON: A Real-time Autonomous Car Chaser Operating


Optimally at Night. 2008

4. J. Giesbrecht and DRDC Suffield. Leader/Follower Behaviour Using the SIFT


Algorithm for Object Recognition. 2006.

5. Adrian Korodi, Alexandru Codrean, Liviu Banita,Constantin Olosencu. Aspects


Regarding the Object Following Control Procedure for Wheeled Mobile Robots.
2008.

6. Rozan Boudville. Obstacle detection and avoidance mobile robot. Universiti


Teknologi Malaysia. 2004.

7. Oreback. Component Framework for Autonomous Mobile Robots. Doctoral


Thesis,Stockholm Sweden, 2004.

8 Ngo, T. D., Raposo, H., Schioler, H. Being Sociable: Multirobots with Self-
sustained Energy. Proceedings of the 15th IEEE Mediterranean Conference on
Control and Automation, Athens, Greece,.
49

9 Remondini D., Saffiotti A., A Modular. Hierarchical, Reconfigurable Controller


for Autonomous Robots. Proceedings of the IEEE International Conference on
Methods and Models in Automation and Robotics (MMAR), Miedzyzdroje,
Poland. 2006.

10. Smart W. D., Kaelbling L. P. Effective Reinforcement Learning for Mobile


Robots. The Proceedings of the International Conference on Robotics and
Automation ICRA. 2002.

11. Thomas Kunkel. Hardware Architecture of a Swarm of Robots. December 2006.

12. Rodney Heil. A Trilaterative Localization System for Small Mobile Robots in
Swarms. 2004.

13. Jonathan Bisson1, Fran_cois Michaud1 and Dominic L_etourneau1. Relative


Positioning of Mobile Robots Using Ultrasounds.

14. Lim Wie Keat. Sensor-based Obstacle Avoidance Strategy for Mobile Robot.
Universiti Teknologi Malaysia. 2001.
50

APPENDIX A

Program in DsPIC30F4013 Microcontroller for The Follwer Robot


;============================================================
; File Name: main_program.c
:Programmer : BOON KHANG HUA
; Date : 27/1/2005
; Supervisor : Dr. Johari Halim Shah Osman
;===========================================================
#include "p30f4013.h"
#define FCY 20000000UL
#include "libpic30.h"
#include "math.h"

/*Initialization for Oscillator*/


_FOSC(CSW_FSCM_OFF & XT_PLL8 );
FWDT(WDT_OFF);

Funtion Declaration for selection_mode.c*/


void initialization_selection_mode(void);
int selection_mode(void);
void initialization_uart1(void);
int uart_call(char latest_position);
void initialization_timer_3(void);
float Input_Capture_call(char *latest_position);
void intialization_decimal_call(void);
void decimal_call(float value_float);
void initialization_timer_1(void);
void initialization_timer_2(void);
void initialization_timer_4(void);
void initialization_timer_5(void);
void intialization_pwm(void);
void motor_R_F(void);

int main(void)
{
/*initialization for motor control*/
initialization_timer_5();
initialization_timer_4();
initialization_timer_2();
initialization_pwm();
/*Initialization timer 1*/
initialization_timer_1();

/* Initialzation for selection_mode.c*/


initialization_selection_mode();

/* Initialization for UART.c */


initialization_uart1();
51

/* Initialization for timer 3 in Input_Capture.c */


initialization_timer_3();

/* Initialization for Decimal_Display.c */


intialization_decimal_call();

int select;
volatile int display;
float distance;
char temp_latest_position;
do
{
select=selection_mode();

switch(select)
{
case 0: decimal_call(88);
display=uart_call(temp_latest_position);
decimal_call(display);
decimal_call(89);
while(1);
break;
case 1: decimal_call(32);
distance=Input_Capture_call(&temp_latest_position);
decimal_call(33);
while(1);
break;
case 2: decimal_call(88);
do
{
display=uart_call(temp_latest_position);
distance=Input_Capture_call(&temp_latest_position);
decimal_call(distance);
}while(1);
break;
case 3: motor_L_spot_max();
while(1);
default:break;
}while(1);
return 0;
}
/*--------------------------------------------------------------------------------------------*/
int selection_mode(void)
{
LATBbits.LATB2=0;
LATBbits.LATB3=0;
decimal_call(22);
int selection=0;
label1:
52

if(_RB0==0)
{

while(_RB0==0)
{
}
__delay_ms(5);
if(_RB0==1)
{
selection++;
}
}
if(selection==4)
{
selection=0;
}

switch(selection)
{
case 0:
LATBbits.LATB2=1; LATBbits.LATB3=1;
break;
case 1:
LATBbits.LATB2=1; LATBbits.LATB3=0;
break;
case 2:
LATBbits.LATB2=0; LATBbits.LATB3=1;
break;
case 3:
LATBbits.LATB2=0; LATBbits.LATB3=0;
break;
default:
break;}
if(_RB1==1){
goto label1;}
else if (_RB1==0){
while(_RB1==0){
;}}
return(selection);}
//------------------------------------------------------------------------------------------
void initialization_selection_mode(void)
{
ADPCFG=0xFFFF;
TRISBbits.TRISB0=1;
TRISBbits.TRISB1=1; //Configure RB1 as input pin for SW2(Execution mode)
TRISBbits.TRISB2=0; //Configure RB2 as output pin for LED1
TRISBbits.TRISB3=0; //Configure RB3 as output pin for LED2
TRISBbits.TRISB6=1;
TRISBbits.TRISB7=1;
TRISBbits.TRISB8=1;
53

TRISBbits.TRISB5=1;
TRISCbits.TRISC13=1;
TRISCbits.TRISC14=1;}

void initialization_uart1(void)
{
/*Transmitter& Receiver*/
U1MODE=0x8000;
U1STA=0;
U1STAbits.UTXEN=1;
U1BRG=1041;
IEC0bits.U1TXIE=0;
IEC0bits.U1RXIE=0;
__delay_ms(2000);}

void initialization_timer_3(void)
{
TMR3=0;
T3CON=0x0000; //Make sure Timer 3 is off
PR3=0xFFFF;
IEC0bits.T3IE=0; //disable interrupt bit for timer3
T3CONbits.TON=1;}

//Initialization for output pin for decimal display


void intialization_decimal_call(void)
{
TRISFbits.TRISF5=0;TRISFbits.TRISF4=0;
TRISFbits.TRISF1=0;TRISFbits.TRISF0=0;
TRISDbits.TRISD3=0;TRISDbits.TRISD2=0;
TRISFbits.TRISF6=0;TRISAbits.TRISA11=0;}

void initialization_timer_1(void)
{
T1CON=0x0000;
TMR1=0;
IEC0bits.T1IE=1; //Enable timer 1 interrupt
PR1=58824;}

void initialization_timer_2()
{
T2CON=0x0000;
TMR2=0x0000;
PR2=10000; //set PWM to 5 kHz
IEC0bits.T2IE=0;}

void initialization_timer_4(void)
{
T4CON=0x0000;
TMR4=0;
PR4=65535;
54

IEC1bits.T4IE=1;
IFS1bits.T4IF=0;}

void initialization_timer_5(void)
{
T5CON=0x0000;
TMR5=0;
PR5=65535;
IEC1bits.T5IE=1;
IFS1bits.T5IF=0;}

;============================================================
; File Name: decimal.c
:Programmer : BOON KHANG HUA
; Date : 27/1/2005
; Supervisor : Dr. Johari Halim Shah Osman
;============================================================
#include "p30f4013.h"
#include "libpic30.h"
void decimal_call(float value_float)
{
int value;
int accumulator_10=0, accumulator_1=0;
value=(int)value_float;
if(value>=100){
value=value-100;
LATBbits.LATB2=0;
LATBbits.LATB3=0;
__delay32(10000000);}
do{
value=value-10;
if(value>=0){
accumulator_10++;}
else if(value<0){
break;}
}while(1);
value=value+10;
do{
value=value-1;
if(value>=0){
accumulator_1++;}
else if(value<0){
break;}
}while(1);
value=0;
LATFbits.LATF4=0;
LATFbits.LATF1=0;
LATFbits.LATF0=0;
LATFbits.LATF5=0;
LATDbits.LATD2=0;
55

LATDbits.LATD1=0;
LATAbits.LATA11=0;
LATDbits.LATD3=0;
switch(accumulator_10)
{
case 0: LATFbits.LATF4=0;
LATFbits.LATF1=0; LATFbits.LATF0=0;LATFbits.LATF5=0;
break;
case 1: LATFbits.LATF4=0;
LATFbits.LATF1=0; LATFbits.LATF0=0;LATFbits.LATF5=1;
break;
case 2: LATFbits.LATF4=0;
LATFbits.LATF1=0;LATFbits.LATF0=1;LATFbits.LATF5=0;
break;
case 3: LATFbits.LATF4=0;
LATFbits.LATF1=0;LATFbits.LATF0=1;LATFbits.LATF5=1;
break;
case 4: LATFbits.LATF4=0;
LATFbits.LATF1=1;LATFbits.LATF0=0;LATFbits.LATF5=0;
break;
case 5: LATFbits.LATF4=0;
LATFbits.LATF1=1;LATFbits.LATF0=0;LATFbits.LATF5=1;
break;
case 6: LATFbits.LATF4=0;
LATFbits.LATF1=1;LATFbits.LATF0=1;LATFbits.LATF5=0;
break;
case 7: LATFbits.LATF4=0;
LATFbits.LATF1=1;LATFbits.LATF0=1;LATFbits.LATF5=1;
break;
case 8: LATFbits.LATF4=1;
LATFbits.LATF1=0;LATFbits.LATF0=0;LATFbits.LATF5=0;
break;
case 9: LATFbits.LATF4=1;
LATFbits.LATF1=0;LATFbits.LATF0=0;LATFbits.LATF5=1;
break;
default: break;
}
switch(accumulator_1)
{
case 0: LATDbits.LATD2=0;
LATFbits.LATF6=0;LATAbits.LATA11=0;LATDbits.LATD3=0;
break;
case 1: LATDbits.LATD2=0;
LATFbits.LATF6=0;LATAbits.LATA11=0;LATDbits.LATD3=1;
break;
case 2: LATDbits.LATD2=0;
LATFbits.LATF6=0;LATAbits.LATA11=1;LATDbits.LATD3=0;
break;
case 3: LATDbits.LATD2=0;
LATFbits.LATF6=0;LATAbits.LATA11=1;LATDbits.LATD3=1;
56

break;
case 4: LATDbits.LATD2=0;
LATFbits.LATF6=1;LATAbits.LATA11=0;LATDbits.LATD3=0;
break;
case 5: LATDbits.LATD2=0;
LATFbits.LATF6=1;LATAbits.LATA11=0;LATDbits.LATD3=1;
break;
case 6: LATDbits.LATD2=0;
LATFbits.LATF6=1;LATAbits.LATA11=1;LATDbits.LATD3=0;
break;
case 7: LATDbits.LATD2=0;
LATFbits.LATF6=1;LATAbits.LATA11=1;LATDbits.LATD3=1;
break;
case 8: LATDbits.LATD2=1;
LATFbits.LATF6=0;LATAbits.LATA11=0;LATDbits.LATD3=0;
break;
case 9: LATDbits.LATD2=1;
LATFbits.LATF6=0;LATAbits.LATA11=0;LATDbits.LATD3=1;
break;
default: break;}}
;============================================================
File Name: input_capture.c
:Programmer : BOON KHANG HUA
; Date : 27/1/2005
; Supervisor : Dr. Johari Halim Shah Osman
;============================================================
#include "p30f4013.h"
#include "math.h"
#include "C:\Users\boon1987\Desktop\Follower_no_sensor_version_2\UART1.c"
#define M _RB6
#define L _RB7
#define LS _RB8
#define B _RB5
#define R _RC13
#define RS _RC14

void initialization_input_capture(void);
void decimal_call(float value_float);
void __attribute__((__interrupt__,auto_psv)) _T1Interrupt(void);
volatile int flag_signal;
volatile int ultrasonic_data_acquired=0;
void calculate_distance_and_degree(float * distance, float * degree);
volatile float smaller_distance_IC7=0;
volatile int check_shortest_distance_status=0;
volatile int timer5_counter=0;
volatile int timer5_counter_check=0;
volatile unsigned int exit_from_916=0;
volatile int timer5_condition=0;
volatile unsigned int B_L_count=0;
volatile unsigned int B_R_count=0;
57

volatile unsigned int B_L=0;


volatile unsigned int B_R=0;
void set_timer5(void);
void reset_timer5(void);
void motor_S(void);
void motor_L_spot_slow(void);
void motor_L_spot_max(void);
void motor_R_spot_slow(void);
void motor_R_spot_max(void);
void motor_B_L_S(void);
void motor_B_L_F(void);
void motor_B_L_M(void);
void motor_B_R_F(void);
void motor_B_R_M(void);
void motor_B_R_S(void);
void motor_F_F(void);
void motor_F_M(void);
void motor_L_F(void);
void motor_L_M(void);
void motor_L_L(void);
void motor_R_F(void);
void motor_R_M(void);
void motor_R_L(void);
void motor_B_F(void);
void motor_B_M(void);
void motor_B_Low(void);

float Input_Capture_call(char * latest_position)


{
float distance_IC7,degree;
float searching_status=0.0;
unsigned int IR_Condition=0;
unsigned int IR_Result=0;
initialization_input_capture();
flag_signal=0;
TMR1=0;
T1CONbits.TON=1;
while(IC1CONbits.ICBNE==0 && !flag_signal);
while(IC2CONbits.ICBNE==0 && !flag_signal);
while(IC7CONbits.ICBNE==0 && !flag_signal);
if(check_shortest_distance_status==1)
{
flag_signal=1;
ultrasonic_data_acquired=1;}
if(flag_signal_interrupt_waiting_ultrasonic<=458)
{
searching_status=1.0;}
else if(flag_signal_interrupt_waiting_ultrasonic<=2289)
{
searching_status=3.0;}
58

else if(flag_signal_interrupt_waiting_ultrasonic<=5035)
{
searching_status=4.0;}
else if(flag_signal_interrupt_waiting_ultrasonic>5035)
{
searching_status=5.0;}
if(flag_signal==1)
{
T1CONbits.TON=0; //stop the waiting signal for ultrasonic signal
T4CONbits.TON=1; //guarding how much the car rotate to find
signal
if(*latest_position!='R' && *latest_position!='L' && *latest_position!='S')
{
*latest_position='L';
}
if(*latest_position=='L')
{
if(flag_signal_interrupt_waiting_ultrasonic<=458){
check_shortest_distance_status=0;
IR_Condition=7;
IR_Result=IR_Check(IR_Condition);}
else if(flag_signal_interrupt_waiting_ultrasonic>458 &&
flag_signal_interrupt_waiting_ultrasonic<=2289){
check_shortest_distance_status=1;
IR_Condition=6;
IR_Result=IR_Check(IR_Condition);
if(ultrasonic_data_acquired==1)
{
calculate_distance_and_degree(&distance_IC7,&degree);
if(smaller_distance_IC7==0)
{
smaller_distance_IC7=distance_IC7;}
else
{
if(smaller_distance_IC7>distance_IC7)
{
smaller_distance_IC7=distance_IC7;
}}}}
else if(flag_signal_interrupt_waiting_ultrasonic>2289 &&
flag_signal_interrupt_waiting_ultrasonic<=3204){
check_shortest_distance_status=1;
if(ultrasonic_data_acquired==1) {
calculate_distance_and_degree(&distance_IC7,&degree);
if(smaller_distance_IC7==0)
{
;}
else if( smaller_distance_IC7<=(distance_IC7+5.0) &&
smaller_distance_IC7>=(distance_IC7-5.0)) {
motor_S();
59

check_shortest_distance_status=0;}}
IR_Condition=6;
IR_Result=IR_Check(IR_Condition);}
else if(flag_signal_interrupt_waiting_ultrasonic>3204
&&flag_signal_interrupt_waiting_ultrasonic<=5035){
check_shortest_distance_status=1;
if(ultrasonic_data_acquired==1)
{
calculate_distance_and_degree(&distance_IC7,&degree);
if(smaller_distance_IC7==0)
{
;
}
else if( smaller_distance_IC7<=(distance_IC7+5.0) &&
smaller_distance_IC7>=(distance_IC7-5.0))
{
motor_S();
check_shortest_distance_status=0;
}
}
IR_Condition=7;
IR_Result=IR_Check(IR_Condition);}
else if(flag_signal_interrupt_waiting_ultrasonic>5035)
{
flag_signal_interrupt_waiting_ultrasonic=459;
smaller_distance_IC7=0.0;
}}
else if(*latest_position=='R')
{
if(flag_signal_interrupt_waiting_ultrasonic<=458){
check_shortest_distance_status=0;
IR_Condition=6;
IR_Result=IR_Check(IR_Condition);}
else if(flag_signal_interrupt_waiting_ultrasonic>458 &&
flag_signal_interrupt_waiting_ultrasonic<=2289){
check_shortest_distance_status=1;
IR_Condition=7;
IR_Result=IR_Check(IR_Condition);
if(ultrasonic_data_acquired==1)
{
calculate_distance_and_degree(&distance_IC7,&degree);
if(smaller_distance_IC7==0) {
smaller_distance_IC7=distance_IC7; }
else
{
if(smaller_distance_IC7>distance_IC7){
smaller_distance_IC7=distance_IC7; }}}}
else if(flag_signal_interrupt_waiting_ultrasonic>2289 &&
flag_signal_interrupt_waiting_ultrasonic<=3204){
check_shortest_distance_status=1;
60

IR_Condition=7;
IR_Result=IR_Check(IR_Condition);
if(ultrasonic_data_acquired==1){
calculate_distance_and_degree(&distance_IC7,&degree);
if(smaller_distance_IC7==0.0){
;}
else if( smaller_distance_IC7<=(distance_IC7+5.0) &&
smaller_distance_IC7>=(distance_IC7-5.0)) {
motor_S();
check_shortest_distance_status=0;}}}
else if(flag_signal_interrupt_waiting_ultrasonic>3204 &&
flag_signal_interrupt_waiting_ultrasonic<=5035){
check_shortest_distance_status=1;
IR_Condition=6;
IR_Result=IR_Check(IR_Condition);
if(ultrasonic_data_acquired==1){
calculate_distance_and_degree(&distance_IC7,&degree);
if(smaller_distance_IC7==0.0){
;}
else if( smaller_distance_IC7<=(distance_IC7+5.0) &&
smaller_distance_IC7>=(distance_IC7-5.0)) {
motor_S();
check_shortest_distance_status=0;}}}
else if(flag_signal_interrupt_waiting_ultrasonic >5035){
flag_signal_interrupt_waiting_ultrasonic=459;
smaller_distance_IC7=0.0;}}
else
{
motor_S();}
IC1CON=0x0000;
IC2CON=0x0000;
IC7CON=0x0000;
goto skip_to_end;}

T1CONbits.TON=0;
TMR1=0;
T4CONbits.TON=0;
TMR4=0;
flag_signal_interrupt_waiting_ultrasonic=0;
smaller_distance_IC7=0;
check_shortest_distance_status=0;
calculate_distance_and_degree(&distance_IC7,&degree);

if(degree>79.5 && degree<81.0)


{
if(distance_IC7>=23.54)
{

reset_timer5();
if(avoid_motion==0) {
61

IR_Condition=1;
IR_Result=IR_Check(IR_Condition);
*latest_position='F';}}
else if(distance_IC7>21.6 && distance_IC7<23.54) {
reset_timer5();
if(avoid_motion==0){
IR_Condition=1;
IR_Result=IR_Check(IR_Condition);
*latest_position='F';}}
else if(distance_IC7>=17.72 && distance_IC7<=21.6){
reset_timer5();
if(avoid_motion==0){
motor_S();
*latest_position='S';
B_L=0;
B_R=0;}}
else if(distance_IC7>14.87 && distance_IC7<17.72){
set_timer5();
if(avoid_motion==0){
IR_Condition=8;
IR_Result=IR_Check(IR_Condition);
*latest_position='B';
B_L=0;
B_R=0;}}
else if(distance_IC7<=14.87){
set_timer5();
if(avoid_motion==0){
IR_Condition=9;
IR_Result=IR_Check(IR_Condition);
*latest_position='B';
B_L=0;
B_R=0;}}}
else if(degree>=0.0 && degree<=79.5){
LATBbits.LATB2=1;
LATBbits.LATB3=0;
if(distance_IC7>=17.72 && distance_IC7<=21.6){
reset_timer5();
if(avoid_motion==0){
IR_Condition=2;
IR_Result=IR_Check(IR_Condition);
*latest_position='R';}}
else if(distance_IC7>21.6){
reset_timer5();
if(avoid_motion==0){
IR_Condition=3;
IR_Result=IR_Check(IR_Condition);
*latest_position='R';}}
else if(distance_IC7>14.87 && distance_IC7<17.72){
set_timer5();
if(avoid_motion==0){
62

IR_Condition=10;
IR_Result=IR_Check(IR_Condition);
*latest_position='A';
B_L=1;
B_R=0;
B_L_count++;}}
else if(distance_IC7<=14.87)
{
set_timer5();
if(avoid_motion==0)
{
IR_Condition=11;
IR_Result=IR_Check(IR_Condition);
*latest_position='A';
B_L=1;
B_R=0;
B_L_count++;}}}
else if(degree >=81.0 && degree<=180.0){
LATBbits.LATB2=0;
LATBbits.LATB3=1;
if(distance_IC7>=17.72 && distance_IC7<=21.6){
reset_timer5();
if(avoid_motion==0){
IR_Condition=4;
IR_Result=IR_Check(IR_Condition);
*latest_position='L';}}
else if(distance_IC7 > 21.6){
reset_timer5();
if(avoid_motion==0){
IR_Condition=5;
IR_Result=IR_Check(IR_Condition);
*latest_position='L';}}
else if(distance_IC7>14.87 && distance_IC7<17.72){
set_timer5();
if(avoid_motion==0){
IR_Condition=12;
IR_Result=IR_Check(IR_Condition);
*latest_position='B';
B_L=0;
B_R=1;
B_R_count++;}}
else if(distance_IC7<=14.87){
set_timer5();
if(avoid_motion==0){
IR_Condition=13;
IR_Result=IR_Check(IR_Condition);
*latest_position='B';
B_L=0;
B_R=1;
B_R_count++;}}}
63

else if(degree>180.0 && degree<=360.0){


motor_S();
decimal_call(99);}
return distance_IC7;
skip_to_end:
return(searching_status);
}

void initialization_input_capture(void){
IC1CON=0x0002;
TRISDbits.TRISD8=1;
IC2CON=0x0002;
TRISDbits.TRISD9=1;
IC7CON=0x0002;
TRISBbits.TRISB4=1;
IC8CON=0x0002;
TRISBbits.TRISB5=1;}

void __attribute__((__interrupt__,auto_psv)) _T1Interrupt(void){


flag_signal=1;
ultrasonic_data_acquired=0;
IFS0bits.T1IF=0;}

void __attribute__((__interrupt__,auto_psv)) _T4Interrupt(void){


IFS1bits.T4IF=0;
flag_signal_interrupt_waiting_ultrasonic++;}

void __attribute__((__interrupt__,auto_psv)) _T5Interrupt(void){


IFS1bits.T5IF=0;
timer5_counter++;}

void reset_timer5(void)
{
if(exit_from_916==1) {
timer5_counter_check++;}
if(timer5_counter_check>=10){
T5CONbits.TON=0;
timer5_condition=0;
timer5_counter=0;
timer5_counter_check=0;
exit_from_916=0;
avoid_motion=0;}}

void set_timer5(void){
if(timer5_condition==0) {
T5CONbits.TON=1;
timer5_condition=1;
timer5_counter=0;
exit_from_916=1;
B_L_count=0;
64

B_R_count=0;
B_L=0;
B_R=0;}
if(timer5_counter>=916){
T5CONbits.TON=0;
avoid_motion=1;
if(B_L==1){
if(B==0){
motor_B_L_S();}
else
{
motor_S();}}
else if(B_R==1){
if(B==0)
{
motor_B_R_S();}
else
{
motor_S();}}
else if(B_R==0 && B_L==0){
if(B_L_count>=B_R_count){
if(B==0){
motor_B_L_S();}
else
{
motor_S();}}
else if(B_L_count<B_R_count){
if(B==0){
motor_B_R_S();}
else
{
motor_S();}}}}}

unsigned int IR_Check(unsigned int IR_Condition){


switch(IR_Condition)
{
case 1: {
if(M==1 && B==1){
motor_S();}
else if(M==1){
motor_B_F();}
else if(B==1){
motor_F_M();}
else if(L==1 && M==0 && R==1){
motor_F_M();}
else if(L==1 && M==0 && R==0){
motor_R_spot_slow();}
else if(L==0 && M==0 && R==1){
motor_L_spot_slow();}
else if(L==0 && M==0 && R==0){
65

if(LS==0 && RS==0){


motor_F_M();}
else if(LS==1 && RS==0){
motor_R_F();}
else if(LS==0 && RS==1){
motor_L_F();}}}
break;
case 2: //Condition 2 mean car suppose to rotate right on the spot
{

if(M==1 && B==0){


motor_B_F();}
else if(R==1 && RS==1){
motor_L_spot_slow();}
else if(R==0 && RS==1){
motor_L_spot_slow();}
else if(R==1 && RS==0){
motor_L_spot_slow();}
else if(R==0 && RS==0){
motor_R_spot_slow();}}
break;
case 3: //Condition 3 mean car suppose to move forward right
{
if(M==1 && B==0){
motor_B_F();}
else if(R==1 && RS==1){
motor_L_spot_slow();}
else if(R==1 && RS==0){
motor_L_spot_slow();}
else if(R==0 && RS==1){
motor_L_spot_slow();}
else if(R==0 && RS==0){
motor_R_F();}}
break;
case 4: //Condition 4 mean car suppose to rotate left on the spot
{
if(M==1 && B==0){
motor_B_F();}
else if(L==1 && LS==1){
motor_R_spot_slow();}
else if(L==0 && LS==1){
motor_R_spot_slow();}
else if(L==1 && LS==0){
motor_R_spot_slow();}
else if(L==0 && LS==0){
motor_L_spot_slow();}}
break;
case 5: //Condition 5 mean car suppose to moving left forward
{
if(M==1 && B==0){
66

motor_B_F();}
else if(L==1 && LS==1){
motor_R_spot_slow();}
else if(L==0 && LS==1){
motor_R_spot_slow();}
else if(L==1 && LS==0){
motor_R_spot_slow();}
else if(L==0 && LS==0){
motor_L_F();}}
break;
case 6: //same as condition 2 but at higher speed
{
if(M==1 && B==0){
motor_B_F();}
else if(R==1 && RS==1){
motor_L_spot_slow();}
else if(R==0 && RS==1){
motor_L_spot_slow();}
else if(R==1 && RS==0){
motor_L_spot_slow();}
else if(R==0 && RS==0){
motor_R_spot_max();}}
break;
case 7: //same as condition 4 but at higher speed
{
if(M==1 && B==0){
motor_B_F();}
else if(L==1 && LS==1){
motor_R_spot_slow();}
else if(L==0 && LS==1){
motor_R_spot_slow();}
else if(L==1 && LS==0){
motor_R_spot_slow();}
else if(L==0 && LS==0){
motor_L_spot_max();}}
break;
case 8:
case 9: {
if(B==0 && IR_Condition==8){
motor_B_M();}
else if(B==0 && IR_Condition==9){
motor_B_F();}
else if(B==1){
motor_S();}}
break;
case 10:
case 11: {
if(B==0 && IR_Condition==10){
motor_B_L_F();}
if(B==0 && IR_Condition==11){
67

motor_B_L_M();}
else if(B==1){
motor_S();}}
break;
case 12:
case 13: {
if(B==0 && IR_Condition==12){
motor_B_R_F();}
if(B==0 && IR_Condition==13){
motor_B_R_M();}
else if(B==1){
motor_S();}}
break;
default: break;}
return 1;}

void calculate_distance_and_degree(float * distance, float * degree){


float distance_IC1,distance_IC2,distance_IC7,a,b,c,X,Y,radian;
unsigned int IC1_data,IC2_data,IC7_data;

IC1_data=IC1BUF;
IC2_data=IC2BUF;
IC7_data=IC7BUF;
IC1CON=0x0000;
IC2CON=0x0000;
IC7CON=0x0000;

distance_IC1=340.0*0.00000005*100.0*IC1_data;
distance_IC2=340.0*0.00000005*100.0*IC2_data;
distance_IC7=340.0*0.00000005*100.0*IC7_data;

a=distance_IC7;
c=distance_IC2;
b=distance_IC1;

X=(((b+c)*(b-c)+10.0*10.0)/(2.0*10.0));
Y=(((b+a)*(b-a)+10.0*10.0)/(2.0*10.0));
radian=atan2f(Y,X);

*distance=distance_IC7;}
;============================================================
; File Name: motor.c
:Programmer : BOON KHANG HUA
; Date : 27/1/2005
; Supervisor : Dr. Johari Halim Shah Osman
;============================================================
========
#include "p30f4013.h"
#define L2 LATBbits.LATB12
#define L1 LATBbits.LATB11
68

#define R2 LATBbits.LATB9
#define R1 LATBbits.LATB10
#define PR OC1RS
#define PL OC2RS
void initialization_pwm(void){
TRISBbits.TRISB12=0;
TRISBbits.TRISB11=0;
TRISBbits.TRISB10=0;
TRISBbits.TRISB9=0;
R1=0; //Left motor forward
R2=0; //Left motor backward
L1=0; //Right motor backward
L2=0; //Right motor forward
/*OC1 pin for left motor*/
OC1R=0; //Set PWM duty cycle to 0
PL=134; //Set PWM duty cycle to 0
OC1CON=0x0006; //Set to PWM mode without fault enabled
IEC0bits.OC1IE=0;
/*OC2 pin for right motor*/
OC2R=0;
PR=0;
OC2CON=0x0006;
IEC0bits.OC2IE=0;

/*Start to send PWM to L293D*/


T2CONbits.TON=1;}

void motor_F_F(void){
L1=1;L2=0;R1=1;R2=0;PR=10000;PL=10000;}
void motor_F_M(void)
{
L1=1;L2=0;R1=1;R2=0;PR=8000;PL=8000;}
void motor_R_F(void)
{
L1=1;L2=0;R1=1;R2=0;PR=5000;PL=10001;}
void motor_R_M(void)
{
L1=1;L2=0;R1=1;R2=0;PR=7000;PL=10001;}
void motor_R_L(void)
{
L1=1;L2=0;R1=1;R2=0;PR=9000;PL=10001;}
void motor_R_spot_slow(void)
{
L1=1;L2=0;R1=0; R2=1;PR=6500;PL=6500;}
void motor_R_spot_max(void)
{
L1=1;L2=0;R1=0;R2=1;PR=10000;PL=10000;}

void motor_L_F(void){
L1=1;L2=0;R1=1;R2=0;PR=10001;PL=5000;}
69

void motor_L_M(void){
L1=1;L2=0;R1=1;R2=0;PR=10001;PL=7000;}
void motor_L_L(void){
L1=1;L2=0;R1=1;R2=0;PR=10001;PL=9000;}
void motor_L_spot_slow(void){
L1=0;L2=1;R1=1;R2=0;PR=6500;PL=6500;}
void motor_L_spot_max(void){
L1=0;L2=1;R1=1;R2=0;PR=10000;PL=10000;}
void motor_B_F(void){
L1=0;R1=0;L2=1;R2=1;PR=10001;PL=10001;}
void motor_B_M(void){
L1=0;R1=0;L2=1;R2=1;PR=7000;PL=7000;}
void motor_B_Low(void){
L1=0;R1=0;L2=1;R2=1;PR=10001;PL=10001;}
void motor_B_L_S(void){
L1=0;R1=0;L2=1;R2=1;PR=10001;PL=5000;}
void motor_B_L_F(void){
L1=0;R1=0;L2=1;R2=1;PR=10001;PL=8000;}
void motor_B_L_M(void){
L1=0;R1=0;L2=1;R2=1;PR=9000;PL=7000;}
void motor_B_R_S(void){
L1=0;R1=0;L2=1;R2=1;PR=5000;PL=10001;}
void motor_B_R_F(void){
L1=0;R1=0;L2=1;R2=1;PR=8000;PL=10001;}
void motor_B_R_M(void){
L1=0;R1=0;L2=1;R2=1;PR=7000;PL=9000;}
void motor_S(void){
L1=0;L2=0;R1=0;R2=0;PR=0;PL=0;
}
;============================================================
===
; File Name: uart.c
:Programmer : BOON KHANG HUA
; Date : 27/1/2005
; Supervisor : Dr. Johari Halim Shah Osman
;===========================================================
#include "p30f4013.h"
#include "uart.h"
#define FCY 20000000UL
#include "libpic30.h"
/*Function Declaration for UART transmission and reception*/
void transmission(void);
int reception(void);
void decimal_call(float value_float);
void __attribute__((__interrupt__,auto_psv)) _T4Interrupt(void);
volatile int flag_signal_interrupt_waiting_RF;
volatile int flag_signal_interrupt_waiting_ultrasonic=0;
void motor_S(void);
void motor_L_spot(void);
void motor_R_spot(void);
70

unsigned int IR_Check(unsigned int IR_Condition);

/*Function used to call the reception only in follower*/


int uart_call(char latest_position){
unsigned int IR_Condition,IR_Result;
U1MODEbits.UARTEN=1;
__delay32(12);
int i;
int received=0x0000;
flag_signal_interrupt_waiting_RF=0;
IFS0bits.T1IF=0;
T1CONbits.TON=1;
do{
if(flag_signal_interrupt_waiting_RF>=68){
decimal_call(2);}
if(flag_signal_interrupt_waiting_RF >=68 &&
flag_signal_interrupt_waiting_RF<=2039{
if(latest_position=='R') {
IR_Condition=6;
IR_Result=IR_Check(IR_Condition);}
else if(latest_position=='L'){
IR_Condition=7;
IR_Result=IR_Check(IR_Condition);}
else
{
motor_S();}}
else if(flag_signal_interrupt_waiting_RF>2039 &&
flag_signal_interrupt_waiting_RF<=4078){
if(latest_position=='L'){
IR_Condition=6;
IR_Result=IR_Check(IR_Condition);}
else if(latest_position=='R'){
IR_Condition=7;
IR_Result=IR_Check(IR_Condition);}
else
{
motor_S();}}
else if(flag_signal_interrupt_waiting_RF>4078){
T1CONbits.TON=0;
flag_signal_interrupt_waiting_RF=0;
motor_S();}
if(U1STAbits.OERR==1){
for(i=0; i<5; ++i){
if(U1STAbits.FERR==0){
received=U1RXREG;
if(received==99){
goto end_of_reception;}}
else if(U1STAbits.FERR==1){
received=U1RXREG;
if(received==99)
71

{
goto end_of_reception; }}}
U1STAbits.OERR=0;}
received=U1RXREG;
if(received==99){
break;}}while(1);
end_of_reception:
T1CONbits.TON=0; //turn off timer for waiting for RF signal
flag_signal_interrupt_waiting_RF=0; //Clear accumulator in timer 4
U1MODEbits.UARTEN=0;
TMR3=0;
IFS0bits.T3IF=0;
return received;}
72

APPENDIX B

Program in DsPIC30F4013 Microcontroller for The Leader Robot

;============================================================
; File Name: main_program.c
:Programmer : BOON KHANG HUA
; Date : 27/1/2005
; Supervisor : Dr. Johari Halim Shah Osman
;============================================================
#include "p30f4013.h"
#include "uart.h"

/*Initialization for Oscillator*/


_FOSC(CSW_FSCM_OFF & XT_PLL8 );
_FWDT(WDT_OFF);
#define FCY 20000000UL
#include <libpic30.h>

/*Funtion Declaration for selection_mode.c*/


void initialization_selection_mode(void);
int selection_mode(void);
void initialization_uart1(void);
void uart_call(void);
void generate_40KHZ_singal(void);
void initialization_timer_1(void);
void initialization_timer_3(void);

/*Main Function*/
int main(void){

/* Initialization for timer 1 & Timer 3*/


initialization_timer_1();
initialization_timer_3();

/* Initialization for selection_mode.c*/


initialization_selection_mode();

/*Initialization for UART 1*/


initialization_uart1();

int select;

do{
select=selection_mode();
switch(select){
case 0: uart_call();
while(1);
break;
73

case 1: generate_40KHZ_singal();
LATBbits.LATB5=1;
LATBbits.LATB8=1;
while(1);
break;
case 2: do{
uart_call();
generate_40KHZ_singal();
__delay32(200000); //delay for 0.01 second
}while(1);
break;
default: break;}while(1);
return 0;}

int selection_mode(void){
int selection_mode=0;
label1:
if(_RD9==0) {
while(_RD9==0){;}
__delay_ms(5);
if(_RD9==1){
selection_mode++;}}
if(selection_mode==4){
selection_mode=0;}

switch(selection_mode){
case 0: LATBbits.LATB5=0; LATBbits.LATB8=0;
break;
case 1: LATBbits.LATB5=1; LATBbits.LATB8=0;
break;
case 2: LATBbits.LATB5=0; LATBbits.LATB8=1;
break;
case 3: LATBbits.LATB5=1;LATBbits.LATB8=1;
break;
default: break;}
if(_RD8==1){
goto label1;}
else if(_RD8==0){
while(_RD8==0){;}}
return(selection_mode);}

void initialization_selection_mode(void){
LATBbits.LATB2=1;
LATBbits.LATB3=1;
TRISBbits.TRISB5=0;
TRISBbits.TRISB8=0;
TRISDbits.TRISD8=1;
TRISDbits.TRISD9=1;}

void initialization_uart1(void){
74

/*Transmitter& Receiver*/

U1MODE=0x8000;
U1STA=0;
U1STAbits.UTXEN=1;
U1BRG=1041; //Set baud rate to 1200 @ FCY=20 Mhz

/* Interrupt*/
IEC0bits.U1TXIE=0; //clear TX interrupt
IEC0bits.U1RXIE=0; }

void initialization_timer_1(void){
T1CON=0x0000;
TMR1=0;
PR1=0xFFFF;}

void initialization_timer_3(void){
T3CON=0x0000;
TMR3=0;
PR3=8307; //the value of PR3
determine how long the delay,eg: time_delay = PR3*1/20000000=0.00041535
second
IEC0bits.T3IE=1;}
;===========================================================
; File Name: uart.c
:Programmer : BOON KHANG HUA
; Date : 27/1/2005
; Supervisor : Dr. Johari Halim Shah Osman
;===========================================================
#include "p30f4013.h"
#include "uart.h"
#define FCY 20000000UL
#include "libpic30.h"

/*Function Declaration for UART transmission and reception*/


void transmission(void);
int reception(void);
volatile unsigned int flag;

/*Function used to call the transmission and reception*/


void uart_call(void){
flag=1;
transmission();}

/*Send the data*/


void transmission(void){
while(1)
{
__delay32(12);
while(U1STAbits.TRMT==0);
75

if(U1STAbits.TRMT==1){
LATBbits.LATB5=1;
LATBbits.LATB8=1;
U1TXREG=55;
U1TXREG=99; //Write data to transmit buffer
break;}}

/*Received and verified the data received from RF module*/


int reception(void){
int received=0x0000;
while(U1STAbits.URXDA==0);
if(U1STAbits.FERR==0){
received=U1RXREG; }
else
{
U1STAbits.FERR=0;
received=0x01;}
U1MODEbits.UARTEN=0;
U1MODEbits.UARTEN=1; //Enable the uart module again
U1STAbits.UTXEN=1;
if(received==99 || received==0x01){
return(received);}
else
{
return(0x00);}}

void __attribute__((__interrupt__,auto_psv)) _T3Interrupt(void){


flag=0;
IFS0bits.T3IF=0;}
;============================================================
; File Name: ultrasonic.c
:Programmer : BOON KHANG HUA
; Date : 27/1/2005
; Supervisor : Dr. Johari Halim Shah Osman
;===========================================================
#include "p30f4013.h"
#include "libpic30.h"

void __attribute__((__interrupt__,auto_psv)) _T2Interrupt(void);


void initialization_timer_2(void);
void initialization_output_compare(void);
unsigned int volatile timer_2_count;

void generate_40KHZ_singal(void){
timer_2_count=0;
initialization_output_compare();
initialization_timer_2();
__delay32(326864);
T2CONbits.TON=1;
while(1){
76

if(timer_2_count==9){break;}}
T2CONbits.TON=0;
OC1CON=0x0000; //Disable OC1 after use
OC2CON=0x0000; //Disable OC2 after use
}

void initialization_timer_2(void){
T2CON=0x0000; //make sure the timer 2 off completely

TMR2=0x01F0; //Initialize the TMR2 to 0x00


PR2=499; //Set the frequency of timer 2 to 40 KHz
IEC0bits.T2IE=1; //Enable interrupt bit for timer2
IPC1bits.T2IP=5; //Set priority of Interrupt for timer2 to level 5
}

void initialization_output_compare(void){
/*Initialization for OC1 pin*/
OC1CON=0x0000;
OC1R=0;;
OC1RS=249;
OC1CON=0x0005;

/*Initialization for OC2 pin*/


OC2CON=0x0000;
OC2R=249;
OC2RS=499;
OC2CON=0x0005;}

void __attribute__((__interrupt__,auto_psv)) _T2Interrupt(void){


timer_2_count++;
IFS0bits.T2IF=0; //Clear Interrupt flag bit of
}

Das könnte Ihnen auch gefallen