Anda di halaman 1dari 104

PSZ 19:16 (Pind.

1/07)
UNIVERSITI TEKNOLOGI MALAYSIA

NOTES: * If the thesis is CONFIDENTAL or RESTRICTED, please attach with the letter from
the organization with period and reasons for confidentiality or restriction.
ii

“I hereby declare that I have read this thesis and in my/our*


opinion this thesis is sufficient in terms of scope and quality for the
award of the degree Bachelor of Engineering (Electrical – Mechatronics)

Signature : ………………………….........
Name of Supervisor : Prof. Shamsudin Bin H. M. Amin
Date : ………………………………..
iii

A LOW COST FRUIT SORTING ROBOT

MUHAMMAD SYAFIQ BIN A RAHMAN

A thesis submitted in fulfilment of the


requirements for the award of the degree of
Bachelor in Electrical Engineering (Electric - Mechatronics)

Faculty of Electrical Engineering


Universiti Teknologi Malaysia

JULY 2014
iv

I declare that this thesis entitled “A Low Cost Fruit Sorting 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 concurrently submitted in candidature of any other degree.

Signature : ....................................................
Name : MUHAMMAD SYAFIQ BIN A RAHMAN
Date : July 2014
v

DEDICATION

Specially dedicated to my beloved mother, father, brother and fiancée for giving
support and encouragement.
vi

ACKNOWLEDGEMENT

Highest gratitude and praise to Allah S.W.T. for giving me blessings,


strengths, patience and ideas during my final year project. I would like to express
heartily gratitude to my supervisor, Prof. Shamsudin Bin H. M. Amin for guiding
and giving ideas for my project. With his support and concerns make this project is
able to complete in time.

I further express my appreciation to my family especially my parent


for educating me with the meaning of perseverance, diligence, patience, motivations,
support and encouragement throughout my studies.

My deepest gratitude to my colleagues Syamsul Fitri Bin Sujak,


Muhammad Aizad Iqram Bin Mohd Sani and Muhammad Fazly Affendi Bin Zamil
who had helped and giving ideas for my project.

Last but not least, I would like to thank all the lecturers that have
taught and support me throughout my study in Universiti Teknologi Malaysia.
.
.
vii

ABSTRACT

Fruit sorting machines are mostly used in the industries to sort fruits in bulk.
The process started by detecting the colour of the fruits to indicate its classification
based on the colour of the fruit. In this project, the fruit sorting machine has been
designed and built for small scale industries needing low cost sorting machines
compared to those now being used, expensive large machines. This project focused
on sorting the different types of apples which are green (Granny Smith), yellowish
(Golden Delicious) and dark red (Red Delicious). The apple will be inserted into a
small tunnel and then entered a box with a controlled lighting system. The controlled
lighting system is in a closed space, protected from the ambience lighting with a
constant lighting supply which gives the same reading of brightness no matter where
and when the process occurred. The mechanism consists of a servo motor and
attached to it is a 45 degrees aluminium beam that is used to hold the fruit during the
scanning process. The scanning process takes place in the middle of the tunnel. A
Cadmium Sulphide photosensors (Cds Photosensor) integrated with Red, Green and
Blue Light Emitting Diode (RGB LED) is used to detect the colour of the fruit. After
the colour detection process is completed, the servo motor will rotate and push the
fruit towards the end of the tunnel. A signal of the detection and classification of the
colour will be then transferred to a mobile robot by using a short range wireless
communication (NRF communication). The mobile robot is used to carry the fruit to
the appropriate apple sorting boxes by moving according to specified path based on
the signal of the colour as sent by the microcontroller (Arduino) by using NRF
communication.
viii

ABSTRAK

Mesin pengasingan buah-buahan kebanyakannya digunakan oleh industri –


industri untuk mengasing buah-buahan dalam skala besar. Proses ini bermula dengan
mengesan warna buah-buahan tersebut untuk menentukan klasifikasinya berdasarkan
warna buah-buahan tersebut. Dalam projek ini, mesin pengasingan buah ini direka
dan dibina untuk industri-industri kecil yang memerlukan mesin pengasingan yang
murah berbanding dengan mesin-mesin yang digunakan kini, yang besar dan mahal
harganya. Projek ini difokuskan kepada berlainan jenis epal yang berwarna hijau
(Granny Smith), kekuningan (Yellow Delicious), merah pekat (Red Delicious). Epal
ini akan dimasukkan ke dalam sebuah terowong kecil yang kemudiannya masuk ke
dalam sebuah kotak yang mempunyai sistem pencahayaan yang terkawal. Sistem
pencahyaan terkawal ini berada di dalam kawasan yang tertutup rapi, terlindung dari
cahaya disekitarnya, serta dibekalkan dengan pencahayaan sekata di mana ia
memberi bacaan kadar kecerahan yang sama tidak kira di mana dan bila proses itu
berlaku. Mekanisme ini terdiri daripada sebuah motor servo dan yang dipasang
padanya sebuah 45 darjah batang aluminium yang diguanakan untuk menahan buah
ketika proses pengimbasan. Proses pengimbasan ini berlaku di pertengahan
terowong. Sebuah fotosensor Cadmium Sulphide photosensor (Cds fotosensor) yang
diintegrasi bersama Diod Pemancar Cahaya Merah Hijau Biru (RGB LED)
digunakan untuk mengesan warna buah tersebut. Selepas selesai proses mengesan
warna, motor servo akan berpusing dan menolak buah tersebut ke hujung terowong.
Isyarat yang mengesan dan mengklasifikasikan warna tersebut kemudiannya
dihantar ke sebuah robot mudah alih dengan menggunakan komunikasi tanpawayer
jarak dekat( komunikasi NRF). Robot mudah alih tersebut digunakan untuk
membawa buah tersebut ke kotak pengasingan epal yang sepatutnya, dengan
bergerak mengikut laluan yang ditetapkan berdasarkan isyarat warna yang dihantar
oleh pengawal mikro (Arduino) melalui komunikasi NRF.
ix

TABLE OF CONTENTS

CHAPTER TITLE PAGE

DEDICATION V

ACKNOWLEDGEMENT VI

ABSTRACT VII

ABSTRAK VIII

TABLE OF CONTENTS IX

LIST OF TABLES XI

LIST OF FIGURES XIII

LIST OF ABBREVIATION XIV

LIST OF APPENDICES XV

1 INTRODUCTION 1
1.1 Background 1
1.2 Objectives of Project 2
1.3 Scope 2
1.4 ProblemStatement 2
1.5 Outline of Thesis 2
2 THEORY AND LITERATURE REVIEW 3
2.1 Introduction 3
2.2 Previous Project 3
2.3 Application of AGV for food and beverage industries 5
2.4 Application of Fruit Sorting Machine in industries 6
2.5 Types of Line Following Robot 7
2.6 Microcontroller 11
2.7 Motor 13
2.8 Sensor 16
2.9 RGB Common Anode LED 18
3 METHODOLOGY 19
3.2 Mechanical and Hardware Design 20
3.3 Electronics Interface 27
x

3.4 Software 33
4 TESTS, RESULTS AND DISCUSSION 38
4.1 Introduction 38
4.2 Tests 38
4.3 Results 41
4.4 Discussion 42
5 CONCLUSION AND RECOMMENDATION 44
5.1 Conclusion 44
5.2 Recommendation 45
6 PROJECT MANAGEMENT 46
6.1 Introduction 46
6.2 Project Schedule 47
6.3 Cost Estimation 48
REFERENCES 50
APPENDIX 1 52
APPENDIX 2 57
xi

LIST OF FIGURES

FIGURE NO. TITLE PAGE


1 Smart Line Detection System Using Fuzzy Data Fusion Mobile Robot 4
2 Line Pattern Recognition For Autonomous Mobile Robot 5
3 AGVs for food and beverage 6
4 Fruit Sorting Machine 7
5 Arduino Uno 12
6 Arduino Mega 13
7 Basic Operation of DC motor 14
8 DC Motor 15
9 Variable Pulse width control servo position 15
10 Servo Motor 16
11 Servo Motor Block Diagram 16
12 Auto Calibrating Line Sensor 17
13 Cds Sensor 17
14 RGB Common Anode LED 18
15 Simplified Project Flow 20
16 PCB stand, screw and Nut 21
17 First prototype 21
18 Actual part of mobile robot side view 22
19 Actual part of mobile robot top view 22
20 Actual part of mobile robot front view 23
21 Illustration of track design 24
22 Control Lighting Box 25
23 A beam hold fruit during scanning process 26
24 Connection DC motors to motor driver 28
25 The movement of calibrating sensor to the line. 30
26 Pins of Auto Calibrating Line sensor 30
xii

27 Connection of servo motor and motor driver 31


28 Connection of servo motor in control lighting box 32
29 Connection of pin to Arduino 32
30 Example of Arduino IDE 33
31 Flow chart for Scanning system 36
32 Flow chart for mobile robot 37
33 Illustration of track line design 39
34 Flow of the system 41
xiii

LIST OF TABLES

TABLE NO. TITLE PAGE


1 Range of colour value VS Colour of fruit 34
2 Line sensor signal VS speed of DC motor 34
3 Type of Apple vs station and colour value 39
4 Percentage of success 42
5 Semester one Gantt Chart 47
6 Semester two Gantt Chart 47
7 Electronics components estimation price 48
8 Hardware materials estimation price 49
xiv

LIST OF ABBREVIATION

AC - Alternating Current
AGV - Automated Guided Vehicle
Cds photosensors - Cadmium Sulphide photosensors
CSN - Chip Select Not
DC - Direct Current
GND - Ground
MISO - Master In Slave Out
MOSI - Master Out Slave In
PCB - Printed Circuit Board
RGB LED - Red, Green, Blue Light Emitting Diode
RX - Receiver
SCK - Data Clock
TX - Transmitter
UART - Universal Asynchronous Reciever-Transmitter
xv

LIST OF APPENDICES

SOURCE CODE (SCANNING SYSTEM)


SOURCE CODE (MOBILE ROBOT)
1

CHAPTER 1

INTRODUCTION

1.1 Background

In fruits packaging industries, a machine is used to sort out fruits into grades.
The grading is due to the sizes, colours and defects of the fruits. The sorting machine
used is large and requiring an appropriate size of the working space. In sorting
processes, this machine is placed at the middle stage where the first step is to put all
the fruits into the conveyer, after that they will undergo grading process in this
sorting machine. At the final stage, the industries are still using human labour to sort
out those fruits into the appropriate box. Some errors will occur due to the parallax
error [1]. It is also to save the labor cost as it is more expensive compared to
machine maintenance [2].

Automated Guided Vehicle (AGV) has been used in the industry to carry
required stocks to the appropriate stations. AGV has the capability of lane detection,
tracking and obstacles avoidance [3]. The AGV moves by using line following. It
has its own track or path to be followed in order to move from one station to another
station [4]. Having AGV in the industries help in reducing cost of labor and reduce
the production time. In another word, AGV helps to increase the productivity of the
factory. Furthermore, AGV operates on rechargeable battery and only requires
periodical maintenance, almost once a year.
2

1.2 Objectives of Project

The objectives of this project are design and build automated robot to
replace humans with robots to perform sorting process in order to reduce cost and to
reduce time consuming during the sorting process. Furthermore, utilisation of robots
ensure the process is precise and accurate [2].

1.3 Scope

In this project, the focus is on the sensors to detect colour of the fruit. Then
perform sorting by using mobile robot according the classification of colours by
following the path to the appropriate station. The second scope is to sort apples
which are Granny Smith(Green), Yellow Delicious (Yellowish) and Red Delicious
(Dark red).

1.4 ProblemStatement

In fruit packaging industries, they use large and expensive fruit sorting machines
which are unaffordable by the small industries.

1.5 Outline of Thesis

This thesis consists six chapters. In first chapter, discusses the project
background, objective, scope and problem statement. Chapter 2 will discuss on
the theory and literature reviews for this project. Chapter 3 will discuss on the
methodology of hardware and software implementation on this project.
Chapter 4 will show the results and discussion of the implemented project.
While Chapter 5 discusses the conclusion of this project and recommendations
for future work. Lastly, in Chapter 6 will discuss on the project management.
3

CHAPTER 2

THEORY AND LITERATURE REVIEW

2.1 Introduction

The research is conducted based on previous theses, journal articles, research


papers and other sources. The main idea of literature review is to obtain enough
relevant information and knowledge on projects done by others so as to enable the
design and building of the sorting system.

2.2 Previous Project

For several past years, many have attempted to design and create a mobile
robot using different approaches. The following are the projects closely related to
line following mobile robot.

2.2.1 Smart Line Detection System Using Fuzzy Data Fusion For Mobile
Robot Application.[5]

A previous project was developed by Mohd Anuar Bin Miskimin @


Mislimin, a mechatronics student at Universiti Teknologi Malaysia, during session
2011/2012. The main idea for that project is to use photo transistor as a receiver in
line detection sensor for mobile robot navigation.
4

The algorithm to detect line was developed by using fuzzy data fusion and
discrete fourier transform. The robot operates in fixed environment and detects the
white line. The system gives a better line following performance compared to digital
sensor and IR sensor. Figure 1 below shows a complete hardware of this project.

Figure 1 Smart Line Detection System Using Fuzzy Data Fusion Mobile Robot

2.2.2 Line Pattern Recognition For Autonomous Mobile Robot.[6]

This project was developed by Kamil Zakwan Bin Mohd Azmi, a


mechatronics student from Universiti Teknologi Malaysia, session 2012/2013. The
main idea of this project is to design a mobile robot to move on its own and develop
algorithm to recognize line pattern.

For the mechanical part, the robot was built by using a lightweight
components and the gravitational point is kept low to the ground to gain more
stability. This robot uses a line sensor array to detect a line pattern and once identify
the pattern is identified, it then will determine the movement corresponding to the
5

line pattern given. Figure 2 shows a complete hardware of line pattern recognition
for the autonomous mobile robot.

Figure 2 Line Pattern Recognition For Autonomous Mobile Robot

2.3 Application of AGV for food and beverage industries

AGV has some unique uses which are:

i) Food transport (Cold storage, freezers, drying kilns)


ii) Transport of fish tubs in salty and oily areas.
iii) Storage of cheese racks and transport to ripening rooms controlled via
recipe management software.

AGV can be used in any stage of food processing. It helps to supply raw materials to
the processing lines. Figure 3 shows AGV used in food and beverage sector.
6

Figure 3 AGVs for food and beverage

2.4 Application of Fruit Sorting Machine in industries

Fruit sorting machine used in industries can undergo a large amount of fruits
within a certain time. The size of the machine is large and needs a large operating
area. The fruits that can undergo this process are apples, cranberries, oranges and
many more.

Most of this fruit sorting machine uses a vision coloured detector to detect
the colour and sort them into the appropriate grades.[7] This type of scanning system
helps the process to be in faster rate.

There are also fruit sorting machine that can sort those fruits regarding its
sizes and weight. This high technology machine helps to sort fruits in bulk amount.
It reduce the production time and increase the rate of productivity.[8]

Instead of grading, fruit sorting machine also can determine the status of the
fruits either it is still fresh or not. The detection is through the skin texture and colour
scanning.[9]
7

Having those criterion and integrate with RGB colour system, it makes this
fruit sorting machine can deliver a high quality of fruits within short time.[10] This
influence the increasing awareness by consumers about their health well-being and a
response by producers on the need to provide quality guaranteed products with
consistency.[11] Figure 4 shows the use of fruit sorting machine in the industry.

Figure 4 Fruit Sorting Machine

2.5 Types of Line Following Robot

Line following robot is the most common robot that was built and developed
by human being. This type of robot is usually used for leisure activity or robotic
tournament. There are many form of design for this type of robot that have been
built. This type of robot divided into two categories:

i) JET
ii) Sandwich
8

2.5.1 JET

JET is the line following robot that can follows line efficiently at over 1
meter per second. The front and rear sensors boards have shiny tinned copper ground
planes. This significantly reduces the amount of ambient light that can have an
impact on the sensors.

The ground plane is present for optical reasons. It may deliver some trivial
elimination in sensor electrical noise as well. Because the robot is built typically
from PCBs and moving fast over statically-charged PVC, a 5.6 V zener diode is
included to each PCB. just in case of an over voltage.

JET works by using the 68HC908GP32 microcontroller with 7.3728 MHz


clock. The microcontroller is capable to make about 7000 complete line following
decisions per second, which are nearly 7 decisions per millimeter when the robot is
heading full speed. By upping the clock, unrolling loops, and eliminating the
wireless debugging and extraneous code, the number of decisions per second by the
microcontrollers on board analogue-to-digital converter for one complete sensor
sweep.

Each motor can draw up to 1.7 A, so it was essential to use power transistors
to enable for the highest possible motor functionality. Even during the worst
moments, the motors still obtain over 97% of the electrical power from the H-bridge.

Because these motors are being pulsed a lot, a power MOSFET driver was
crucial. MOSFET heating does not seem to be a problem. The chips do not even
seem warm after long durations of use. This is because of the power MOSFET
9

driver, Schottky diodes, and over sizing of the MOSFET resistance versus the
maximum motor draw. In other words, the chips stay cool, so the resistance stays
low, so the motors perform properly.

Jet has an infrared remote control start and stop, which is the Panasonic
PNA4602M 38 kHz infrared decoder for start, stop, speed and Ramtron 25640 non-
volatile serial memory for settings. To build this robot a little more advanced, the
DIP switches and pushbuttons is taken out from the board. The robot is designed
using either the remote control, wireless serial, or hard-wired serial. The
configurations are stored in the non-volatile memory, which is also rapid enough for
data logging.

The JET‟s body material is mainly built for PCB. There are some nylon
spacers, steel screws, aluminium screws, and aluminium angle motor mounts. For
the motors, two rare-earth magnet 13 mm diameter 1.2 W 6 V Maxon motors with
17:1 metal gear heads and precious-metal brushes are used.

Jet goes a little bit less than half (104 cm/s) the maximum no-load speed
(216 cm/s) because the motor speed slows down as it has a load. Also, the robot does
not drive the motor at 100% speed, as it deviates speed to steer. Also, the robot does
not match the track length exactly; there’s some under steer, some over steer, and
some wiggle.

2.5.2 Sandwich

Sandwich is the model of simple line following robot that are built from
sandwich box. It can move pretty fast, but not smooth. This robot is being built by
beginner robot modellers. Rather using a microcontroller, Sandwich uses a
10

comparator for processing. Because of this, Sandwich has poor functionality, such
as:

i. Flashing lights are not as fancy.


ii. Driving is not as smooth.
iii. Cannot take sharp turns.
iv. Will not stop when the path ends
v. Cannot automatically detect if the line is light or dark

It will not be hard to improve Sandwich’s design by contributing a small


microcontroller and a few support circuits to deliver better attributes. Sandwich’s
body, motors, connectors, and other elements could be kept unchanged.

Many line following robots have problems following masking tape when the
robot makes use of infrared LEDs and sensors. It seems most masking tape is to
some extent transparent to infrared, thus the background “colour” bleeds through and
robot cannot detect the line.

To prevent that trouble, Sandwich employs two pairs of ordinary cadmium-


sulphide photo resistors. Also called photocells, they are capable to see visible light
in the some ranges like human eyes.

9 V batteries are great for most small robots due to the lightweight size of
the battery and the connector. But, the issue with 9 V batteries is their quick running
time, which is particularly true of rechargeable varieties.

A version of Sandwich swaps in a 6 V battery pack of four AAA Rayovac


alkaline rechargeable. By choosing a larger wheel diameter, the robot is able to
11

compensate for the lower voltage and attain the same overall speed. In this settings,
this version of Sandwich is able to work for longer periods.

The LEGO wheels are attached with homemade couplers. The coupler can
be made to suit different sizes of motor shafts. The other end of the coupler has a
LEGO cross axle that allows nearly any LEGO wheel to be connected. This makes it
fast and easy to test with various wheels. Also, the wheels can be pulled off, without
tools, for storing or transporting the robot.

For such a pea brain, Sandwich does a great job of following lines. The robot
never strays off the path, so long as a reasonable contrast between the line and the
floor is chosen, and so long as the turns are not too sharp. Generally, after a decent
course has been set up, the robot will zip around endlessly, until the battery finally
gives out.

2.6 Microcontroller

2.6.1 Arduino Uno

The Arduino Uno is a microcontroller board based on the ATmega328. It has


14 digital input/output pins (of which 6 can be used as PWM outputs), 6 analog
inputs, a 16 MHz crystal oscillator, a USB connection, a power jack, an ICSP
header, and a reset button. It contains everything needed to support the
microcontroller; simply connect it to a computer with a USB cable or power it with
an AC-to-DC adapter or battery to get started. The Uno differs from all preceding
boards in that it does not use the FTDI USB-to-serial driver chip. Instead, it features
the Atmega8U2 programmed as a USB-to-serial converter
12

The board can operate on an external supply of 6 to 20 volts. If supplied with


less than 7V, however, the 5V pin may supply less than five volts and the board may
be unstable. If using more than 12V, the voltage regulator may overheat and damage
the board. The recommended range is 7 to 12 volts. Figure 5 shows an example of
Arduino UNO board.

Figure 5 Arduino Uno

2.6.2 Arduino Mega

The Arduino Mega 2560 is a microcontroller board based on the


ATmega2560. It has 54 digital input/output pins (of which 14 can be used as PWM
outputs), 16 analog inputs, 4 UARTs (hardware serial ports), a 16 MHz crystal
oscillator, a USB connection, a power jack, an ICSP header, and a reset button. It
contains everything needed to support the microcontroller; simply connect it to a
computer with a USB cable or power it with an AC-to-DC adapter or battery to get
started.
13

The board can operate on an external supply of 6 to 20 volts. If supplied with


less than 7V, however, the 5V pin may supply less than five volts and the board may
be unstable. If using more than 12V, the voltage regulator may overheat and damage
the board. The recommended range is 7 to 12 volts. Figure 6 shows an example of
Arduino Mega board.

Figure 6 Arduino Mega

2.7 Motor

Motor is a device which convert electrical energy to mechanical movement.


Motor consist of copper winding, stator and rotor. Electric field is needed to move
the rotor.
14

2.7.1 DC Motor

A DC motor is an electrical motor powered by direct current (DC). In the


motor, there is a rotor which is an armature winding where an electromagnetic field
generated when the power is supplied. The stator will produce a static field. The
direction can be change due the changes of the polarity of the power supplied. The
speed varies due to the change of voltage supplied to the motor. Figure 7 shows the
basic operation of DC motor. Figure 8 shows an example of DC motor used in the
project.

Figure 7 Basic Operation of DC motor


15

Figure 8 DC Motor

2.7.2 Servo Motor

A servo motor is a motor that allow user to control its rotation angle. The
servo motor is controlled by using simple pulse controlling. Figure 9 shows the basic
concept of Variable pulse width control servo position. Figure 10 and Figure 11
show an example of servo motor used and a servo motor block diagram.

Figure 9 Variable Pulse width control servo position


16

Figure 10 Servo Motor

Figure 11 Servo Motor Block Diagram

2.8 Sensor

A sensor is a device that read physical quantity and convert it into digital
signal that can be process by microcontroller. In this project, an auto calibrating line
sensor and Cds sensors are used. This line sensor can cover about 1cm to 3cm
distance. This line sensor has the ability to memorize and recognize the surface even
after restarting the power supplied. Figure 12 shows Auto calibrating line sensor.
17

Figure 12 Auto Calibrating Line Sensor

Cds sensor or also called as photoresistor is used to detect the colour of the
item. It will integrate with RGB led. This sensors will read three values to indicate
Red, Green and Blue colours. Figure 13 shows an example of Cds sensor used.

Figure 13 Cds Sensor


18

2.9 RGB Common Anode LED

Red, Green, Blue common anode LED produce three type of light which are
red, green and blue. This type of LED has four leg which indicate red, +5V, green
and blue. Figure 14 shows a RGB common LED.

Figure 14 RGB Common Anode LED


19

CHAPTER 3

METHODOLOGY

This chapter describes the methodology taken for this project. It starts from
the discussion of workflow, followed by the system design procedure, techniques
and tool utilized in this work. In this chapter, the fabrication of hardware and circuit
connection will be discussed.

3.1 Project Workflow

Any project needs a good planning and well construct event in order to make
it successful in a given time constraint. Project flow is the flow of the planned action
that need to be done to finish the project. The most important to kick start a project is
to know sufficient knowledge on the background of the project itself. Reading
journals, books, searching on internet and so many other ways may help to gain the
knowledge that required. After gaining knowledge on theories and concepts, proceed
with the circuit and hardware design. Here, to develop the circuit and hardware,
appropriate components are needed based on the knowledge from the reading. To
ensure the circuit design is correct, software development is then take place.
Integration between hardware and software is a need to know the functionality of the
circuit. Then, fine tuning required in order to get a precise and accurate results. If the
circuit did not function as required, software and hardware integration need to be
done once more time. Figure 15 shows a simplified project flow.
20

Figure 15 Simplified Project Flow

3.2 Mechanical and Hardware Design

This project consists of two parts of hardware. The first part is the mobile
robot used to carry the fruits after scanning process has finished. The second is the
control lighting box in which the scanning process occurs.

3.2.1 Mobile Robot

The robot consist of two parts which are the main body as the base for the
robot and the carrier where fruits are carried on it. The materials used to build this
robot are aluminium bar and acrylic material in order to build a lighter robot. The
other materials are PCB stands, screws and nuts. Figure 16 shows an example of
PCB stand, screw and nut.
21

Figure 16 PCB stand, screw and Nut

3.2.1.1 Solidwork Design

Firstly, before building a hardware, a solidwork design was drawn to get full
view for this robot. The design has to take into account the stability and smoothness
of the movement. Figure 17 below shows the first prototype for Fruit Sorting Robot.

Figure 17 First prototype


22

Figure 18 below shows the actual part of the mobile robot. A castor is placed
at the back part of the robot to support and to make a smooth movement of the robot.

Figure 18 Actual part of mobile robot side view

The lifter component is designed based on a lorry concept. It has a beam that
will push the carrier up while the servo motor is rotating clockwise. Figure 19 shows
the top side view of the mobile robot.

Figure 19 Actual part of mobile robot top view


23

The carrier is covered by a span to reduce the impact while the fruits fall
down into the robot. At the back of the carrier, there is a holder to avoid the fruits
fall down from the carrier while the robot is moving. Figure 20 shows the front view
of the mobile robot.

Figure 20 Actual part of mobile robot front view

The line sensor is placed at the centre of the robot in order to ensure to
movement is smooth and centralized between two tyres. This robot will do tasks
based on the information it get from the NRF signal [12]. It decides its path based on
the different categories that has been programmed in it.
24

3.2.2 Track Design

This track was designed with three station which is Station A, Station B and
Station C. Each station indicates for each type of apple which are green, red, and
yellowish. This track was also designed for delivery path and return path.

Robot will receive the fruits at the starting point and start to move to the
stations depending on the signal received by the NRF communication. Figure 21
illustrate the track design.

Figure 21 Illustration of track design

3.2.3 Control Lighting Box

The control lighting box is built from a polystyrene ice box. This polystyrene
ice box is covered by an aluminium foil at inner part in order to create light
reflection effect in the box as to reduce the loss of the light intensity. The concept of
this control lighting box is based on the X-ray box at the airport.
25

The benefit of having this control lighting box is the process of scanning of
fruits can be done at any time and any place. Also it helps to get a stable reading by
sensors in classifying the fruits. Figure 22 shows the setup of the control lighting
box.

Figure 22 Control Lighting Box

There is a tunnel into which the fruits can roll through this control lighting
box. The scanning process occurred in the tunnel which is situated in the box. The
process is done at the middle part of the tunnel. In here, the light was supplied by a
bright white LED which is situated almost 45 degrees from the tunnel and scanning
region. A servo motor attached with a 45 degrees beam, acts as a holder for the fruits
during the scanning process. Figure 23 shows a beam used to hold fruit during
scanning process.
26

Figure 23 A beam hold fruit during scanning process

During the scanning process, RGB LED will blink continuously. Cds sensor
will detect the reflection of the colour from the skin of the fruit. This RGB LED
gives a wider colour space for Cds sensor to transform it into a digital value [13].
The value of colour reading may vary due to the angle of the lighting to the fruit’s
skin [14].

After the scanning process finished, the servo motor will rotate and makes
the beam to push the fruits to the end of the tunnel. During the process of pushing
the fruits to the end of the tunnel, the microcontroller will send the information based
on the colour classification to the mobile robot by using NRF communication.

NRF communication is a short range wireless communication [15]. It can go


up to 3 meters away from the transmitter.
27

3.3 Electronics Interface

3.3.1 DC Motor

There are two DC motors used in this project. A DC motor is used to drive
the mobile robot wheels. The DC motor is controlled by using a motor driver shield.
This motor driver is capable of handling 4 DC motors at one time. The motor driver
can drive bi-directional movement of the DC motor. The motor’s speed also can be
adjusted and varied at 0.5% increments which gives a smooth movement. Figure 24
shows the connect of DC motor to the motor driver. Other features of this motor
driver are [16]:

i. L293D is a monolithic integrated, high voltage, high current,


4-channel driver.
ii. Basically this means that by using this chip enables the DC
motors and power supplies of up to 10 Volts, some pretty
large motors and the chip can supply a maximum current of
600mA per channel. The L293D chip is also known as a type
of H-Bridge.
iii. The H-Bridge is typically an electrical circuit that enables a
voltage to be applied across a load in either direction to an
output, e.g. motor.
iv. Two interface for 5V Servo connected to the Arduino's high-
resolution dedicated timer - no jitter.
v. Can drive 4 DC motors or 2 stepper motors or 2 Servo.
vi. Up to 4 bi-directional DC motors with individual 8-bit speed
selection.
vii. Up to 2 stepper motors (unipolar or bipolar) with single coil,
double coil or interleaved stepping.
viii. Four H-Bridges: per bridge provides 0.6A (1.2A peak current)
with thermal protection, can run motors on 4.5V to 10V DC.
ix. Pull down resistors keep motors disabled during power-up.
x. Reset button.
28

xi. Two external terminal power interface, for separate


logic/motor supplies.
xii. Tested compatible for Arduino Mega, Diecimila &
Duemilanove.
xiii. Size:6.8cm x 5.5cm x 2cm - 2.68inch x 2.17inch x 0.79inch.

Figure 24 Connection DC motors to motor driver

The motor driver uses the following pins on Arduino Mega in order to
control DC motor speed:

 Digital pin 11 : M1
 Digital pin 3 : M2
 Digital pin 5 : M3
 Digital pin 6 : M4

DC motor direction is controlled by the following pins:


29

 Digital pin 4
 Digital pin 7
 Digital pin 8
 Digital pin 12

3.3.2 Auto Calibrating Line Sensor

The sensor has to be connected to 5V and ground pin to supply power to this
sensor. The other pins are connected to Arduino pins according to the initialization
in the programming.

In interfacing this sensor, LSS05 need to be calibrated to retrieve the dark


and bright value of the surface to do the line following. Every one of the IR sensor
pairs need to be exposed to the dark and bright surface for it to read the value and
store it. LSS05 will save the value in EEPROM, and it will retrieve back the data
from the EEPROM every time it is switched on. Hence, only one time calibration is
needed for the same surface and line. To calibrate LSS05, simply press the
calibration push button once or pulling down the Cal. for few milliseconds.
Calibration will be start by exposing the sensor to the bright surface and then to the
dark surface as indicated by the LEDs. Three LEDs blinking mean the bright
calibration (2.5 seconds) and two LED blinking mean the dark calibration (2.5
seconds) [17].

Calibrating the sensor is done by putting the sensor to the line as shown in
Figure 25 below. The sensor will memorize the brightness and the darkness value
from the calibration process. Figure 25 shows the movement of calibrating sensor to
the line.
30

Figure 25 The movement of calibrating sensor to the line.

The process of calibrating is the most important step which cannot be


skipped because it will then give a wrong signal to microcontroller which then will
not give an accurate result of robot movement. Figure 26 shows pins of auto
calibrating line sensor.

Figure 26 Pins of Auto Calibrating Line sensor


31

3.3.3 Servo Motor

In this project, there are two servo motors used. One is used in the mobile
robot to lift the carrier and the other one is used at the tunnel where the scanning
process occurs. The servo motor needs 6V to operate. The most common way is to
put an external power source to avoid any disturbance to the microcontroller power
supply. As in the mobile robot, the connection of the servo motor is as shown in
Figure 27 below.

Figure 27 Connection of servo motor and motor driver

Meanwhile, in the control lighting box, the servo motor is connected directly
to the Arduino Uno and the supply is from 6V external batteries. The connection is
as shown in Figure 28 below.
32

Figure 28 Connection of servo motor in control lighting box

3.3.4 NRF 24L01

NRF is a radio frequency communication used as short range wireless


communication. It has a Master and a Slave which indicate which one is the
transmitter and the transceiver. To communicate, Master and Slave must have the
same frequency which is also known as an assign channel [18]. The connection of
pin is as shown in Figure 29 below.

Figure 29 Connection of pin to Arduino


33

3.4 Software

3.4.1 Arduino IDE

Arduino IDE (Integrated Development Environment) is a simplistic text


editor with compilers and communication abilities with any Arduino board. The
program uses C and C++ language which is easier to understand. By using this
software, it is a much user friendly and also an open source whereby anyone can use
and get the code easily. Figure 30 shows the example of Arduino IDE.

Figure 30 Example of Arduino IDE

3.4.2 Control Algorithm for Colour Scanner

Based on the programming on Arduino IDE, the range of the colour value of
red, green and blue is calculated during the scanning process and compared to the
value which has been set initially to indicate the colour of the fruits. To get the initial
reading, several samples are tested and the range of colour value is taken. The range
of value from the initial reading is being used as based on readings to indicate the
34

colour of the fruits. The range of colour has been obtained and collected as shown in
Table 1 below.

Table 1 Range of colour value VS Colour of fruit


Colour of Fruit

Range of Dark Red Yellowish Red Green


colour value
RED 500<X<1100 1500<X<2300 -1500<X<-700

GREEN 15<X<90 100<X<200 X>750

BLUE 0 0 0

3.4.3 Control Algorithm for Line Following

The programming for this algorithm is uploaded into Arduino Mega 2560.
The mobile robot will wait for a signal from the scanner at the start point. When
robot receives the signal from the scanner by using NRF communication, it will
move to the appropriate path to send the fruit to the correct basket. Then, the mobile
robot will go back to the starting point and wait for the next signal. Basically, the
movement of the robot is based on the signal given by the line sensor. Below is
Table 2 that shows a basic movement when there are changes on the line sensor.

Table 2 Line sensor signal VS speed of DC motor


Line Sensor Speed for Left DC Speed for Right
Binary Input Signal Motor (PWM) DC Motor
(PWM)
00000 0 0
00100 125 135
35

00110 125 110


00010 125 100
00011 125 90
00001 125 0
01100 110 135
01000 100 135
11000 90 135
10000 0 135

3.4.4 Flow Chart of Fruit Sorting Robot

For the scanning system, the system will undergo an initialize process. In this
initialize process, the initialize readings were taken first when no fruit at the
scanning place. RGB LED will blinking twice to indicate a white based value for the
first time and black based value at the second blink. Then the reading were add and
divided to get the average value. Then the average value is used as a based value to
indicate any changes of reading when the scanning process occurs. The scanning
process will start when an apple is inserted into the tunnel and get into the scanning
place. The changes of the colour reading are determined and calculated to indicated
what are the specification of the colour. After the colour was determined,
simultenously the servo motor will turn to push the apple to the end of the tunnel and
the microcontroller send the information regarding the colour through a NRF
communication to a mobile robot. Figure 31 shows the flow chart of the scanning
system.
36

Figure 31 Flow chart for Scanning system

For the sorting process, a mobile robot is used as a carrier to bring the apple
to the appropriate box. Firstly, in the initial stage, the mobile robot will stay at the
starting line to wait for the signal from scanning system. When the mobile robot
receive a signal from the scanning system, it will differentiate the data that send
through that signal whether it is a data which indicate dark red or yellowish red or
green apples. After that, robot will move according to the appropriate path according
the colour of the apples. Then after finishing the task, mobile robot will return to the
starting point and wait for the next signal. Figure 32 shows a flow chart for the
mobile robot process flow.
37

Figure 32 Flow chart for mobile robot


38

CHAPTER 4

TESTS, RESULTS AND DISCUSSION

4.1 Introduction

In this chapter, we discuss about how the tests are carried out and the result
obtained. We also discuss about the failure during the testing period and what are the
causes of the failure.

4.2 Tests

The test is carried out once a complete program is uploaded into the Arduino
Uno at the scanning tunnel which is located in the control lighting box and Arduino
Mega at the mobile robot. This system is tested by inserting several different apples
into the scanning tunnel. Then, as a result from the scanning process, a mobile robot
is tested its movement through a line track and go to the appropriate station based on
the information received from the NRF signal given by Arduino UNO.

On this track line as shown in Figure 33 below, the mobile robot is tested for
its capability of going to the station and returning back to the starting point.
39

Figure 33 Illustration of track line design

The system has been programmed to identify three types of apples which are
Granny Smith (Green), Yellowish (Golden Delicious) and Dark Red (Red
Delicious). Each type of the apple will be sent to the different stations. Table 3
below shows the stations that apples will be sent according to the colour of the
apples.

Table 3 Type of Apple vs station and colour value


Type of Apple Station Colour Value

Red = 500<X<1100

A Green = 15<X<90

Blue = 0

Dark Red
40

Red = 1500<X<2300

B Green = 100<X<200

Blue = 0

Yellowish Red

Red = -1500<X<-700

C Green = X>750

Blue = 0

Green

Figure 34 shows the flow chart of the system in term of actual picture. Each
colour of apples has different colours specification which will be divide into three
types of colours which are dark red, yellowish red and green. From the flow chart we
can see there are different path for each type of the colour. The paths are made of
black sallotape width 18 milimetre each. At each station, it has a line to tell the robot
that it has arrive to the station. Then the carrier will lift up to drop the apple into the
basket. After that process, the carrier is lifted down back and mobile robot return to
the initial start point.
41

Figure 34 Flow of the system

4.3 Results

There are some failures that occurred during the testing process. The testing
process uses ten samples and the probability of success has been calculated for each
type of apples.
42

Table 4 Percentage of success


Trial/

Type of 1st 2nd 3rd 4th 5th 6th 7th 8th 9th 10th Percentage Of
Apple Success

Red X X X X X X X 70%

Yellowish X X X X X X X 70%

Green X X X X X X X X X 90%

4.4 Discussion

Based on the tested results, all of the trial’s reading above, all of them are
more than 50 percent of success. So, the test is considered a success and using RGB
LED in the scanning process helps the system to easier recognize the colour and
obtain accurate values [19].

The failure may be caused by the scanning system or mobile robot. In case of
scanning system failure, it is caused by the value read by the photo sensors (Cds) is
different from the initialize value. This problem occurs because the apples have
slightly different tones of skin colour depending on the position of the apples during
the scanning process. The colour read by Cds sensor also depends on the angle of the
RGB LED and the apples to the Cds sensors [14]. When the system reads a false
value, then it will send the false signal to the mobile robot. To overcome this
problem, the apples which have the same tone of colours around its skin are chosen.
43

On the mobile robot perspective, the failure may come from the line sensors
unable to detect the line accurately. This line sensors also need to be placed at the
right position on the robot. In this project, the line sensors are placed parallel with
the tyres. Furthermore, the speed of the robot also can cause the line sensors to miss
a junction line whereas it needs to make a turn at that particular condition. So, to
overcome this problem, the track line is designed to be thicker to ensure there are no
lines will be skipped during the line tracking process.
44

CHAPTER 5

CONCLUSION AND RECOMMENDATION

5.1 Conclusion

This project comprised of three parts which are hardware, electronics and
software development. Basically, this project is focused on apple sorting based on its
colour. Fruit Sorting Robot has two parts of hardware which are a mobile robot and a
control lighting box where the scanning process occurs. The algorithm built in the
mobile robot is to make sure the apple is sent to the appropriate station based on its
colour. The algorithm built in the scanner system is to detect the colour of the apple
and send a signal to mobile robot to indicate which category the apple falls into.
From the result, all of the readings are more than 50 percent success. With this
result, we can conclude this project is successful.

In conclusion, development of the Fruit Sorting Robot algorithm could help


the small industries to have cheaper and affordable price of fruit sorting machine. It
also can reduce the production cost as to lower down the labour cost and in the same
time to increase the productivity.
45

5.2 Recommendation

There are a lot of improvements that can be made on Fruit Sorting Robot in
order to increase the sensitivity and accuracy. The suggestions of improvement are:

1) To build the carrier with the front holder to avoid the apples from fall
down.
2) To use a stronger DC motor to support and move heavier loads (larger
apples such as Fuji apples).
3) To use advanced line sensors in order to ensure the movement is smooth.
4) To use a better photo sensor to get accurate readings of different colours.
5) To use a robot arm with pneumatic suction method to sort those apples
[20].
6) To use image processing to detect the colours.
7) To use Integrated Filter-less BiCMOS Sensor for RGB-LED colour
determination [21].
46

CHAPTER 6

PROJECT MANAGEMENT

6.1 Introduction

In this chapter, we discuss on how the project was managed during the period
of two semesters given to complete this project. The objective of the project
management is to ensure the project is completed on time and to monitor all
activities to be done according to the plan. The main goals of project management
are to get effective project planning, organizing and controlling resources within a
specified period of time.

In order to control the resources, the cost of the project need to be estimated
to avoid any excessive use of money on buying unnecessary items. Cost estimation
is important in order to ensure minimum project cost on achieving project goals. All
electronics component were bought from Cytron and MRJ Store because of their
price is affordable and cheaper.
47

6.2 Project Schedule

Table 5 Semester one Gantt Chart

Table 6 Semester two Gantt Chart


48

6.3 Cost Estimation

For the cost estimation, there are two parts of components which are
electronics components and hardware component.

Table 7 Electronics components estimation price


Component Name Unit Price Per Unit Subtotal
(RM)
(RM)

LED Stripe 1 meter 1 50.00 50.00

Arduino Mega 2560 1 105.00 105.00

Arduino Uno 1 58.00 58.00

DC Motor 2 27.00 54.00

Servo Motor 2 88.00 176.00

3 in 1 Motor Driver 1 40.00 40.00

NRF module 2 25.00 50.00

RGB LED 3 1.20 3.60

Cds Sensors 3 1.20 3.60

Power jack 2 2.00 4.00

Rechargeable Battery 2 75.00 150.00

Rechargeable Battery Charger 1 89.00 89.00

Line sensors 1 49.90 49.90

TOTAL 833.10
49

Table 8 Hardware materials estimation price


Item Name Unit Price Per Unit Subtotal
(RM)
(RM)

Castor 2 2.50 5.00

Acrylic 1 50.00 50.00

Plastic Pipe 1 20.00 20.00

Sponge 2 5.00 10.00

Aluminium Flat Bar 2 4.00 8.00

Aluminium Angle Bar (Small) 1 5.00 5.00

PCB Stands 4 1.20 4.80

Ice Box Large 1 10.00 10.00

White board 1 8.90 8.90

Aluminium Foil 1 7.90 7.90

Colour Spray 2 10.00 20.00

Screws - 4.00 4.00

Wires - 20.00 20.00

Battery Holder 2 3.00 6.00

TOTAL 179.60

Electronics Components 833.10

Hardware materials 179.60

TOTAL RM1012.70
50

REFERENCES

1. Buzera, M., et al. Techniques of Analysing the Colour of Produces for


Automatic Classification. in Intelligent Engineering Systems. INES 2008.
International Conference on. 2008.
2. Golpira, H., et al. Improvement of an apple sorting machine using PSNR
criterion. in Advanced Mechatronic Systems (ICAMechS), 2012 International
Conference on. 2012.
3. Al-Zaher, T.S.A., et al. Lane tracking and obstacle avoidance for
Autonomous Ground Vehicles. in Mechatronics (MECATRONICS) , 2012 9th
France-Japan & 7th Europe-Asia Congress on and Research and Education
in Mechatronics (REM), 2012 13th Int'l Workshop on. 2012.
4. Hague, T., M. Brady, and S. Cameron. Using moments to plan paths for the
Oxford AGV. in Robotics and Automation, 1990. Proceedings., 1990 IEEE
International Conference on. 1990.
5. Mohd Anuar Bin Miskimin @ Mislimin, Smart Line Detection System Using
Fuzzy Data Fusion For Mobile Robot Application, Thesis, Bachelor of
Engineering (Electric – Mechatronics), UTM, Session 2011/2012.
6. Kamil Zakwan Bin Mohd Azmi, Line Pattern Recognition For Autonomous
Mobile Robot, Thesis, Bachelor of Engineering (Electric – Mechatronics),
UTM, Session 2012/2013.
7. Pham, T., B. Per, and J.L. Troncoso. Fuzzy analysis of color images of fruits.
in Information Science and Engineering (ICISE), 2010 2nd International
Conference on. 2010.
8. Guo, F. and Q. Cao. Study on color image processing based intelligent fruit
sorting system. in Intelligent Control and Automation, 2004. WCICA 2004.
Fifth World Congress on. 2004.
9. Pawar, M.M. and M.M. Deshpande. Skin defect detection of pomegranates
using color texture features and DWT. in Computing and Communication
Systems (NCCCS), 2012 National Conference on. 2012.
10. Kay, G. and G. De Jager. A versatile colour system capable of fruit sorting
and accurate object classification. in Communications and Signal
Processing, 1992. COMSIG '92., Proceedings of the 1992 South African
Symposium on. 1992.
11. Deepa, P. and S.N. Geethalakshmi. Improved Watershed Segmentation for
Apple Fruit Grading. in Process Automation, Control and Computing
(PACC), 2011 International Conference on. 2011.
12. Bru, et al. Adaptive signal strength prediction based on radio propagation
models for improving multi-robot navigation strategies. in Robot
Communication and Coordination, 2009. ROBOCOMM '09. Second
International Conference on. 2009.
13. Chang, Y.N., et al. Auto mixed light for RGB LED backlight module. in
Industrial Electronics, 2009. ISIE 2009. IEEE International Symposium on.
2009.
51

14. Chherawala, Y., R. Lepage, and G. Doyon. Food Grading/Sorting Based on


Color Appearance trough Machine Vision: the Case of Fresh Cranberries. in
Information and Communication Technologies, 2006. ICTTA '06. 2nd. 2006.
15. Guodong, C. and Y. Harada. Design of Group Robots System Based on
Wireless Communication. in Information Technology, Computer Engineering
and Management Sciences (ICM), 2011 International Conference on. 2011.
16. store.mrj.my
17. www.cytron.com.my
18. Kuo, V. and R. Fitch. A multi-radio architecture for neighbor-to-neighbor
communication in modular robots. in Robotics and Automation (ICRA), 2011
IEEE International Conference on. 2011.
19. Muthu, S. and J. Gaines. Red, green and blue LED-based white light source:
implementation challenges and control design. in Industry Applications
Conference, 2003. 38th IAS Annual Meeting. Conference Record of the.
2003.
20. Kondo, N. Fruit grading robot. in Advanced Intelligent Mechatronics, 2003.
AIM 2003. Proceedings. 2003 IEEE/ASME International Conference on.
2003.
21. Polzer, A., et al. Integrated filter-less BiCMOS sensor for RGB-LED color
determination. in Sensors, 2011 IEEE. 2011.
52

APPENDIX 1

SOURCE CODE (SCANNING SYSTEM)

#include <Servo.h>
#include <SPI.h>
#include <nRF24L01.h>

char mypacket;
int sendingByte;
// Define colour sensor LED pins
int ledArray[] = {3,4,5};
Servo myservo;

// boolean to know if the balance has been set


boolean balanceSet = false;

//place holders for colour detected


int red=0;
int green=0;
int blue=0;

char test;

//floats to hold colour arrays


float colourArray[] = {0,0,0};
float whiteArray[] = {0,0,0};
float blackArray[] = {0,0,0};
float tempcolour[] = {0,0,0};

//place holder for average


int avgRead;

void setup(){
Serial.begin(9600);
Mirf.spi = &MirfHardwareSpi;
Mirf.csnPin = 10; // define CSN pin
Mirf.cePin = 9; // define CE pin
Mirf.init();
Mirf.setRADDR((byte *)"5469"); // receiving address
53

Mirf.setTADDR((byte *)"5469"); // transmitting address


Mirf.payload = sizeof(mypacket); // up to 32 byte only
Mirf.channel = 100; // pipeline channel
Mirf.config();
//setup the outputs for the colour sensor
pinMode(3,OUTPUT);
pinMode(4,OUTPUT);
pinMode(5,OUTPUT);
myservo.attach (2);
myservo.write(180);
//begin serial communication
Serial.begin(9600);

void loop(){

Mirf.getData((byte *) &mypacket);
test = Serial.read();
if(test == 'X')
{
myservo.write(110);
delay(1000);
myservo.write(180);

}
checkBalance();
checkColour();
printColour();
if (mypacket = 'X')
{

sortsys();
}
}

void checkBalance(){
//check if the balance has been set, if not, set it
if(balanceSet == false){
delay(3000);
setBalance();
}
}

void setBalance(){
//set white balance
delay(5000); //delay for five seconds, this gives us time to get a white sample in front of our
sensor
//scan the white sample.
54

//go through each light, get a reading, set the base reading for each colour red, green, and blue to the white array
for(int i = 0;i<=2;i++){
digitalWrite(ledArray[i],LOW);
delay(100);
baca(); //number is the number of scans to take for average, this whole function is redundant, one reading
works just as well.
whiteArray[i] = avgRead;
digitalWrite(ledArray[i],HIGH);
delay(100);
}
//done scanning white, now it will pulse blue to tell you that it is time for the black (or grey) sample.
//set black balance
delay(5000); //wait for five seconds so we can position our black sample
//go ahead and scan, sets the colour values for red, green, and blue when exposed to black
for(int i = 0;i<=2;i++){
digitalWrite(ledArray[i],LOW);
delay(100);
baca();
blackArray[i] = avgRead;
//blackArray[i] = analogRead(2);
digitalWrite(ledArray[i],HIGH);
delay(100);
}
for(int j = 0;j<=2;j++){
digitalWrite(ledArray[j],LOW); //turn or the LED, red, green or blue depending which iteration
delay(100); //delay to allow CdS to stabalize, they are slow
baca(); //take a reading however many times
tempcolour[j] = avgRead; //set the current colour in the array to the average reading
float greyDiff = whiteArray[j] - blackArray[j]; //the highest possible return minus the lowest returns
the area for values in between
tempcolour[j] = (tempcolour[j] - blackArray[j])/(greyDiff)*255; //the reading returned minus the lowest value
divided by the possible range multiplied by 255 will give us a value roughly between 0-255 representing the value for the
current reflectivity(for the colour it is exposed to) of what is being scanned
digitalWrite(ledArray[j],HIGH); //turn off the current LED
delay(100);
}
//set boolean value so we know that balance is set
balanceSet = true;
delay(5000); //delay another 5 seconds to let us catch up
}

int checkColour(){

for(int i = 0;i<=2;i++){
digitalWrite(ledArray[i],LOW); //turn or the LED, red, green or blue depending which iteration
delay(100);
baca(); //take a reading however many times
colourArray[i] = avgRead; //set the current colour in the array to the average reading
float greyDiff = whiteArray[i] - blackArray[i];
55

colourArray[i] = (colourArray[i] - blackArray[i])/(greyDiff)*255;


colourArray[i] = colourArray[i] + tempcolour[i];
digitalWrite(ledArray[i],HIGH);
delay(100);
}

int baca()
{
int times =5;
int reading1;
int reading2;
int reading3;
int tally=0;
int tally1=0;
int tally2=0;
int tally3=0;
//take the reading however many times was requested and add them up
for(int i = 0;i < times;i++){
reading1 = analogRead(0);
tally1 = reading1 + tally1;
reading2 = analogRead(1);
tally2 = reading2 + tally2;
reading3 = analogRead(2);
tally3 = reading3 + tally3;

delay(10);
}
tally = tally1+tally2+tally3;
//calculate the average and set it
avgRead = (tally)/times;
}

//prints the colour in the colour array, in the next step, we will send this to processing to see how good the sensor
works.
void printColour(){
Serial.print("R = ");
Serial.println(int(colourArray[0]));
Serial.print("G = ");
Serial.println(int(colourArray[1]));
Serial.print("B = ");
Serial.println(int(colourArray[2]));
//delay(2000);
}

void sortsys()
56

{
if((colourArray[0] >= 500) && (colourArray[0] <= 1100))
{
if((colourArray[1] >= 15) && (colourArray[1] <= 90))
{
myservo.write(110);
sendingByte = 'A';
delay(3000);
mypacket=sendingByte;
Mirf.send((byte *) &mypacket);
while(Mirf.isSending()){}
myservo.write(180);
}
else if((colourArray[1] >= 100) && (colourArray[1] <= 200))
{
myservo.write(110);
sendingByte = 'B';
delay(3000);
mypacket=sendingByte;
Mirf.send((byte *) &mypacket);
while(Mirf.isSending()){}
myservo.write(180);

}
}
else if((colourArray[0] >= 1500) && (colourArray[0] <= 2300))
{
myservo.write(110);
sendingByte = 'B';
delay(3000);
mypacket=sendingByte;
Mirf.send((byte *) &mypacket);
while(Mirf.isSending()){}
myservo.write(180);
}

else if ((colourArray[0] >= -1500) && (colourArray[0] <= -700))


{
myservo.write(110);
sendingByte = 'C';
delay(3000);
mypacket=sendingByte;
Mirf.send((byte *) &mypacket);
while(Mirf.isSending()){}
myservo.write(180);
}
}
57

APPENDIX 2

SOURCE CODE (MOBILE ROBOT)

#include <AFMotor.h>
#include <Servo.h>
#include <SPI.h>
#include <nRF24L01.h>

AF_DCMotor motor2(2, MOTOR12_64KHZ);


AF_DCMotor motor1(1, MOTOR12_64KHZ);

void(* resetFunc)(void) = 0;

boolean state = true;


boolean state1 = true;
//boolean state2= true;

int LeftSen = 30; //output of left sensor from LSS05 is connected to pin 24
int LeftMSen = 31; //output of left middle sensor from LSS05 is connected to pin 26
int MidSen = 32; //output of Middle sensor from LSS05 is connected to pin 28
int RightMSen = 33; //output of Right Middle sensor from LSS05 is connected to pin 30
int RightSen = 34; //output of Right sensor from LSS05 is connected to pin 32
int x;
int y;

char mypacket;
int sendingByte;
Servo myservo;
void setup()
{
Serial.begin(9600);
Mirf.spi = &MirfHardwareSpi;
Mirf.csnPin = 49; // define CSN pin
Mirf.cePin = 48; // define CE pin
Mirf.init();
Mirf.setRADDR((byte *)"5469"); // receiving address
Mirf.setTADDR((byte *)"5469"); // transmitting address
Mirf.payload = sizeof(mypacket); // up to 32 byte only
Mirf.channel = 100; // pipeline channel
Mirf.config();
myservo.attach (10);
58

myservo.write(180);

void loop()
{
Mirf.getData((byte *) &mypacket);
switch(mypacket)
{
case 'A':
{

GoPathA();

break;
}

case 'B':
{
GoPathB();
break;

}
case 'C':
{

GoPathC();

break;

}
default:
{
motor2.run(RELEASE); // turn it on going forward
motor1.run(RELEASE); // turn it on going forward
break;
}
}

void GoPathA()
{

motor2.run(FORWARD); // turn it on going forward


motor2.setSpeed(125);
motor1.run(FORWARD); // turn it on going forward
motor1.setSpeed(135);
delay(250);
Serial.print(mypacket);
59

while (1)
{

if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 1) &&


(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 0))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(125);
motor1.run(FORWARD); // turn it on going forward
motor1.setSpeed(135);
}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 1) && (digitalRead(MidSen) == 1) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 0))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(125);
motor1.run(FORWARD); // turn it on going forward
motor1.setSpeed(110);
}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 1) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 0))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(125); //PWM Speed Control
motor1.run(FORWARD); //PWM Speed Control
motor1.setSpeed(100);
}
else if((digitalRead(LeftSen) == 1) && (digitalRead(LeftMSen) == 1) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 0))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(125); //PWM Speed Control
motor1.run(FORWARD); //PWM Speed Control
motor1.setSpeed(100);

}
else if((digitalRead(LeftSen) == 1) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 0))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(125); //PWM Speed Control
motor1.run(BACKWARD); //PWM Speed Control
motor1.setSpeed(100);

}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 1) &&
(digitalRead(RightMSen) == 1) && (digitalRead(RightSen) == 0))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(100); //PWM Speed Control
60

motor1.run(FORWARD); //PWM Speed Control


motor1.setSpeed(135);

}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 1) && (digitalRead(RightSen) == 0))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(90); //PWM Speed Control
motor1.run(FORWARD); //PWM Speed Control
motor1.setSpeed(135);
}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 1) && (digitalRead(RightSen) == 1))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(90); //PWM Speed Control
motor1.run(FORWARD); //PWM Speed Control
motor1.setSpeed(135);
}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 1))
{
motor2.run(BACKWARD); // turn it on going forward
motor2.setSpeed(90); //PWM Speed Control
motor1.run(FORWARD); //PWM Speed Control
motor1.setSpeed(135);
}
else if((digitalRead(LeftSen) == 1) && (digitalRead(LeftMSen) == 1) && (digitalRead(MidSen) == 1) &&
(digitalRead(RightMSen) == 1) && (digitalRead(RightSen) == 1))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(125); //PWM Speed Control
motor1.run(FORWARD); //PWM Speed Control
motor1.setSpeed(135);
delay(300);
while(1)
{
if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 1) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 0))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(125);
motor1.run(FORWARD); // turn it on going forward
motor1.setSpeed(135);
}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 1) && (digitalRead(MidSen) == 1) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 0))
{
motor2.run(FORWARD); // turn it on going forward
61

motor2.setSpeed(125);
motor1.run(FORWARD); // turn it on going forward
motor1.setSpeed(110);
}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 1) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 0))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(125); //PWM Speed Control
motor1.run(FORWARD); //PWM Speed Control
motor1.setSpeed(100);
}
else if((digitalRead(LeftSen) == 1) && (digitalRead(LeftMSen) == 1) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 0))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(125); //PWM Speed Control
motor1.run(FORWARD); //PWM Speed Control
motor1.setSpeed(100);

}
else if((digitalRead(LeftSen) == 1) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 0))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(125); //PWM Speed Control
motor1.run(BACKWARD); //PWM Speed Control
motor1.setSpeed(100);

}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 1) &&
(digitalRead(RightMSen) == 1) && (digitalRead(RightSen) == 0))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(100); //PWM Speed Control
motor1.run(FORWARD); //PWM Speed Control
motor1.setSpeed(135);

}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 1) && (digitalRead(RightSen) == 0))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(90); //PWM Speed Control
motor1.run(FORWARD); //PWM Speed Control
motor1.setSpeed(135);
}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 1) && (digitalRead(RightSen) == 1))
{
62

motor2.run(FORWARD); // turn it on going forward


motor2.setSpeed(90); //PWM Speed Control
motor1.run(FORWARD); //PWM Speed Control
motor1.setSpeed(135);
}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 1))
{
motor2.run(BACKWARD); // turn it on going forward
motor2.setSpeed(90); //PWM Speed Control
motor1.run(FORWARD); //PWM Speed Control
motor1.setSpeed(135);
}
else if((digitalRead(LeftSen) == 1) && (digitalRead(LeftMSen) == 1) && (digitalRead(MidSen) == 1) &&
(digitalRead(RightMSen) == 1) && (digitalRead(RightSen) == 1))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(125); //PWM Speed Control
motor1.run(FORWARD); //PWM Speed Control
motor1.setSpeed(135);
delay(700);
while(1)
{
if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 1) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 0))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(125);
motor1.run(FORWARD); // turn it on going forward
motor1.setSpeed(135);
}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 1) && (digitalRead(MidSen) == 1) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 0))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(125);
motor1.run(FORWARD); // turn it on going forward
motor1.setSpeed(110);
}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 1) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 0))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(125); //PWM Speed Control
motor1.run(FORWARD); //PWM Speed Control
motor1.setSpeed(100);
}
else if((digitalRead(LeftSen) == 1) && (digitalRead(LeftMSen) == 1) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 0))
{
63

motor2.run(FORWARD); // turn it on going forward


motor2.setSpeed(125); //PWM Speed Control
motor1.run(FORWARD); //PWM Speed Control
motor1.setSpeed(100);

}
else if((digitalRead(LeftSen) == 1) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 0))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(125); //PWM Speed Control
motor1.run(BACKWARD); //PWM Speed Control
motor1.setSpeed(100);

}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 1) &&
(digitalRead(RightMSen) == 1) && (digitalRead(RightSen) == 0))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(100); //PWM Speed Control
motor1.run(FORWARD); //PWM Speed Control
motor1.setSpeed(135);

}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 1) && (digitalRead(RightSen) == 0))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(90); //PWM Speed Control
motor1.run(FORWARD); //PWM Speed Control
motor1.setSpeed(135);
}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 1) && (digitalRead(RightSen) == 1))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(90); //PWM Speed Control
motor1.run(FORWARD); //PWM Speed Control
motor1.setSpeed(135);
}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 1))
{
motor2.run(BACKWARD); // turn it on going forward
motor2.setSpeed(90); //PWM Speed Control
motor1.run(FORWARD); //PWM Speed Control
motor1.setSpeed(135);
}
else if((digitalRead(LeftSen) == 1) && (digitalRead(LeftMSen) == 1) && (digitalRead(MidSen) == 1) &&
(digitalRead(RightMSen) == 1) && (digitalRead(RightSen) == 1))
64

motor2.run(RELEASE); // turn it off going forward


motor1.run(RELEASE); // turn it off going forward
delay(1000);
myservo.write(90);
delay(2000);
myservo.write(180);
delay(2000);
motor2.run(BACKWARD); // turn it on going forward
motor2.setSpeed(125); //PWM Speed Control
motor1.run(BACKWARD); //PWM Speed Control
motor1.setSpeed(125);
delay(1585);
motor2.run(RELEASE); // turn it off going forward
motor1.run(RELEASE);
mypacket = 'V';
Mirf.send((byte *) &mypacket);
while(Mirf.isSending()){}

resetFunc();

}
}
}

}
}
}
}

void GoPathC()
{

motor2.run(FORWARD); // turn it on going forward


motor2.setSpeed(125);
motor1.run(FORWARD); // turn it on going forward
motor1.setSpeed(135);
delay(250);
Serial.print(mypacket);
while (1)
{

if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 1) &&


(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 0))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(125);
motor1.run(FORWARD); // turn it on going forward
65

motor1.setSpeed(135);
}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 1) && (digitalRead(MidSen) == 1) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 0))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(125);
motor1.run(FORWARD); // turn it on going forward
motor1.setSpeed(110);
}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 1) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 0))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(125); //PWM Speed Control
motor1.run(FORWARD); //PWM Speed Control
motor1.setSpeed(100);
}
else if((digitalRead(LeftSen) == 1) && (digitalRead(LeftMSen) == 1) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 0))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(125); //PWM Speed Control
motor1.run(FORWARD); //PWM Speed Control
motor1.setSpeed(100);

}
else if((digitalRead(LeftSen) == 1) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 0))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(125); //PWM Speed Control
motor1.run(BACKWARD); //PWM Speed Control
motor1.setSpeed(100);

}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 1) &&
(digitalRead(RightMSen) == 1) && (digitalRead(RightSen) == 0))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(100); //PWM Speed Control
motor1.run(FORWARD); //PWM Speed Control
motor1.setSpeed(135);

}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 1) && (digitalRead(RightSen) == 0))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(90); //PWM Speed Control
66

motor1.run(FORWARD); //PWM Speed Control


motor1.setSpeed(135);
}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 1) && (digitalRead(RightSen) == 1))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(90); //PWM Speed Control
motor1.run(FORWARD); //PWM Speed Control
motor1.setSpeed(135);
}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 1))
{
motor2.run(BACKWARD); // turn it on going forward
motor2.setSpeed(90); //PWM Speed Control
motor1.run(FORWARD); //PWM Speed Control
motor1.setSpeed(135);
}
else if((digitalRead(LeftSen) == 1) && (digitalRead(LeftMSen) == 1) && (digitalRead(MidSen) == 1) &&
(digitalRead(RightMSen) == 1) && (digitalRead(RightSen) == 1))
{
while(1)
{
motor2.run(BACKWARD); // turn it on going forward
motor2.setSpeed(100); //PWM Speed Control
motor1.run(FORWARD); //PWM Speed Control
motor1.setSpeed(135);
delay(500);
if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 1) &&
(digitalRead(RightMSen) == 1) && (digitalRead(RightSen) == 1))
{
while(1)
{
if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 1) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 0))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(125);
motor1.run(FORWARD); // turn it on going forward
motor1.setSpeed(135);
}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 1) && (digitalRead(MidSen) == 1) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 0))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(125);
motor1.run(FORWARD); // turn it on going forward
motor1.setSpeed(110);
}
67

else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 1) && (digitalRead(MidSen) == 0) &&


(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 0))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(125); //PWM Speed Control
motor1.run(FORWARD); //PWM Speed Control
motor1.setSpeed(100);
}
else if((digitalRead(LeftSen) == 1) && (digitalRead(LeftMSen) == 1) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 0))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(125); //PWM Speed Control
motor1.run(FORWARD); //PWM Speed Control
motor1.setSpeed(100);

}
else if((digitalRead(LeftSen) == 1) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 0))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(125); //PWM Speed Control
motor1.run(BACKWARD); //PWM Speed Control
motor1.setSpeed(100);

}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 1) &&
(digitalRead(RightMSen) == 1) && (digitalRead(RightSen) == 0))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(100); //PWM Speed Control
motor1.run(FORWARD); //PWM Speed Control
motor1.setSpeed(135);

}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 1) && (digitalRead(RightSen) == 0))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(90); //PWM Speed Control
motor1.run(FORWARD); //PWM Speed Control
motor1.setSpeed(135);
}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 1) && (digitalRead(RightSen) == 1))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(90); //PWM Speed Control
motor1.run(FORWARD); //PWM Speed Control
motor1.setSpeed(135);
68

}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 1))
{
motor2.run(BACKWARD); // turn it on going forward
motor2.setSpeed(90); //PWM Speed Control
motor1.run(FORWARD); //PWM Speed Control
motor1.setSpeed(135);
}
else if((digitalRead(LeftSen) == 1) && (digitalRead(LeftMSen) == 1) && (digitalRead(MidSen) == 1) &&
(digitalRead(RightMSen) == 1) && (digitalRead(RightSen) == 1))
{
while(1)
{
if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 1) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 0))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(125);
motor1.run(FORWARD); // turn it on going forward
motor1.setSpeed(135);
}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 1) && (digitalRead(MidSen) == 1) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 0))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(125);
motor1.run(FORWARD); // turn it on going forward
motor1.setSpeed(110);
}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 1) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 0))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(125); //PWM Speed Control
motor1.run(FORWARD); //PWM Speed Control
motor1.setSpeed(100);
}
else if((digitalRead(LeftSen) == 1) && (digitalRead(LeftMSen) == 1) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 0))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(125); //PWM Speed Control
motor1.run(FORWARD); //PWM Speed Control
motor1.setSpeed(100);

}
else if((digitalRead(LeftSen) == 1) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 0))
{
69

motor2.run(FORWARD); // turn it on going forward


motor2.setSpeed(125); //PWM Speed Control
motor1.run(BACKWARD); //PWM Speed Control
motor1.setSpeed(100);

}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 1) &&
(digitalRead(RightMSen) == 1) && (digitalRead(RightSen) == 0))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(100); //PWM Speed Control
motor1.run(FORWARD); //PWM Speed Control
motor1.setSpeed(135);

}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 1) && (digitalRead(RightSen) == 0))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(90); //PWM Speed Control
motor1.run(FORWARD); //PWM Speed Control
motor1.setSpeed(135);
}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 1) && (digitalRead(RightSen) == 1))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(90); //PWM Speed Control
motor1.run(FORWARD); //PWM Speed Control
motor1.setSpeed(135);
}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 1))
{
motor2.run(BACKWARD); // turn it on going forward
motor2.setSpeed(90); //PWM Speed Control
motor1.run(FORWARD); //PWM Speed Control
motor1.setSpeed(135);
}
else if((digitalRead(LeftSen) == 1) && (digitalRead(LeftMSen) == 1) && (digitalRead(MidSen) == 1) &&
(digitalRead(RightMSen) == 1) && (digitalRead(RightSen) == 1))
{
motor2.run(RELEASE); // turn it on going forward
motor1.run(RELEASE); //PWM Speed Control
delay(1000);
myservo.write(90);
delay(2000);
myservo.write(180);
delay(2000);
motor2.run(BACKWARD); // turn it on going forward
70

motor2.setSpeed(125); //PWM Speed Control


motor1.run(BACKWARD); //PWM Speed Control
motor1.setSpeed(135);
delay(300);

while(1)
{

if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 1) &&


(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 0))
{
motor2.run(BACKWARD); // turn it on going forward
motor2.setSpeed(125);
motor1.run(BACKWARD); // turn it on going forward
motor1.setSpeed(135);
}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 1) && (digitalRead(MidSen) == 1) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 0))
{
motor2.run(BACKWARD); // turn it on going forward
motor2.setSpeed(125);
motor1.run(BACKWARD); // turn it on going forward
motor1.setSpeed(110);
}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 1) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 0))
{
motor2.run(BACKWARD); // turn it on going forward
motor2.setSpeed(125); //PWM Speed Control
motor1.run(BACKWARD); //PWM Speed Control
motor1.setSpeed(100);
}
else if((digitalRead(LeftSen) == 1) && (digitalRead(LeftMSen) == 1) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 0))
{
motor2.run(BACKWARD); // turn it on going forward
motor2.setSpeed(125); //PWM Speed Control
motor1.run(BACKWARD); //PWM Speed Control
motor1.setSpeed(100);

}
else if((digitalRead(LeftSen) == 1) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 0))
{
motor2.run(BACKWARD); // turn it on going forward
motor2.setSpeed(125); //PWM Speed Control
motor1.run(FORWARD); //PWM Speed Control
motor1.setSpeed(100);

}
71

else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 1) &&


(digitalRead(RightMSen) == 1) && (digitalRead(RightSen) == 0))
{
motor2.run(BACKWARD); // turn it on going forward
motor2.setSpeed(100); //PWM Speed Control
motor1.run(BACKWARD); //PWM Speed Control
motor1.setSpeed(135);

}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 1) && (digitalRead(RightSen) == 0))
{
motor2.run(BACKWARD); // turn it on going forward
motor2.setSpeed(90); //PWM Speed Control
motor1.run(BACKWARD); //PWM Speed Control
motor1.setSpeed(135);
}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 1) && (digitalRead(RightSen) == 1))
{
motor2.run(BACKWARD); // turn it on going forward
motor2.setSpeed(90); //PWM Speed Control
motor1.run(BACKWARD); //PWM Speed Control
motor1.setSpeed(135);
}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 1))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(90); //PWM Speed Control
motor1.run(BACKWARD); //PWM Speed Control
motor1.setSpeed(135);
}

else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 1) &&


(digitalRead(RightMSen) == 1) && (digitalRead(RightSen) == 1))
{
motor2.run(BACKWARD); // turn it on going forward
motor2.setSpeed(125);
motor1.run(BACKWARD); // turn it on going forward
motor1.setSpeed(135);

else if((digitalRead(LeftSen) == 1) && (digitalRead(LeftMSen) == 1) && (digitalRead(MidSen) == 1) &&


(digitalRead(RightMSen) == 1) && (digitalRead(RightSen) == 1))
{
while(1)
{
motor2.run(BACKWARD); // turn it on going forward
72

motor2.setSpeed(125);
motor1.run(FORWARD); // turn it on going forward
motor1.setSpeed(135);
if((digitalRead(LeftSen) == 1) && (digitalRead(LeftMSen) == 1) && (digitalRead(MidSen) == 1) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 0))
{
while(1)
{
if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 1) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 0))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(125);
motor1.run(FORWARD); // turn it on going forward
motor1.setSpeed(135);
}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 1) && (digitalRead(MidSen) == 1) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 0))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(125);
motor1.run(FORWARD); // turn it on going forward
motor1.setSpeed(110);
}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 1) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 0))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(125); //PWM Speed Control
motor1.run(FORWARD); //PWM Speed Control
motor1.setSpeed(100);
}
else if((digitalRead(LeftSen) == 1) && (digitalRead(LeftMSen) == 1) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 0))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(125); //PWM Speed Control
motor1.run(FORWARD); //PWM Speed Control
motor1.setSpeed(100);

}
else if((digitalRead(LeftSen) == 1) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 0))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(125); //PWM Speed Control
motor1.run(BACKWARD); //PWM Speed Control
motor1.setSpeed(100);

}
73

else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 1) &&


(digitalRead(RightMSen) == 1) && (digitalRead(RightSen) == 0))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(100); //PWM Speed Control
motor1.run(FORWARD); //PWM Speed Control
motor1.setSpeed(135);

}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 1) && (digitalRead(RightSen) == 0))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(90); //PWM Speed Control
motor1.run(FORWARD); //PWM Speed Control
motor1.setSpeed(135);
}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 1) && (digitalRead(RightSen) == 1))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(90); //PWM Speed Control
motor1.run(FORWARD); //PWM Speed Control
motor1.setSpeed(135);
}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 1))
{
motor2.run(BACKWARD); // turn it on going forward
motor2.setSpeed(90); //PWM Speed Control
motor1.run(FORWARD); //PWM Speed Control
motor1.setSpeed(135);
}

else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 1) &&


(digitalRead(RightMSen) == 1) && (digitalRead(RightSen) == 1))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(125);
motor1.run(FORWARD); // turn it on going forward
motor1.setSpeed(135);

else if((digitalRead(LeftSen) == 1) && (digitalRead(LeftMSen) == 1) && (digitalRead(MidSen) == 1) &&


(digitalRead(RightMSen) == 1) && (digitalRead(RightSen) == 1))
{
if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 1) &&
(digitalRead(RightMSen) == 1) && (digitalRead(RightSen) == 1))
{
74

while(1)
{
if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 1) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 0))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(125);
motor1.run(FORWARD); // turn it on going forward
motor1.setSpeed(135);
}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 1) && (digitalRead(MidSen) == 1) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 0))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(125);
motor1.run(FORWARD); // turn it on going forward
motor1.setSpeed(110);
}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 1) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 0))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(125); //PWM Speed Control
motor1.run(FORWARD); //PWM Speed Control
motor1.setSpeed(100);
}
else if((digitalRead(LeftSen) == 1) && (digitalRead(LeftMSen) == 1) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 0))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(125); //PWM Speed Control
motor1.run(FORWARD); //PWM Speed Control
motor1.setSpeed(100);

}
else if((digitalRead(LeftSen) == 1) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 0))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(125); //PWM Speed Control
motor1.run(BACKWARD); //PWM Speed Control
motor1.setSpeed(100);

}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 1) &&
(digitalRead(RightMSen) == 1) && (digitalRead(RightSen) == 0))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(100); //PWM Speed Control
motor1.run(FORWARD); //PWM Speed Control
75

motor1.setSpeed(135);

}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 1) && (digitalRead(RightSen) == 0))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(90); //PWM Speed Control
motor1.run(FORWARD); //PWM Speed Control
motor1.setSpeed(135);
}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 1) && (digitalRead(RightSen) == 1))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(90); //PWM Speed Control
motor1.run(FORWARD); //PWM Speed Control
motor1.setSpeed(135);
}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 1))
{
motor2.run(BACKWARD); // turn it on going forward
motor2.setSpeed(90); //PWM Speed Control
motor1.run(FORWARD); //PWM Speed Control
motor1.setSpeed(135);
}

else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 1) &&


(digitalRead(RightMSen) == 1) && (digitalRead(RightSen) == 1))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(125);
motor1.run(FORWARD); // turn it on going forward
motor1.setSpeed(135);

else if((digitalRead(LeftSen) == 1) && (digitalRead(LeftMSen) == 1) && (digitalRead(MidSen) == 1) &&


(digitalRead(RightMSen) == 1) && (digitalRead(RightSen) == 1))
{
if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 1) &&
(digitalRead(RightMSen) == 1) && (digitalRead(RightSen) == 1))
{
while(1)
{
if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 1) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 0))
{
motor2.run(FORWARD); // turn it on going forward
76

motor2.setSpeed(125);
motor1.run(FORWARD); // turn it on going forward
motor1.setSpeed(135);
}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 1) && (digitalRead(MidSen) == 1) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 0))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(125);
motor1.run(FORWARD); // turn it on going forward
motor1.setSpeed(110);
}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 1) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 0))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(125); //PWM Speed Control
motor1.run(FORWARD); //PWM Speed Control
motor1.setSpeed(100);
}
else if((digitalRead(LeftSen) == 1) && (digitalRead(LeftMSen) == 1) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 0))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(125); //PWM Speed Control
motor1.run(FORWARD); //PWM Speed Control
motor1.setSpeed(100);

}
else if((digitalRead(LeftSen) == 1) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 0))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(125); //PWM Speed Control
motor1.run(BACKWARD); //PWM Speed Control
motor1.setSpeed(100);

}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 1) &&
(digitalRead(RightMSen) == 1) && (digitalRead(RightSen) == 0))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(100); //PWM Speed Control
motor1.run(FORWARD); //PWM Speed Control
motor1.setSpeed(135);

}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 1) && (digitalRead(RightSen) == 0))
{
77

motor2.run(FORWARD); // turn it on going forward


motor2.setSpeed(90); //PWM Speed Control
motor1.run(FORWARD); //PWM Speed Control
motor1.setSpeed(135);
}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 1) && (digitalRead(RightSen) == 1))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(90); //PWM Speed Control
motor1.run(FORWARD); //PWM Speed Control
motor1.setSpeed(135);
}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 1))
{
motor2.run(BACKWARD); // turn it on going forward
motor2.setSpeed(90); //PWM Speed Control
motor1.run(FORWARD); //PWM Speed Control
motor1.setSpeed(135);
}

else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 1) &&


(digitalRead(RightMSen) == 1) && (digitalRead(RightSen) == 1))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(125);
motor1.run(FORWARD); // turn it on going forward
motor1.setSpeed(135);

else if((digitalRead(LeftSen) == 1) && (digitalRead(LeftMSen) == 1) && (digitalRead(MidSen) == 1) &&


(digitalRead(RightMSen) == 1) && (digitalRead(RightSen) == 1))
{
motor2.run(RELEASE); // turn it on going forward
motor1.run(RELEASE); // turn it on going forward
mypacket = 'V';
Mirf.send((byte *) &mypacket);
while(Mirf.isSending()){}

resetFunc();

}
78

}
}}}}}}}}}}}}}}}}}}
void GoPathB()
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(125);
motor1.run(FORWARD); // turn it on going forward
motor1.setSpeed(135);
delay(250);
Serial.print(mypacket);
while (1)
{

if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 1) &&


(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 0))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(125);
motor1.run(FORWARD); // turn it on going forward
motor1.setSpeed(135);
}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 1) && (digitalRead(MidSen) == 1) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 0))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(125);
motor1.run(FORWARD); // turn it on going forward
motor1.setSpeed(110);
}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 1) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 0))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(125); //PWM Speed Control
motor1.run(FORWARD); //PWM Speed Control
motor1.setSpeed(100);
}
else if((digitalRead(LeftSen) == 1) && (digitalRead(LeftMSen) == 1) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 0))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(125); //PWM Speed Control
motor1.run(FORWARD); //PWM Speed Control
motor1.setSpeed(100);

}
else if((digitalRead(LeftSen) == 1) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 0))
{
motor2.run(FORWARD); // turn it on going forward
79

motor2.setSpeed(125); //PWM Speed Control


motor1.run(BACKWARD); //PWM Speed Control
motor1.setSpeed(100);

}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 1) &&
(digitalRead(RightMSen) == 1) && (digitalRead(RightSen) == 0))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(100); //PWM Speed Control
motor1.run(FORWARD); //PWM Speed Control
motor1.setSpeed(135);

}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 1) && (digitalRead(RightSen) == 0))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(90); //PWM Speed Control
motor1.run(FORWARD); //PWM Speed Control
motor1.setSpeed(135);
}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 1) && (digitalRead(RightSen) == 1))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(90); //PWM Speed Control
motor1.run(FORWARD); //PWM Speed Control
motor1.setSpeed(135);
}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 1))
{
motor2.run(BACKWARD); // turn it on going forward
motor2.setSpeed(90); //PWM Speed Control
motor1.run(FORWARD); //PWM Speed Control
motor1.setSpeed(135);
}
else if((digitalRead(LeftSen) == 1) && (digitalRead(LeftMSen) == 1) && (digitalRead(MidSen) == 1) &&
(digitalRead(RightMSen) == 1) && (digitalRead(RightSen) == 1))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(125);
motor1.run(FORWARD); // turn it on going forward
motor1.setSpeed(135);
delay(300);
while(1)
{
80

if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 1) &&


(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 0))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(125);
motor1.run(FORWARD); // turn it on going forward
motor1.setSpeed(135);
}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 1) && (digitalRead(MidSen) == 1) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 0))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(125);
motor1.run(FORWARD); // turn it on going forward
motor1.setSpeed(110);
}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 1) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 0))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(125); //PWM Speed Control
motor1.run(FORWARD); //PWM Speed Control
motor1.setSpeed(100);
}
else if((digitalRead(LeftSen) == 1) && (digitalRead(LeftMSen) == 1) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 0))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(125); //PWM Speed Control
motor1.run(FORWARD); //PWM Speed Control
motor1.setSpeed(100);

}
else if((digitalRead(LeftSen) == 1) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 0))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(125); //PWM Speed Control
motor1.run(BACKWARD); //PWM Speed Control
motor1.setSpeed(100);

}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 1) &&
(digitalRead(RightMSen) == 1) && (digitalRead(RightSen) == 0))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(100); //PWM Speed Control
motor1.run(FORWARD); //PWM Speed Control
motor1.setSpeed(135);
81

}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 1) && (digitalRead(RightSen) == 0))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(90); //PWM Speed Control
motor1.run(FORWARD); //PWM Speed Control
motor1.setSpeed(135);
}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 1) && (digitalRead(RightSen) == 1))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(90); //PWM Speed Control
motor1.run(FORWARD); //PWM Speed Control
motor1.setSpeed(135);
}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 1))
{
motor2.run(BACKWARD); // turn it on going forward
motor2.setSpeed(90); //PWM Speed Control
motor1.run(FORWARD); //PWM Speed Control
motor1.setSpeed(135);
}
else if((digitalRead(LeftSen) == 1) && (digitalRead(LeftMSen) == 1) && (digitalRead(MidSen) == 1) &&
(digitalRead(RightMSen) == 1) && (digitalRead(RightSen) == 1))
{
while(1)
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(125); //PWM Speed Control
motor1.run(BACKWARD); //PWM Speed Control
motor1.setSpeed(135);
if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 1) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 0))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(125);
motor1.run(FORWARD); // turn it on going forward
motor1.setSpeed(135);
delay(300);
while(1)
{
if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 1) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 0))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(125);
motor1.run(FORWARD); // turn it on going forward
82

motor1.setSpeed(135);
}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 1) && (digitalRead(MidSen) == 1) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 0))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(125);
motor1.run(FORWARD); // turn it on going forward
motor1.setSpeed(110);
}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 1) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 0))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(125); //PWM Speed Control
motor1.run(FORWARD); //PWM Speed Control
motor1.setSpeed(100);
}
else if((digitalRead(LeftSen) == 1) && (digitalRead(LeftMSen) == 1) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 0))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(125); //PWM Speed Control
motor1.run(FORWARD); //PWM Speed Control
motor1.setSpeed(100);

}
else if((digitalRead(LeftSen) == 1) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 0))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(125); //PWM Speed Control
motor1.run(BACKWARD); //PWM Speed Control
motor1.setSpeed(100);

}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 1) &&
(digitalRead(RightMSen) == 1) && (digitalRead(RightSen) == 0))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(100); //PWM Speed Control
motor1.run(FORWARD); //PWM Speed Control
motor1.setSpeed(135);

}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 1) && (digitalRead(RightSen) == 0))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(90); //PWM Speed Control
83

motor1.run(FORWARD); //PWM Speed Control


motor1.setSpeed(135);
}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 1) && (digitalRead(RightSen) == 1))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(90); //PWM Speed Control
motor1.run(FORWARD); //PWM Speed Control
motor1.setSpeed(135);
}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 1))
{
motor2.run(BACKWARD); // turn it on going forward
motor2.setSpeed(90); //PWM Speed Control
motor1.run(FORWARD); //PWM Speed Control
motor1.setSpeed(135);
}
else if((digitalRead(LeftSen) == 1) && (digitalRead(LeftMSen) == 1) && (digitalRead(MidSen) == 1) &&
(digitalRead(RightMSen) == 1) && (digitalRead(RightSen) == 1))
{
motor2.run(RELEASE); // turn it on going forward
motor1.run(RELEASE); //PWM Speed Control
delay(1000);
myservo.write(90);
delay(2000);
myservo.write(180);
delay(2000);
motor2.run(BACKWARD); // turn it on going forward
motor2.setSpeed(125); //PWM Speed Control
motor1.run(BACKWARD); //PWM Speed Control
motor1.setSpeed(135);
delay(300);
while(1)
{

if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 1) &&


(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 0))
{
motor2.run(BACKWARD); // turn it on going forward
motor2.setSpeed(125);
motor1.run(BACKWARD); // turn it on going forward
motor1.setSpeed(135);
}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 1) && (digitalRead(MidSen) == 1) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 0))
{
motor2.run(BACKWARD); // turn it on going forward
motor2.setSpeed(125);
84

motor1.run(BACKWARD); // turn it on going forward


motor1.setSpeed(110);
}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 1) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 0))
{
motor2.run(BACKWARD); // turn it on going forward
motor2.setSpeed(125); //PWM Speed Control
motor1.run(BACKWARD); //PWM Speed Control
motor1.setSpeed(100);
}
else if((digitalRead(LeftSen) == 1) && (digitalRead(LeftMSen) == 1) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 0))
{
motor2.run(BACKWARD); // turn it on going forward
motor2.setSpeed(125); //PWM Speed Control
motor1.run(BACKWARD); //PWM Speed Control
motor1.setSpeed(100);

}
else if((digitalRead(LeftSen) == 1) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 0))
{
motor2.run(BACKWARD); // turn it on going forward
motor2.setSpeed(125); //PWM Speed Control
motor1.run(FORWARD); //PWM Speed Control
motor1.setSpeed(100);

}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 1) &&
(digitalRead(RightMSen) == 1) && (digitalRead(RightSen) == 0))
{
motor2.run(BACKWARD); // turn it on going forward
motor2.setSpeed(100); //PWM Speed Control
motor1.run(BACKWARD); //PWM Speed Control
motor1.setSpeed(135);

}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 1) && (digitalRead(RightSen) == 0))
{
motor2.run(BACKWARD); // turn it on going forward
motor2.setSpeed(90); //PWM Speed Control
motor1.run(BACKWARD); //PWM Speed Control
motor1.setSpeed(135);
}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 1) && (digitalRead(RightSen) == 1))
{
motor2.run(BACKWARD); // turn it on going forward
85

motor2.setSpeed(90); //PWM Speed Control


motor1.run(BACKWARD); //PWM Speed Control
motor1.setSpeed(135);
}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 1))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(90); //PWM Speed Control
motor1.run(BACKWARD); //PWM Speed Control
motor1.setSpeed(135);
}

else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 1) &&


(digitalRead(RightMSen) == 1) && (digitalRead(RightSen) == 1))
{
motor2.run(BACKWARD); // turn it on going forward
motor2.setSpeed(125);
motor1.run(BACKWARD); // turn it on going forward
motor1.setSpeed(135);

else if((digitalRead(LeftSen) == 1) && (digitalRead(LeftMSen) == 1) && (digitalRead(MidSen) == 1) &&


(digitalRead(RightMSen) == 1) && (digitalRead(RightSen) == 1))
{
while(1)
{
motor2.run(BACKWARD); // turn it on going forward
motor2.setSpeed(125);
motor1.run(FORWARD); // turn it on going forward
motor1.setSpeed(135);
if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 1) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 0))
{
while(1)
{
if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 1) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 0))
{
motor2.run(BACKWARD); // turn it on going forward
motor2.setSpeed(125);
motor1.run(BACKWARD); // turn it on going forward
motor1.setSpeed(135);
}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 1) && (digitalRead(MidSen) == 1) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 0))
{
motor2.run(BACKWARD); // turn it on going forward
motor2.setSpeed(125);
86

motor1.run(BACKWARD); // turn it on going forward


motor1.setSpeed(110);
}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 1) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 0))
{
motor2.run(BACKWARD); // turn it on going forward
motor2.setSpeed(125); //PWM Speed Control
motor1.run(BACKWARD); //PWM Speed Control
motor1.setSpeed(100);
}
else if((digitalRead(LeftSen) == 1) && (digitalRead(LeftMSen) == 1) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 0))
{
motor2.run(BACKWARD); // turn it on going forward
motor2.setSpeed(125); //PWM Speed Control
motor1.run(BACKWARD); //PWM Speed Control
motor1.setSpeed(100);

}
else if((digitalRead(LeftSen) == 1) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 0))
{
motor2.run(BACKWARD); // turn it on going forward
motor2.setSpeed(125); //PWM Speed Control
motor1.run(FORWARD); //PWM Speed Control
motor1.setSpeed(100);

}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 1) &&
(digitalRead(RightMSen) == 1) && (digitalRead(RightSen) == 0))
{
motor2.run(BACKWARD); // turn it on going forward
motor2.setSpeed(100); //PWM Speed Control
motor1.run(BACKWARD); //PWM Speed Control
motor1.setSpeed(135);

}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 1) && (digitalRead(RightSen) == 0))
{
motor2.run(BACKWARD); // turn it on going forward
motor2.setSpeed(90); //PWM Speed Control
motor1.run(BACKWARD); //PWM Speed Control
motor1.setSpeed(135);
}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 1) && (digitalRead(RightSen) == 1))
{
motor2.run(BACKWARD); // turn it on going forward
87

motor2.setSpeed(90); //PWM Speed Control


motor1.run(BACKWARD); //PWM Speed Control
motor1.setSpeed(135);
}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 1))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(90); //PWM Speed Control
motor1.run(BACKWARD); //PWM Speed Control
motor1.setSpeed(135);
}

else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 1) &&


(digitalRead(RightMSen) == 1) && (digitalRead(RightSen) == 1))
{
motor2.run(BACKWARD); // turn it on going forward
motor2.setSpeed(125);
motor1.run(BACKWARD); // turn it on going forward
motor1.setSpeed(135);

else if((digitalRead(LeftSen) == 1) && (digitalRead(LeftMSen) == 1) && (digitalRead(MidSen) == 1) &&


(digitalRead(RightMSen) == 1) && (digitalRead(RightSen) == 1))
{
motor2.run(BACKWARD); // turn it on going forward
motor2.setSpeed(125);
motor1.run(BACKWARD); // turn it on going forward
motor1.setSpeed(135);
delay(300);
while(1)
{
if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 1) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 0))
{
motor2.run(BACKWARD); // turn it on going forward
motor2.setSpeed(125);
motor1.run(BACKWARD); // turn it on going forward
motor1.setSpeed(135);
}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 1) && (digitalRead(MidSen) == 1) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 0))
{
motor2.run(BACKWARD); // turn it on going forward
motor2.setSpeed(125);
motor1.run(BACKWARD); // turn it on going forward
motor1.setSpeed(110);
}
88

else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 1) && (digitalRead(MidSen) == 0) &&


(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 0))
{
motor2.run(BACKWARD); // turn it on going forward
motor2.setSpeed(125); //PWM Speed Control
motor1.run(BACKWARD); //PWM Speed Control
motor1.setSpeed(100);
}
else if((digitalRead(LeftSen) == 1) && (digitalRead(LeftMSen) == 1) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 0))
{
motor2.run(BACKWARD); // turn it on going forward
motor2.setSpeed(125); //PWM Speed Control
motor1.run(BACKWARD); //PWM Speed Control
motor1.setSpeed(100);

}
else if((digitalRead(LeftSen) == 1) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 0))
{
motor2.run(BACKWARD); // turn it on going forward
motor2.setSpeed(125); //PWM Speed Control
motor1.run(FORWARD); //PWM Speed Control
motor1.setSpeed(100);

}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 1) &&
(digitalRead(RightMSen) == 1) && (digitalRead(RightSen) == 0))
{
motor2.run(BACKWARD); // turn it on going forward
motor2.setSpeed(100); //PWM Speed Control
motor1.run(BACKWARD); //PWM Speed Control
motor1.setSpeed(135);

}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 1) && (digitalRead(RightSen) == 0))
{
motor2.run(BACKWARD); // turn it on going forward
motor2.setSpeed(90); //PWM Speed Control
motor1.run(BACKWARD); //PWM Speed Control
motor1.setSpeed(135);
}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 1) && (digitalRead(RightSen) == 1))
{
motor2.run(BACKWARD); // turn it on going forward
motor2.setSpeed(90); //PWM Speed Control
motor1.run(BACKWARD); //PWM Speed Control
motor1.setSpeed(135);
89

}
else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 0) &&
(digitalRead(RightMSen) == 0) && (digitalRead(RightSen) == 1))
{
motor2.run(FORWARD); // turn it on going forward
motor2.setSpeed(90); //PWM Speed Control
motor1.run(BACKWARD); //PWM Speed Control
motor1.setSpeed(135);
}

else if((digitalRead(LeftSen) == 0) && (digitalRead(LeftMSen) == 0) && (digitalRead(MidSen) == 1) &&


(digitalRead(RightMSen) == 1) && (digitalRead(RightSen) == 1))
{
motor2.run(BACKWARD); // turn it on going forward
motor2.setSpeed(125);
motor1.run(BACKWARD); // turn it on going forward
motor1.setSpeed(135);

else if((digitalRead(LeftSen) == 1) && (digitalRead(LeftMSen) == 1) && (digitalRead(MidSen) == 1) &&


(digitalRead(RightMSen) == 1) && (digitalRead(RightSen) == 1))
{
motor2.run(RELEASE); // turn it on going forward
motor1.run(RELEASE); // turn it on going forward
mypacket = 'V';
Mirf.send((byte *) &mypacket);
while(Mirf.isSending()){}

resetFunc();

}
}}}}}}}}}}}}}}}}