REPUBLIC OF YEMEN

SANA’A UNIVERSITY
FACULTY OF ENGINEERING
MECHATRONICS ENGINEERING DEPARTMENT

DESIGN, PROGRAM AND FABRICATION OF

AUTONOMOUS MOBILE SCARA
A THESIS
Submitted in partial fulfilment of the
requirements for the award of the degree
of
BACHELOR OF ENGINEERING
IN
MECHATRONICS ENGINEERING

BY:

Mohammed Ahmed Rizq Rashad Nagi Alshagdary

Essam Abdulrahman Almizgagi Hilal Adnan Isaac

Abualwafa Ahmed Isaac Mohanad Khaled Alhaifi

Supervised by:
Dr. Hatem Al-Dois

JANUARY 2018
 CANDIDATE’S DECLARATION

We hereby certify that the work which is being presented in the thesis entitled “Design,
program and fabrication of autonomous mobile SCARA” in partial fulfilment of the requirement
for the award of the degree of Bachelor of Engineering and submitted in Sana’a University (Faculty
of Engineering) Mechatronics Engineering Department an authentic record of our own work, under
the supervision of:

Dr. Hatem Al-Dois, Assistant Professor in Department of Electrical Engineering, Vice-Dean

for Development and Quality Assurance in Faculty of Engineering and Architecture, Ibb
University.

The matter presented in this thesis has not been submitted by us for the award of any other degree
of this University or any other University.

Mohammed Ahmed Rizq.

Rashad Nagi Alshagdary.
Essam Abdulrahman Almizgagi.
Hilal Adnan Isaac.
Abualwafa Ahmed Isaac.
Mohanad Khaled Alhaifi.

This is to certify that the above statement made by the candidates is correct to the best of our
knowledge.
Dr. Hatem Al-Dois.

i
 ACKNOWLEDGEMENT

It is always a pleasure to mention the fine people in this graduation project for their sincere
guidance we received to raise our practical as well as theoretical skills in mechatronics
engineering.

We would like to express our special thanks to Dr.Hatem Al-Dois who gave us this golden
opportunity to do a wonderful project on robotics. This opportunity helped us to do a lot of research
and came to know about many new aspects in the field. We are really thankful to him. We also
want to thank the Mechatronics department staff and the workers in the faculty for their efforts
that helped us reaching this success.

ii
 ABSTRACT

The project consists of two main parts; the manipulator and the mobile robot. The
manipulator is of type SCARA and mounted on the platform of a mobile robot. The function of
the manipulator is to take stuff from and onto the platform of the mobile robot. The mobile robot
has for wheels and is able to move autonomously to the specified position. The motion of the car
is programmed using A* search technique which finds the shortest path between the initial and
final positions and avoids any obstacles. The path is followed using PID control scheme. A
Raspberry-pi controller programmed in python along with several motor drivers are used. The
controller is programmed to receive the motion command wirelessly for a remote location. The
robot is equipped with different sensors to ensure the correct executive of the commanded motion
and to prevent the robot from damage in case of collision with surrounding objects or itself. A GUI
is developed in MATLAB for the manipulator and in python for the car to simulate the
performance of the robot. Finally, the mobile robot motion and manipulator functionally are tested
and the results are reported.

iii
 LIST OF FIGURES

Figure Title Page No.

Fig. (2.1) Serial and Parallel Manipulator 5
Fig. (2.2) Robot Configurations 7
Fig. (2.3) Robot Locomotion 8
Fig. (2.4) Robot Coordinate System 9
Fig. (2.5) Block Diagram of PID Control 12
Fig. (3.1) Raspberry pi 3 Model B 14
Fig. (3.2) Servo Motor Driver 15
Fig. (3.3) Linear Motor 16
Fig. (3.4) Robot Gripper 16
Fig. (3.5) Analog to Digital Converter 17
Fig. (3.6) SAVOX Servo Motor 18
Fig. (3.7) L298n Motor Driver 18
Fig. (3.8) Servo to Shaft Coupler 19
Fig. (3.9) Sharp IR Sensor 19
Fig. (3.10) Toothed pulley 20
Fig. (3.11) Rear Motor of the Car 20
Fig. (3.12) HC-SR04 Ultrasonic 20
Fig. (3.13) Router 21
Fig. (3.14) Shafts and Bearings of the Links 22
Fig. (3.15) Links of the Arm 23
Fig. (3.16) Shaft to Link Coupler 23
Fig. (3.17) Gear Mechanism of the First Link 24
Fig. (3.18) Movement Mechanism 24
Fig. (3.19) Base of the Robotic Arm 24
Fig. (3.20) Gripper Connection 24
Fig. (3.21) Steering Mechanism 25
Fig. (3.22) Platform of the Robot 25
Fig. (3.23) Robotic Arm after Assembly 26

iv
Fig. (3.24) The Car of the Project 26
Fig. (3.25) Inner Construction of the Car 26
Fig. (3.26) The Final Project 27
Fig. (4.1) SCARA Robot Coordinate Assignment 28
Fig. (4.2) Grid Based Algorithm 31
Fig. (4.3) A* Search Algorithm 32
Fig. (4.4) Path Smoothing 32
Fig. (4.5) Bicycle Model of the Car 33
Fig. (4.6) GUI Simulation for the SCARA Manipulator 34
Fig. (4.7) Inverse Kinematic Simulation 34
Fig. (4.8) Torque Comparison Simulation 35
Fig. (4.9) Trajectory Simulation 35
Fig. (4.10) Mobile Robot Simulation 36
Fig. (5.1) Raspberry pi Desktop 37
Fig. (5.2) Project Flow Chart 39
Fig. (5.3) Grids of the Workspace 47
Fig. (6.1) Steering Angle and Distance Error 54

v
 LIST OF TABLES
Table Title Page No.
3.1 Raspberry pi 3 Specifications 15
3.2 Linear Motor Specifications 16
3.3 Savox Servo Motor Specifications 18
4.1 Link Coordinate Parameters 29

vi
 CONTENTS
Page No.

CANDIDATE’S DECLARATION. i

ACKNOWLEDGEMENT. ii

ABSTRACT. iii

LIST OF FIGURES. iv

LIST OF TABLES. vi

CHAPTER 1
1
“INTRODUCTION”

1.1 Background. 1
1.2 Problem Definition. 2
1.3 Motivation. 2
1.4 Goals of the Project. 2
1.5 Project Overview. 2
1.5.1 Project out lines. 3
CHAPTER 2
4
“ROBOTICS TECHNOLOGY”

2.1 Introduction. 4
2.2 Classes of Manipulators. 4
2.3 Robot Configurations. 6
2.4 Robots Locomotion. 7
2.5 Mechanics and Control of Robotics. 8

CHAPTER3
14
“HARDWARE DESIGN”

3.1 Electrical, Electronic and Mechanical Components of the Project. 14

3.2 Mechanical Construction. 22
 CHAPTER 4
28
“MODELING AND SIMULATION”

4.1 The kinematics models of the manipulator. 28

4.2 Mobile Robot Modeling. 31
4.3 SCARA Robotic Arm Simulation. 34
4.4 Mobile Robot Simulation. 36
CHAPTER 5
37
“SOFTWARE DESIGN AND IMPLEMENTATION”
5.1 Introduction. 37
5.2 Project Flow Chart. 38
5.3 SCARA robotic arm software. 40
5.4 The Mobile Robot Code. 47

CHAPTER 6
54
“PRACTICAL RESULTS”

6.1 Introduction. 54
6.2 Testing the manipulator positioning accuracy. 54
6.3 Testing the mobile robot positioning accuracy. 54
CHAPTER 7
56
“CONCLUSION AND FUTURE DEVELOPMENT”

REFRENCES. 57

Appendix A: Robotics Arm Codes. 59

Appendix B: Mobile Robot Codes. 83

 CHAPTER 1
INTRODUCTION

1.1 Background
Robotics can be described as the current pinnacle of technical development. Robotics is a
confluence science using the continuing advancements of mechanical engineering, material science,
sensor fabrication, manufacturing techniques, and advanced algorithms. The study and practice of
robotics will expose a dabbler or professional to hundreds of different avenues of study. For some,
the romanticism of robotics brings forth an almost magical curiosity of the world leading to creation
of amazing machines. A journey of a lifetime awaits in robotics. Robotics can be defined as the
science or study of the technology primarily associated with the design, fabrication, theory, and
application of robots. While other fields contribute the mathematics, the techniques, and the
components, robotics creates the magical end product. The practical applications of robots are to
drive development of robotics and drive advancements in other sciences in turn. Crafters and
researchers in robotics study more than just robotics. The promise of robotics is easy to describe but
hard for the mind to grasp. Robots hold the promise of moving and transforming materials with the
same lean and ease as a computer program transforms data. Today, robots mine minerals, assemble
semi-processed materials into automobile components, and assemble those components into
automobiles. On the immediate horizon are self-driving cars, robotics to handle household chores,
and assemble specialized machines on demand. It is not unreasonable to imagine robots that are
given some task, such as reclaim desert into photovoltaic cells and arable land, and left to make their
own way. Then the promise of robotics exceeds the minds grasp .

Robotics has achieved its greatest success to date in the world of industrial manufacturing. A
fixed manipulator has a limited range of motion that depends on where it is bolted down. In contrast,
a mobile robot would be able to travel throughout the manufacturing plant, flexibly applying its
talents wherever it is most effective. This Project focuses on the technology of mobility: how can a
mobile robot move unsupervised through real-world environments to fulfill its tasks? The first
challenge is locomotion itself. How a mobile robot moves, and what should be, is it about a
particular locomotion mechanism that makes it superior to alternative locomotion mechanisms?
These robots are compelling not for reasons of mobility but because of their autonomy, and so their
ability to maintain a sense of position and to navigate without human intervention is paramount. The

1
dynamics and control theory must be understood beside the mechanism and kinematics for solving
the locomotion problem. [1]

1.2 Problem Definition

It is not easy to make a robotic or a mobile arm which move autonomously. This project
congregates a SCARA manipulator with mobile robot that’s move autonomously. No one in this
country tried to make such this robot of its kind. It will solve many problems in our country. It can
help people with disabilities to bring them things as they need from a nearby place and other
applications that will be discussed later in this report.

1.3 Motivation
Robotics is one of the most interesting since nowadays, there are international competitions
between the greatest countries in the world in producing the highest intelligence robots with high
technologies. Yemen as one country of the third world this kind of completion is not there because of
the limitation of the technology. However, lately with the appearance of mechatronics engineering in
Sana’a University and other universities in the capital of Yemen and other big cities such as Emirates
University and Malaysian university, students started to make a good starting robotics projects.

1.4 Goals of the Project

Every project should have aims and goals to reach so that everyone waiting it be satisfied of the
work, in this project some goals are listed below:

1. To apply mechatronics concepts that, we studied during five years in a real project.
2. Improve the transmission of objects in the industries of our county.
3. To open the gate of robotics competition inside Yemen.

1.5 Project Overview

It is an intelligence mobile manipulator which contains two main parts, the SCARA arm and the
mobile robot. It was built from the beginning by our hand in the workshop. Each link and each part
of the project were made locally, we bought the raw materials like the aluminum and applied the
mechanical operation we studied in the first year in workshop and the third year in manufacturing

2
processing to produce all the links and the mechanical parts such as the shafts, gears, pulley and
bearings holders. The same thing with the mobile robot we produced it by mechanical operation, the
design of the project will be shown in chapter 3. The robot can move easily to the required places
autonomously depending on the application that will be chosen to. Manipulator, mobile robot,
electrical devices such as servo motors, dc motors, some drivers, shields and microcontroller are the
main hardware parts of this project. In general robotics arms are needed to be used in industrial areas
and some other areas. The locomotion characteristic makes the robot’s scope too much larger. The
project is an autonomous mobile manipulator with a large scope which can be used in different areas
of human life. Industrial factories, hospitals restaurants, cafeterias, homes, schools and companies. In
general, it is multi-functional robot depends on its gripper. The project contains a small hand as its
gripper which make its scope clearly identified. Taking small things from nearby places to required
positions is the main job for this robot with high intelligence and autonomous locomotion.

1.5.1 Project out lines:

Chapter 2 Covers a general overview about robotics, mobile robot and applications.
Chapter 3 Describes the project hardware including design and kits used in the project.
Chapter 4 Describes the modeling and simulation for this project in details.
Chapter 5 Describes the software design for this project in details.
Chapter 6 Presents the results obtained for practical test
Chapter 7 Presents the conclusion and future development.

3
 CHAPTER 2
ROBOTICS TECHNOLOGY

2.1 Introduction
The term robot means different things to different people. Science fiction books and movies
have strongly influenced what many people expect a robot to be or what it can do. Sadly, the practice
of robotics is far behind this popular conception. One thing is certain though – robotics will be an
important technology in this century. Products such as vacuum cleaning robots are the vanguard of a
wave of smart machines that will appear in our homes and workplaces. [2]

A robot is formally defined in the International Standard of Organization (ISO) as a reprogrammable,

multifunctional manipulator designed to move material, parts, tools, or specialized devices through
variable programmed motions for the performance of a variety of tasks. [3]

Another formal definitions of robot is: "A robot is a mechanical device with links and joints, guided
by sensors, driven by actuators and controlled through a programmed software, to handle and
manipulate parts, materials, tools and devices for performing various tasks in variety of work
environments". The definition stated above answers the question like-what type of device is the
robot. What are its elements? How is it moved? How is it made to function? What does it do? And
where can it work? [4]

2.2 Classes of Manipulators

There are two broad classes of robots and associated kinematics equations: serial manipulators
and parallel manipulators.

2.2.1 Serial manipulators:

Serial manipulators are the most common industrial robots. They are designed as a series of
links connected by motor-actuated joints that extend from a base to an end-effector. Often, they have
an anthropomorphic arm structure described as having a "shoulder", an "elbow", and a "wrist" such
as shown in figure 2.1(a). Serial robots often have six joints, because it requires at least six degrees
of freedom to place a manipulated object in an arbitrary position and orientation in the workspace of
the robot. A popular application for serial robots in today's industry is the pick-and-place assembly
robot, called a SCARA robot, which has four degrees of freedom. In its most general form, a serial

4
robot consists of a number of rigid links connected with joints. Simplicity considerations in
manufacturing and control have led to robots with only revolute or prismatic joints and orthogonal,
parallel and/or intersecting joint axes (instead of arbitrarily placed joint axes). [5]

Figure 2.1a Serial manipulator Figure 2.1b Parallel manipulator

2.2.2 Parallel manipulators:

A parallel manipulator is a mechanical system that uses several computer-controlled serial
chains to support a single platform, or end-effector. Perhaps, the best known parallel manipulator is
formed from six linear actuators that support a movable base for devices such as flight simulators.
This device is called a Stewart platform or the Gough-Stewart platform in recognition of the
engineers who first designed and used them as shown in figure (2.1 b). [6]

In the Stewart platform, the actuators are paired together on both the basis and the platform.
These systems are articulated robots that use similar mechanisms for the movement of either the
robot on its base, or one or more manipulator arms. Their 'parallel' distinction, as opposed to a serial
manipulator, is that the end effector (or 'hand') of this linkage (or 'arm') is connected to its base by a
number of (usually three or six) separate and independent linkages working in parallel. 'Parallel' is
used here in the computer science sense, rather than the geometrical; these linkages act together, but
it is not implied that they are aligned as parallel lines; here parallel means that the position of the end
point of each linkage is independent of the position of the other linkages. [7]

5
2.3 Robot Configurations
The possible types of movements that a robot can provide define the configuration of a
particular robot. The different configurations of different robots help in positioning of the robot hand
in the defined co-ordinate of the work-envelope. If 'P' represents the prismatic joint and 'R' represents
the revolute joint, then a robot with three prismatic and 2 revolute joints is configured as 3P2R robot.
The different robot configuration are as follows. [8]
2.3.1 SCARA (Selective Compliance Assembly Robot Arm):
This is a specially configured robot which has two horizontal and parallel revolute joints with
the axis vertical and one prismatic joint which can move the arm vertically up and down. This gives
high stiffness due to the vertical axis and dexterity in the horizontal points due to the rotary joints.
This finds use in assembly operations. Figure 2.2(a) shows schematic of the SCARA robots.

2.3.2 Cartesian (3P) Robot:

These types of robots have three degrees of rigid body freedom. They have three prismatic
joint which produces three linear motions in x, y and z directions. With Cartesian robots the accuracy
is the highest and the dexterity is the lowest. The illustration is given in Figure 2.2(b). There are two
types of Cartesian robot which are the Cantilevered robot or the Gantry or Box robot.

2.3.3 Cylindrical (RPP) Robot:

This type of robots has two prismatic joints and one revolute joint. The two prismatic joints
give linear movements about any two axes and the third movement, rotation is produced by the
revolute joint. The sketch of such a robot is given in Figure 2.2(c). Usually rotation of 360⁰ cannot
be achieved. The accuracy decreases and dexterity increases; however, reaching a point lower than
the robot base is not possible.

2.3.4 Polar/Spherical (2RP) Robot:

This type of robots has two revolute joints and one prismatic joint characterize this type of
robot in which there is one linear and two rotary movements produced at the joints. Refer Figure
2.2(d) for the illustration. This type can reach points lower than the base and its accuracy decreases
while going far from the base axis.

2.3.5 Articulated/Anthropomorphic/Jointed arm Robot (3R):

The robots of this type have three revolute joints giving three rotary movements resembling the
human hand Figure 2.2(e). This type has the most dexterity and lowest accuracy.

6
 (a) SCARA (b) Cartesian (c) Cylindrical

(d) Polar/Spherical (e) Articulated

Figure 2.2 Type of Robot by Configuration

2.4 Robots Locomotion

Robots locomotion is the way for robot motivation from position to another. For each way
these are specific algorithms which considered as one of the most difficult problems that people face
while dealing with robots. Figure (2.3) shows the different types of robot locomotion.

Robots used in industry are:

2.4.1 Stationary Robots:

This type of robots is typically an arm-type manipulator on a fixed base that performs
repetitive tasks within a local work cell without changing their positions. Referring the robot as
“stationary” does not mean that the robot actually is not moving. What “stationary” means is the base
of the robot does not move during operation. These kinds of robots generally manipulate their
environment by controlling the position and orientation of an end-effector. Stationary robot category
includes robotic arms, Cartesian robots, cylindrical robots, spherical robots, SCARA robots and
parallel robots. [9]

7
2.4.2 Mobile robot:
An autonomous robot acts as a stand-alone system, complete with its own computer (called the
controller). The most advanced example is the smart robot, which has a built-in artificial intelligence
(AI) system that can learn from its environment and its experience and build on its capabilities based
on that knowledge Mobile robot category includes wheeled robots, legged robots, flying robots,
swimming robots, rolling robotic, swarm robots, modular robots, micro robots, Nano robots and soft
elastic robots. [10]

Figure 2.3 Robot locomotion examples

2.5 Mechanics and Control of Robotics

Our focus in this section will be on the mechanics, planning and control of robot mechanisms.
Basically, a mechanism is constructed by connecting rigid bodies, called links, together with joints,
so that relative motion between adjacent links becomes possible. Actuation of the joints, typically by
electric motors, then causes the robot to move and exert forces in desired ways. [11]

2.5.1 Position and orientation:

With robots, we are constantly concerned with the location of objects in three-dimensional
space. These objects are the links of the manipulator, the parts and tools with which it deals, and
other objects in the manipulator's environment. In order to describe the position and orientation of a
body in space, we will always attach a coordinate system, or frame, rigidly to the object. We then
proceed to describe the position and orientation of this frame with respect to some reference
coordinate system. Any frame can serve as a reference system within which to express the position

8
and orientation of a body, so we often think of transforming or changing the description of these
attributes of a body from one frame to another see figure (2.4). [11]

Figure 2.4 Robot coordinate system or “frame”

2.5.2 Kinematics:
Kinematics is the science of motion that treats motion without regard to the forces which cause
it. Within the science of kinematics, one studies position, velocity, acceleration, and all higher order
derivatives of the position variables (with respect to time or any other variable(s)). Hence, the study
of the kinematics of manipulators refers to all the geometrical and time-based properties of the
motion. The dimensions of the robot and its kinematics equations define the volume of space
reachable by the robot, known as its workspace. This science covers many topics such as forward
kinematics, inverse kinematics, and velocities.

▪ Forward Kinematics:
Forward kinematics specifies the joint parameters and computes the configuration of the chain.
For serial manipulators this is achieved by direct substitution of the joint parameters into the forward
kinematics equations for the serial chain. For parallel manipulators substitution of the joint
parameters into the kinematics equations requires solution of a set of polynomial constraints to
determine the set of possible end-effector locations. [12]

▪ Inverse Kinematics:

Inverse kinematics specifies the end-effector location and computes the associated joint angles.
For serial manipulators this requires solution of a set of polynomials obtained from the kinematics
equations and yields multiple configurations for the chain. The case of a general 6R serial

9
manipulator (a serial chain with six revolute joints) yields sixteen different inverse kinematics
solutions, which are solutions of a sixteenth-degree polynomial. For parallel manipulators, the
specification of the end-effector location simplifies the kinematics equations, which yields formulas
for the joint parameters. [13]

▪ Velocities, static forces, singularities:

To deal with static positioning problems, we may wish to analyze manipulators in motion.
Often, in performing velocity analysis of a mechanism, it is convenient to define a matrix quantity
called the Jacobian of the manipulator. The Jacobian specifies a mapping from velocities in joint
space to velocities in Cartesian space. The nature of this mapping changes as the configuration of
the manipulator varies. At certain points, called singularities, this mapping is not invertible. An
understanding of the phenomenon is important to designers and users of manipulators. [11]

2.5.3 Dynamics:
Dynamics is the study of motion taking into account the forces and torques that cause it.
Analogous to the notions of a robot’s forward and inverse kinematics, the forward dynamics problem
seeks to determine the resulting joint trajectory for a given input joint torque profile. The inverse
dynamics problem is concerned with determining the input joint torque profile for a desired joint
trajectory. The dynamic equations relating the forces and torques to the motion of the robot’s links
are given by a set of second-order ordinary differential equations. [14]

2.5.4 Trajectory generation:

A common way of causing a manipulator to move from here to there in a smooth, controlled
fashion is to cause each joint to move as specified by a smooth function of time. Commonly, each
joint start and ends its motion at the same time, so that the manipulator motion appears coordinated.
Exactly how to compute these motion functions is the problem of trajectory generation. [16]

2.5.5 Motion planning:

Motion planning is the problem of finding a robot motion from a start state to a goal state while
avoiding obstacles in the environment and satisfying other constraints, such as joint limits or torque
limits. Motion planning addresses the problem of finding a collision-free motion for a robot through
a cluttered workspace and it is one of the most active subfields of robotics. Motion planning can be
subcategorized into Path Planning and Path Smoothing. [14]

10
 ▪ Path Planning:
Path planning problem is a sub-problem of the general motion planning problem, and seeks to
determine a collision-free path between a start and goal configuration. Path planning methodologies
are often computationally expensive and require a relatively accurate map of the environment in
order to determine the optimal path. The frequency of re-planning is dependent on the efficiency of
the algorithm. Some popular techniques to accomplish path planning include the A* Search
algorithm, the Wavefront algorithm and having the user manually input a desired path. [15]

1. A* Search Algorithm: The A* algorithm is a best first tree search algorithm which uses
a combination of the path cost and a heuristic function to determine the order in which
it visits the tree nodes. The path cost is associated with the cost moving from one
position to another and the heuristic function provides an estimate of the desirability of
visiting a given node. This algorithm guarantees to find the optimal path to the goal
position if one exists.
2. Wavefront: The wave front algorithm (also referred to as Distance Transform Path
Planning) is unique in the way that it determines the optimal path by traversing
backwards from the goal position towards the robot start position. This method is
guaranteed to offer an optimal path provided that the given environment map is
accurate.
3. User-defined: In terms of determining an acceptable path; having the end user manually
determine the path is perhaps the simplest methodology. However, in terms of
implementation complexity it is not the best solution because it requires a relatively
complex Graphic User Interface (GUI) to be built in order to obtain the desired path
from the user. Furthermore, manually inputted paths are not guaranteed to be optimal
even though they may be acceptable to the end user; usually such solutions are offered
as a secondary option.

▪ Path Smoothing:

In order to avoid wheel slippage or mechanical damage during the mobile robot navigation, it
is necessary to smoothly change driving velocity or direction of the mobile robot. This means that
dynamic constraints of the mobile robot should be considered in the design of path smoothing
algorithm.

11
2.5.6 Robot Control:
Robotic control can be seen as a mixture of engineering and cognitive science and as such it
presents unusual challenges to the programmer. Robotic control methodologies have tended to move
from simplistic, predefined actuator actions based on specific input criteria to tight feedback loops
with input from the environment, giving more robust solutions.

Robots are classified by control method into servo and non-servo robots. The earliest robots were
non-servo robots. These robots are essentially open-loop devices whose movement is limited to
predetermined mechanical stops, and they are useful primarily for materials transfer. Servo robots
use closed-loop computer control to determine their motion and are thus capable of being truly
multifunctional, reprogrammable devices. [16]. A mostly used closed loop control in robotics is the
PID controller.
- PID Controller:
In Control Theory, a PID controller is a feedback based control loop used in industrial systems.
It is used in more than half the industrial controllers in use nowadays, which has led to the
development of a variety of systems to adjust the tuning of the control parameters. One main
advantage of this type of controllers is that they can be applied in many different systems, which
makes them very versatile, especially when a mathematical model of the system to be controlled
cannot be known.

𝑡
𝑑
𝑢(𝑡) = 𝐾𝑝 𝑒(𝑡) + 𝑇𝑖 (∫ 𝑒(𝑡) 𝑑𝑡) + 𝑇𝑑 ( 𝑒(𝑡)) … … . . (1)
0 𝑑𝑡

Figure 2.5 Block diagram of PID-controlled system

A PID controller works by calculating the error between the output of the plant and a given set point.
With this error, an output is calculated based on equation (1). This output is then given as input to the
controlled system as shown in Figure (2.5). Equation (1) can be divided in three parts, which are the
three actions of the PID controller:

12
• Proportional: the actual error is multiplied by a gain of Kp, which means that the bigger the
error, the bigger the output. This action defines the response speed of the controller: ideally,
this constant should be as large as possible in order to get a perfect response. However,
increasing the gain too much can cause oscillation and overshooting in output values.
• Integral: the error is integrated over time and multiplied by Ti, making it possible to correct
the constant error that the proportional part can have. The integral action allows to eliminate
the steady-state error, as it will increase its output over time when a constant deviation
happens. In digital controllers, the integration is often approximated as the sum of errors in
each time step. This approach can cause the errors introduced by sensors or noise to
accumulate, which can distort the output if the gain is set too high.
• Derivative: the error is derived with respect to time in order to get the error change rate, and
is then multiplied by Td. This way, the controller can predict future error based on the way
the actual error is changing. The derivative action is sensitive to noise, which could cause
spikes in the output if not filtered correctly.

13
 CHAPTER 3
HARDWARE DESIGN

3.1 Electrical, Electronic and Mechanical Components of the Project

In the following subsection the project components are detailed.

3.1.1 Raspberry pi 3:
The controller of the robot is a single board computer Raspberry Pi 3. Figure (3.1) shows
Raspberry Pi 3 B architecture with GPIO connectors. CPU performance can be compared to a
Pentium II 300 MHz processor. It has a variety of interfacing peripherals, including Wi-Fi, HDMI
port, USB port, SD card storage and 40 pin GPIO port for expansion, table (3.1) shows Raspberry pi
3 specifications. Monitor, keyboard, and mouse can be connected to the Raspberry Pi 3 through
HDMI and USB connectors and it can be used like a desktop computer. It supports many of
operating systems like a Debian based Linux distort, Raspbian which is used in this project.
Raspberry Pi 3 can be connected to a local area network by using an Ethernet cable, and then it can
be accessed through remote login. Raspberry bi is a small computer that is used for control the input
(as sensors) and the output (as servo and dc motor) of robot. [17]

Figure (3.1) Raspberry pi 3 model B

14
 Table (3.1) Raspberry pi 3 model B specifications

Characteristic Raspberry Pi 3
CPU Quad cortex A53 @ 1.2GHz
GPU 400MHz Video Core IV
RAM 1GB SDRAM
Storage Micro SD
Ethernet Port Yes
Wireless 802.11n/Bluetooth 4.0
GPIO 40 pins

3.1.2 Servo Driver:

Figure (3.2) shows Servo driver with 16 channels used in our project.

It has the following specifications: [18]

▪ It's an i2c-controlled PWM driver with a built-in clock. Which means we can control up
to 16 servos using only 2 pins.
▪ It is 5V compliant, which means you can control it from a 3.3V microcontroller and
still safely drive up to 6V outputs.
▪ 6addresses select pins so you can wire up to 62 of these on a single i2c bus, a total of
992 outputs - that's a lot of servos.
▪ Adjustable frequency PWM up to about 1.6 KHz.
▪ 12-bit resolution for each output - for servos, that means about 4us resolution at 60Hz
update rate
▪ Output enable pin to quickly disable all the outputs.

Figure (3.2) Servo driver

15
3.1.3 Dc linear motor:
Linear motors are electromagnetic direct drives in tubular form as shown in Figure (3.3). The
linear motion is produced purely electrically and wear-free, without any intermediate coupling of
mechanical gearboxes, spindles, or belts. The linear motor consists of just two parts: the slider and
the stator. Table (3.2) shows its specifications. Linear motor is used to move the gripper of the arm
vertically.

Figure (3.3) Linear motor

Table (3.2) Liner motor specifications

Stroke size 12in

Force 5 lbs.
Speed 0.59"/sec

Voltage 12 VDC

3.1.4 Robot gripper:

A gripper is a device which enables the holding of an object to be manipulated. The easier
way to describe a gripper is to think of the human hand. Just like a hand, a gripper enables holding,
tightening, handling and releasing of an object. A gripper is just one component of an automated
system. A gripper can be attached to a robot or it can be part of a fixed automation system. Many
styles and sizes of grippers exist so that the correct model can be selected for the application [19].
Figure (3.4) shows the robot gripper of the project, which can hold up to 0.6 Kg.

Figure (3.4) Robot gripper

16
3.1.5 ADC 16-bit:
Raspberry pi doesn't have analog-to-digital converter, so we used the ADS1115 to provide 16-
bit precision at 860 samples/second over I2C. The chip can be configured as 4 single-ended input
channels, or two differential channels. As a nice bonus, it even includes a programmable gain
amplifier, up to x16, to help boost up smaller single/differential signals to the full range. We like this
ADC because it can run from 2V to 5V power/logic, can measure a large range of signals and its
super easy to use. It is a great general purpose 16-bit converter [20]. The analog to digital converter
is shown in Figure (3.5).

Figure (3.5) ADS 115 (analog-to-digital converter)

3.1.6 Servo Motor:

Servos are controlled by sending an electrical pulse of variable width, or pulse width
modulation (PWM), through the control wire. There is a minimum pulse, a maximum pulse, and a
repetition rate. A servo motor can usually only turn 90° in either direction for a total of 180°. The
motor's neutral position is defined as the position where the servo has the same amount of potential
rotation in the both the clockwise or counter-clockwise direction. The PWM sent to
the motor determines position of the shaft, and based on the duration of the pulse sent via the control
wire; the rotor will turn to the desired position. The servo motor expects to see a pulse every 20 ms
and the length of the pulse will determine how far the motor turns. [21]. Figure (3.6) shows the servo
motor that we used in the project, three servos are used in robot; two are used for the links rotation
and the other one is used for steering of the mobile robot. The specifications of the servo motor are
shown in Table (3.3).

17
 Figure (3.6) SAVOX servo motor

Table (3.3) SAVOX servo motor specifications

SAVOX SA-1230 SG specifications

Torque (at 4.8v) 30kg-cm

Torque (at 6v) 36kg-cm

Speed 0.2 sec/60 dig

Gear Steel

Type Digital Servo

3.1.7 DC-Motor drivers:

H-Bridge Motor Driver used to drive the DC-motors with variable speed and to change the
direction of motion. It is powerful enough to drive motors from 5-35V at up to 2A peak, the l298n
motor driver is shown in Figure (3.7). Two Dc-Motor Drivers are used in our robot; one to drive the
linear motor and the gripper motor, and the other one is used to drive the rear motors of the car.

Figure (3.7) L298n motor driver

18
3.1.8 Servo to shaft Coupler:
A Servo to Shaft Coupler shown on Figure (3.8), it offers a simple and solid way to attach a
shaft in line with the output spline of a servo. Once installed onto the servo the coupler is then
fastened to the servo using a servo screw. The shaft can then be slid into place and clamped using the
supplied socket head screw. [22]

Figure (3.8) Servo to shaft coupler

3.1.9 IR Sharp Sensor:

The Sharp GP2Y0A21 IR Infrared Distance Sensor that shown in Figure (3.9) is an analog
proximity sensor with a range of 10-80cm.This sensor provides accurate distance measurements
within the 10-80cm range. Infrared proximity sensors work by detecting the reflection of an invisible
infrared light. The GP2Y0A21 contains both an IR emitter LED and an IR detector in the same
package along with some control circuitry. It has three pins for connecting to the project: power,
ground and an analog voltage output. [23]

Three IR sharp sensor IR Sensor are used in our project for the car in avoiding obstacles while the
mobile robot is moving. They are connected to the analog-to-digital converter then the ADC send the
signal to the raspberry pi via I2C.

Figure (3.9) Sharp GP2Y0A21 IR Infrared Distance Sensor

19
3.1.10 Pulley:
We used two Toothed-pulleys for the second link as shown in Figure (3.10). The Pulley is
used to transmit the motion from the servo motor to the second link by the belt which is connected to
the pulley of the servo shaft in one end and the other end is connected to the pulley of the shaft that
fixed to the second link.

Figure (3.10) Toothed-pulley

3.1.11 Rear motors:

There are two rear motors 12volt DC moving at the same time to give the cart higher torque.

They are fixed in the rear tires of the car. It is shown in Figure (3.11).

Figure (3.11) rear motor of the car

3.1.12 Ultrasonic sensor:

HC-SR04 is an ultrasonic ranging module that provides 2-cm to 400-cm non-contact
measurement function. The ranging accuracy can reach to 3mm and effectual angle is < 15°. It can
be powered from a 5V power supply. It’s shown in figure (3.12). Ultrasonic sensor is used in our
projects as a feedback for the linear motor.

Figure (3.12) HC-SR04 ultrasonic

20
 HC-SR04 Ultrasonic Specifications are as follows: [24]

▪ Working Voltage: DC 5V.

▪ Working Current: 15-mA.
▪ Working Frequency: 40Hz.
▪ Max Range: 4-m.
▪ Min Range: 2-cm.
▪ Measuring Angle: 15 degrees.
▪ Trigger Input Signal: 10µS TTL pulse.
▪ Echo Output Signal Input TTL lever signal and the range in proportion.
▪ Dimension 45 * 20 * 15-mm.

3.1.13 Power supply:

In the project we used two batteries as a power supply for the project:

1. 12V battery used to supply power to the DC-motors (linear motor, gripper, and the rear
motors).
2. 6V battery used to supply power to the three servo motors.
3. 5V power bank used to supply power to the raspberry pi.

3.1.14 Wi-Fi Access (Router):

The commands are sent to the raspberry pi via Wi-Fi access point router shown in figure
(3.13). So, the connection between the raspberry and the computer is wirelessly.

Figure (3.13) Router

21
3.2 Mechanical Construction
3.2.1 Shafts and bearings:
We start with the shafts and bearings that will connect the links of the arm with each other. The
dimensions of the shafts are 12-cm long and 2.5-cm diameter, see Figure (3.14c). Two shafts are
used; one to connect the first link with the base and the other is connected between first links and
second, the shaft is shown in Figure (3.14a).

Four bearings are used, two for the first link and two for the second. The bearing is shown in Figure
(3.14b). The material of the shafts and bearings is aluminum.

Figure (3.14a) Shaft Figure (3.14b) Bearing

Figure (3.14c) Shaft modeling

3.2.2. Links of the arm:

The links of the arm are made from aluminum with dimensions 28-cm length for the first link
and 22-cm for the next link as shown in Figure (3.15a). The width of the two links is 6.5cm and the
height is 5.5cm. Figure (3.15b) shows the two links.

22
 Figure (3.15a) Link dimension

Figure (3.15b) Links of the arm

3.2.3. Shaft to link coupler:

The shaft to link coupler is pinned down at the ends of link for the shaft to the inserted in. Its
dimension is 5-cm height and 2.5-cm diameter. The shaft to link coupler is shown in Figure (3.16).

Figure (3.16) Link to shaft coupler

3.2.4. Movement mechanism:

For the first link, the movement mechanism consists of two gears that transform the motion
from the servo to the first link by the shaft attached to it. The first gear is fixed to the shaft of servo
motor and has hole of 10mm as shown in Figure (3.17). The second is fixed to the shaft and inserted
to the pulley and has a hole of 25-mm.

The movement of the second link is by using toothed pulleys and belt to transform the motion from
the servo to the second link. The movement mechanisms are shown in Figure (3.18).

23
 Figure (3.17) Gear mechanism of the first link

Figure (3.18) Movement mechanism of the arm

3.2.5. The base of the arm

It is used for mounting the links of the arm on it. The base of the arm is shown in Figure (3.19).

Figure (3.19) The base of the arm

3.2.6. Gripper connection:

Figure (3.20) shows how we connected the gripper to the Linear motor.

Figure (3.20) The gripper

24
3.2.7. Steering mechanism of the cart:
The steering mechanism is shown in Figure (3.21). The maximum steering angle of the mobile
robot is from (+30 to -30 degrees) controlled by a servo motor.

Figure (3.21) Steering mechanism

3.2.8. The platform of the robot:

The platform of the mobile robot is used to mount the robotic arm. We designed an adjustable
highest table adjusted by bolts, see Figure (3.22).

Figure (3.22) Adjustable table top view and side view

3.2.9. The robotic arm:

The configuration of the arm is of type SCARA (Selective-Compliance Assembly Robot Arm),
Figure (3.23) shows the robotic arm after the assembly process. This configuration was proposed as a
means to provide motion capabilities to the end-effector that are required by the assembly of printed-
board circuits and other electronic devices with a flat geometry. These robots have received special
attention because of their special structure, offering an extremely high stiffness. [21]

25
 Figure (3.23) SCARA robotic arm after the assembly process

3.2.10. The car of the project:

The mechanical design must be in balance with the function of the robot either the manipulator
or the mobile robot. So, we decided to make a car with four wheels to insure stable movement as
shown in Figure (3.24), the inner constructions of the car and its dimensions are shown Figure (3.25).

Figure (3.24) The car of the mobile robot

Figure (3.25) Inner construction of the car

26
3.2.11. The final project:
After assembling the robotic arm and the cart of the mobile robot, we reach our final step by
putting the arm on the straight platform. That makes the final project accurately balanced as shown
in Figure (3.26).

Figure (3.26) The final project

27
 CHAPTER 4
MODELING AND SIMULATION

In this chapter, the models of the robot are derived and the simulation process is illustrated.

4.1 The kinematics models of the manipulator

Kinematics is the science of motion that treats the subject without regard to the forces that
cause it.

4.1.1 Forward kinematics:

The Modified Denavit–Hartenberg parameters are the four parameters associated with a
particular convention for attaching reference frames to the links of a spatial kinematic chain, or robot
manipulator. [11] Frames are assigned as shown in the Figure (4.1), The D-H parameters are
tabulated in Table (4.1).

Figure (4.1) SCARA robot coordinates assignment

28
 Table (4.1) Link coordinate parameters

Link i 𝛼𝑖−1 𝑎𝑖−1 𝑑𝑖 𝜃𝑖

1 0 0 0 𝜃1
2 0 0.28 0.05 𝜃2
3 𝜋 0.22 𝑑3 + 0.05 0

4.1.2 Inverse Kinematics:

In this section we consider the problem of computing the set of joint angles when given the
desired position and orientation of the end effector relative to the base.

Since

cos(q1 + q2 ) sin(q1 + q2 ) 0 a2 ∗ cos(q1 + q2) + a1 ∗ cos(q1)

sin(q1 + q2 ) −cos(q1 + q2) 0 a2 ∗ sin(q1 + q2) + a1 ∗ sin(q1)
0T3=[ ]
0 0 −1 d2 − d3 − d4
0 0 0 1

if

cos(ϕ) sin(ϕ) 0 X
sin(ϕ) −cos(ϕ) 0 Y
0T3 =[ ]
0 0 −1 Z
0 0 0 1

Then

Z = d2 − d3 – d4

So

d3 = d2 − d4 − Z ..................................... (1)

X = a2 ∗ cos(q1 + q2) + a1 ∗ cos(q1)

Y = a2 ∗ sin(q1 + q2) + a1 ∗ sin(q1)

X2 + Y2 = α12 + α22 + 2 * α1 * α2 * cos(q2)

29
 𝑋 2 + 𝑌 2 −𝑎1 2 − 𝑎2 2
Cos(q2) =
2∗𝑎1 ∗𝑎2

Sine(q2) = ±√1 − cos(q2)2

q2 = atan2(sin(q2), cos(q2)) ……………………………………………….…… (2)

X = k1*cos(q1) - k2*sin(q1),

Y = k1*sin(q1) + k2*cos(q1),

Where

k1 = α1 + α2 *cos(q2),

k2 = α2 * sin(q2).

If

r = +√𝑘1 2 + 𝑘2 2

and

γ = atan2(k2, k1),

then

k1 = r * cos(γ),

k2 = r * sin(γ).

𝑋
= cos(𝛾) ∗ cos(𝑞1) − sin(𝛾) ∗ sin(𝑞1)
𝑟
𝑌
= cos(𝛾) ∗ sin(𝑞1) + sin(𝛾) ∗ cos(𝑞1)
𝑟

So

𝑋
cos(γ+q1) = 𝑟

𝑌
sin(γ + q1) = 𝑟

γ + q1 = atan2(y, x)

30
 and so

q1 = atan2(y, x) – atan2(k2, k1) …………………………………..… (3)

4.2 Mobile Robot Modeling

4.2.1 Grid-Based Search:
Grid-based approaches overlay a grid on configuration space, and assume each configuration is
identified with a grid point as shown in Figure (4.2). At each grid point, the robot is allowed to move
to adjacent grid points as long as the line between them is completely contained within C free (this is
tested with collision detection). This discretizes the set of actions, and search algorithms (A*) is used
to find a path from the start to the goal. [2]

Figure (4.2) Grid-Based search

4.2.2 Search algorithm for motion planning:

A basic motion planning problem is to produce a continuous motion that connects a start
configuration S and a goal configuration G, while avoiding collision with known obstacles. The
algorithm that we used in the project for motion planning is A* search which is an informed search
algorithm, or a best-first search, meaning that it solves problems by searching among all possible
paths to the solution (goal) for the one that incurs the smallest cost (least distance travelled, shortest
time, etc.), and among these paths it first considers the ones that appear to lead most quickly to the
solution. Figure (4.3) shows a path from initial position to the goal position by the use of A* search.
[25]

31
 Figure (4.3) A* search algorithm

4.2.3 Path Smoothing:

The path shown in Figure (4.4a) has many disadvantages. You don’t want the robot to go
straight, take a 90-degree turn, and then go straight again. A car cannot even make a 90-degree turn,
and this route will force the robot to move really slowly around the corners. A much better path
would look like the red curve in Figure (4.4b). [25]

Figure (4.4a) Original path generated from A* search Figure (4.4b) Smoothed path

4.2.4 PID control

A commonly used control scheme for a four-wheeled car-like vehicle is the PID control of a
bicycle model shown in Figure (4.5). The bicycle has a rear wheel fixed to the body and the plane of
the front wheel rotates about the vertical axis to steer the vehicle. [2]

32
 Figure (4.5) Bicycle model of the car

We might wish to follow a path that is defined as some locus on the xy-plane. The path come
from a sequence of coordinates generated by a motion planner, such as discussed above.

The robot maintains a distance d* behind the path and we formulate an error:

𝑒 = √(𝑥 ∗ − 𝑥)2 + (𝑦 ∗ − 𝑦)2 − 𝑑 ∗ ……………………….… (4)

That we regulate to zero by controlling the robot’s velocity using a proportional integral (PI)
controller. The integral term is required to provide a finite velocity demand v* when the following
error is zero.

𝑣 ∗ = 𝑘𝑣 ∗ 𝑒 + 𝑘𝑖 ∗ ∫ 𝑒 𝑑𝑡…………………………………….(5)

The second controller steers the robot toward the target which is at the relative angle

𝑦 ∗ −𝑦
𝛩 ∗ = 𝑡𝑎𝑛−1 𝑥 ∗−𝑥 ……………………………………………....(6)

and a simple proportional controller turns the steering wheel so as to drive the robot toward the
target.

ɑ = 𝑘ℎ (𝛩∗ − 𝛩), 𝑘ℎ > 0…………………………………….(7)

33
4.3 SCARA Robotic Arm Simulation
A software developed in MATLAB is equipped with a GUI for simulating the performance of
SCARA manipulator motion. The interface panel includes entries for different analyses, such as
forward kinematics, inverse kinematics, path construction by teach through and joints/Cartesian
space trajectories. Figure (4.6) below shows a screenshot for the developed software GUI.

Figure (4.6) GUI for SCARA simulation software.

The developed software can a coronate any 6 DOF or less serial manipulator. It has the
following capabilities:

1. Plotting the robot from D-H table inputs as shown in the Figure (4.6) above.
2. Calculating the inverse kinematics as shown in Figure (4.7).

(a) (b) (c)

Figure (4.7) (a) Inverse Kinematic GUI program (b) right solution (c) left solution.

34
3. Comparing joints torques between right and left solutions, Figure (4.8) shows a torque
comparison between the joints of the robot when it moves through a predefined trajectory.

Figure (4.8) Torque comparison.

4. Generating Trajectory in joint space and Cartesian space, Figure (4.9) shows the generated
trajectory. The user just enters the initial and final position of the robot and the trajectory will
be generated automatically.

Joint Space Trajectory

Cartesian Space Trajectory

Figure (4.9) generated trajectory in the simulation.

5. Animating the robot motion along the generated trajectory.

6. Calculating the joints torque when adding a payload.
7. Teaching the robot to move between customized points.

The simulation analysis for SCARA manipulator using the developed software in MATLAB
could give a clear picture of the arm characteristics with respect to the proposed application.

35
4.4 Mobile Robot Simulation
The mobile robot simulation is implemented using Anaconda software. Anaconda is a
freemium open source distribution of the Python and R programming languages for large-scale data
processing, predictive analytics, and scientific computing, that aims to simplify package management
and deployment. Package versions are managed by the package management system conda. [26]

The simulation of the mobile car is shown in Figure (4.10). The red circles are the obstacles, the
black curve is the original path generated by A* search, and the blue curve is the mobile robot
motion trial to follow the original curve. The performance of the mobile robot is enhanced by
changing the values of the PID parameters of its control scheme.

Figure (4.10) Mobile robot simulation

The simulation showed the correction of the proposed A*search algorithm to move the mobile
robot from given points to a destination which obstacles are avoided. It also illustrated the effect of
changing the PID parameters on the path following task and helped in choosing the most appropriate
gain values.

36
 CHAPTER 5
SOFTWARE DESIGN AND IMPLEMENTATION

5.1 Introduction
The main controller of the project is raspberry pi 3 model B. It is a general-purpose computer,
usually with a Linux operating system as shown in Figure (5.1), with the ability to run multiple
programs. It is more complicated to use than an Arduino. It can run a full Linux based operating
system and has hardware support for SPI, I2C and UART.

Figure (5.1) Raspberry pi Desktop,

Many programming languages can be used, including Python, C/C++, Java, Ruby and more.
This allows to implement a powerful control for systems, such as the autonomous mobile robots,
while keeping low-level access to the GPIO pins, used to read digital information from sensors, or to
control motors.

The programming language used in the project is Python. It has the following advantages:

▪ Readability: Python's syntax is very clear, so it is easy to understand program code.

▪ High-Level Language.
▪ Object oriented programming.
▪ Cross-platform: Python runs on all major operating systems like Microsoft Windows, Linux,
and Mac OS.

37
 ▪ Artificial intelligence: Python is widely used for artificial intelligence, with packages for a
number of applications including General AI, Machine Learning, Natural Language
Processing and Neural Networks.

5.2 Project Flow Chart

The program starts by entering the goal position to the remote terminal. Based on the current position
of the mobile robot, the goal position, and the obstacles in between, the path is generated using
A*search algorithm implemented in the Raspberry controller.

The algorithm also applies the required smoothen to the generated path. Having the smoothed path
generated, the robot starts moving along the path. At each grid point the controller checks if the goal
point is reached, otherwise the motion is continued.

If the mobile robot has reached its goal points, the controller starts moving the arm motors to move
and grasp or release the object based on the inverse kinematics algorithm and trajectory estimated.

If the mobile robot is programmed to another points, the cycle is repeated, otherwise, the motion is
ended and the program stops.

While the mobile robot is in motion, if it receives a signal the attached sensors about the proximity of
an object, the motion stops and wouldn’t be continued unless the path is clear. A flowchart summary
for the project is shown in Figure (5.2).

38
Figure (5.2) Flow chart summary

39
5.3 SCARA robotic arm software
The software that runs the motion of the SCARA manipulator is briefly as follows:
1. The user inters the position of the object in [x, y, z], then the joints angles are
calculated using the inverse kinematic equations (explained in chapter 4). The inverse
kinematic code is shown below:

def Inv_K(self , X, Y, Z, Config = 'R', angle = 'd'):

if sqrt(X**2 + Y**2) > 50 or sqrt(X**2 + Y**2) < 31.2 or Z > (d2-d4) or Z < -30:

print("Values exceds min/max reachable points ! ")

return

else:

#Calculation of Theta(2)

Cos_q2 = ((X**2)+(Y**2)-(a1**2)-(a2**2))/(2*a1*a2)

if(sqrt((X**2)+(Y**2)) >= 50):

Sin_q2a = 0

Sin_q2b = 0

else:

Sin_q2a = sqrt(1-(Cos_q2**2))

Sin_q2b = -sqrt(1-(Cos_q2**2))

Theta2_a = atan2(Sin_q2a,Cos_q2)

Theta2_b = atan2(Sin_q2b,Cos_q2)

#Calculation of Theta(1)

K1 = a1+(a2*Cos_q2)

K2_a = a2*Sin_q2a

K2_b = a2*Sin_q2b

40
 Theta1_a = atan2(Y, X)-atan2(K2_a, K1)

Theta1_b = atan2(Y, X)-atan2(K2_b, K1)

#Calculation of d3

d3 = d2 - d4 - Z

#----------------------

if angle == 'r':

if (Config=='R'):

T = [Theta1_a + (pi/2), Theta2_a + (pi/2), d3]

b = (T[0] - 0) * (0 - pi) / (pi - 0) + pi

T[0] = b

elif (Config=='L'):

T = [Theta1_b + (pi/2), Theta2_b + (pi/2), d3]

b = (T[0] - 0) * (0 - pi) / (pi - 0) + pi

T[0] = b

elif angle == 'd':

if (Config=='R'):

T = [Theta1_a*deg+(90), Theta2_a*deg+(90), d3]

b = (T[0] - 0) * (0 - 180) / (180 - 0) + 180

T[0] = b

elif (Config=='L'):

T = [Theta1_b*deg+(90), Theta2_b*deg+(90), d3]

b = (T[0] - 0) * (0 - 180) / (180 - 0) + 180

T[0] = b

return T

41
2. A trajectory is generated from initial to final position. The trajectory code is shown
below; the user can choose the type of trajectory (cubic or quadratic):

def moveRobot(self, space, x_q1, y_q2, z_d3, t, traj, config = 'R'):

# Check the type of space (cartisian or Joints)

tf = max(t)

if space == 'cart':

x, y, z = x_q1, y_q2, z_d3

q0 = [self.q1 * rad, self.q2 * rad, self.d3]

qf_deg = self.Inv_K(x, y, z, config)

qf = self.Inv_K(x, y, z, config, 'r')

elif space == 'joint':

T = self.forwardK(x_q1, y_q2, z_d3)

x, y, z = T[0], T[1], T[2]

q0 = [self.q1 * rad, self.q2 * rad, self.d3]

qf_deg = [x_q1, y_q2, z_d3]

qf = [x_q1 * rad, y_q2 * rad, z_d3]

# Check the type of Trajectory (quantic or quibic)

# Quadratic

if traj == 5:

##### Joint (1) #####

a0_1 = q0[0]

a3_1 = (10*(qf[0]-q0[0]))/(tf**3)

a4_1 = (-15*(qf[0]-q0[0]))/(tf**4)

a5_1 = (6*(qf[0]-q0[0]))/(tf**5)

42
q1 = a0_1 + (a3_1*(t**3)) + (a4_1*(t**4)) + (a5_1*(t**5))

q1 = q1*deg

v1 = (3*a3_1*(t**2)) + (4*a4_1*(t**3)) + (5*a5_1*(t**4))

a1 = (6*a3_1*t) + (12*a4_1*(t**2)) + (20*a5_1*(t**3))

##### Joint (2) #####

a0_2 = q0[1];

a3_2 = (10*(qf[1]-q0[1]))/(tf**3)

a4_2 = (-15*(qf[1]-q0[1]))/(tf**4)

a5_2 = (6*(qf[1]-q0[1]))/(tf**5)

q2 = a0_2 + (a3_2*(t**3)) + (a4_2*(t**4)) + (a5_2*(t**5))

q2 = q2*deg

v2 = (3*a3_2*(t**2)) + (4*a4_2*(t**3)) + (5*a5_2*(t**4))

a2 = (6*a3_2*t) + (12*a4_2*(t**2)) + (20*a5_2*(t**3))

##### Joint (3) #####

a0_3 = q0[2];

a3_3 = (10*(qf[2]-q0[2]))/(tf**3)

a4_3 = (-15*(qf[2]-q0[2]))/(tf**4)

a5_3 = (6*(qf[2]-q0[2]))/(tf**5)

#convert the z axis reading to ultrasonic reading

43
qf[2] = myMap(qf[2], 30, 0, 0, 30)

q3 = (len(q2)) * [qf[2]]

q = np.array([q1, q2, q3])

q = q.transpose()

q = np.around(q, decimals = 1)

v = np.array([v1, v2])

v = v.transpose()

v = np.around(v, decimals = 4)

a = np.array([a1, a2])

a = a.transpose()

a = np.around(a, decimals = 4)

# Cuibic

elif traj == 3:

##### Joint (1) #####

a0_1 = q0[0]

a2_1 = (qf[0]-q0[0])*(3/(tf*tf))

a3_1 = -(qf[0]-q0[0])*(2/(tf*tf*tf))

q1 = a0_1 + (a2_1*(t**2)) + (a3_1*(t**3))

44
 q1 = q1 * deg

v1 = (2*a2_1*t) + (3*a3_1*t**2)

a1 = (2*a2_1) + (6*a3_1*t)

##### Joint (2) #####

a0_2 = q0[1]

a2_2 = (qf[1]-q0[1])*(3/(tf*tf));

a3_2 = -(qf[1]-q0[1])*(2/(tf*tf*tf));

q2 = a0_2 + (a2_2*(t**2)) + (a3_2*(t**3))

q2 = q2*deg

v2 = (2*a2_2*t) + (3*a3_2*t**2)

a2 = (2*a2_2) + (6*a3_2*t)

##### Joint (3) #####

a0_3 = q0[2]

a2_3 = (qf[2]-q0[2])*(3/(tf*tf));

a3_3 = -(qf[2]-q0[2])*(2/(tf*tf*tf));

#convert the z axis reading to ultrasonic reading

qf[2] = myMap(qf[2], 30, 0, 0, 30)

q3 = (len(q2)) * [qf[2]]

q = np.array([q1, q2, q3])

45
 q = q.transpose()

q = np.around(q, decimals = 1)

v = np.array([v1, v2])

v = v.transpose()

v = np.around(v, decimals = 4)

a = np.array([a1, a2])

a = a.transpose()

a = np.around(a, decimals = 4)

# Update the current pos and joint angle

self.joints_update(qf_deg[0], qf_deg[1], qf_deg[2])

self.coord_update(x, y, z)

return (q, v, a)

The full code of the robotic arm is given in appendix A.

46
5.4 The Mobile Robot Code
The code that runs the mobile robot motion based on the A* search is explained as follows:
1. The workspace is divided into grids using grid-based algorithm as shown in Figure
(5.3a). The user inters a matrix of the workspace with zeros and ones, one means there
is obstacle in that grid as shown in Figure (5.3b) or zero means there is no obstacle.

Figure (5.3a) Girds of the workspace Figure (5.3b) Grids matrix example

2. The path from initial to final position is generated using the technic of Artificial
intelligence (A* Search). The path planning code is shown below:

#class plan: used for finding the best and cheap path from start to goal
#By the use of A star Search (Artificail Inteligence)
class plan:
def __init__(self, grid, init, goal, cost = 1):
self.cost = cost
self.grid = grid
self.init = init
self.goal = goal
self.make_heuristic(grid, goal, self.cost)
self.path = []
self.spath = []

# --------
# make heuristic function for a grid

def make_heuristic(self, grid, goal, cost):

self.heuristic = [[0 for row in range(len(grid[0]))]
for col in range(len(grid))]
for i in range(len(self.grid)):
for j in range(len(self.grid[0])):
self.heuristic[i][j] = abs(i - self.goal[0]) + \

47
abs(j - self.goal[1])

# ------------------------------------------------
# A* for searching a path to the goal

def astar(self):

if self.heuristic == []:
raise ValueError("Heuristic must be defined to run A*")
# internal motion parameters
delta = [[-1, 0], # go up
[ 0, -1], # go left
[ 1, 0], # go down
[ 0, 1]] # do right

# open list elements are of the type: [f, g, h, x, y]

closed = [[0 for row in range(len(self.grid[0]))]

for col in range(len(self.grid))]
action = [[0 for row in range(len(self.grid[0]))]
for col in range(len(self.grid))]

closed[self.init[0]][self.init[1]] = 1

x = self.init[0]
y = self.init[1]
h = self.heuristic[x][y]
g=0
f=g+h
open = [[f, g, h, x, y]]
found = False # flag that is set when search complete
resign = False # flag set if we can't find expand
count = 0

while not found and not resign:

# check if we still have elements on the open list
if len(open) == 0:
resign = True
print('###### Search terminated without success')

else:
# remove node from list
open.sort()
open.reverse()

48
 next = open.pop()
x = next[3]
y = next[4]
g = next[1]

# check if we are done

if x == self.goal[0] and y == self.goal[1]:

found = True
# print '###### A* search successful'

else:
# expand winning element and add to new open list
for i in range(len(delta)):
x2 = x + delta[i][0]
y2 = y + delta[i][1]
if x2 >= 0 and x2 < len(self.grid) and y2 >= 0 \
and y2 < len(self.grid[0]):
if closed[x2][y2] == 0 and self.grid[x2][y2] == 0:
g2 = g + self.cost
h2 = self.heuristic[x2][y2]
f2 = g2 + h2
open.append([f2, g2, h2, x2, y2])
closed[x2][y2] = 1
action[x2][y2] = i

count += 1

# extract the path

invpath = []
x = self.goal[0]
y = self.goal[1]
invpath.append([x, y])
while x != self.init[0] or y != self.init[1]:
x2 = x - delta[action[x][y]][0]
y2 = y - delta[action[x][y]][1]
x = x2
y = y2
invpath.append([x, y])

self.path = []

49
 for i in range(len(invpath)):
self.path.append(invpath[len(invpath) - 1 - i])

# ------------------------------------------------
# this is the smoothing function

def smooth(self, weight_data = 0.1, weight_smooth = 0.1,

tolerance = 0.000001):

if self.path == []:
raise ValueError("Run A* first before smoothing path")

self.spath = [[0 for row in range(len(self.path[0]))] \

for col in range(len(self.path))]
for i in range(len(self.path)):
for j in range(len(self.path[0])):
self.spath[i][j] = self.path[i][j]

change = tolerance
while change >= tolerance:
change = 0.0
for i in range(1, len(self.path)-1):
for j in range(len(self.path[0])):
aux = self.spath[i][j]

self.spath[i][j] += weight_data * \
(self.path[i][j] - self.spath[i][j])

self.spath[i][j] += weight_smooth * \
(self.spath[i-1][j] + self.spath[i+1][j]
- (2.0 * self.spath[i][j]))
if i >= 2:
self.spath[i][j] += 0.5 * weight_smooth * \
(2.0 * self.spath[i-1][j] - self.spath[i-2][j]
- self.spath[i][j])
if i <= len(self.path) - 3:
self.spath[i][j] += 0.5 * weight_smooth * \
(2.0 * self.spath[i+1][j] - self.spath[i+2][j]
- self.spath[i][j])

change += abs(aux - self.spath[i][j])

# ------------------------------------------------

50
3. The robot moves along the generated path using PID control. The control code is
shown below:
def run(grid, init, orientation, goal, spath, params, printflag = False, distance =
1, timeout = 100):

myrobot = robot()
myrobot.set(init[0], init[1], orientation)
myrobot.set_noise(steering_noise, distance_noise, measurement_noise)
robotPos_x = []
robotPos_y = []
robot_orientation = []
steeringAngles = []
cte = 0.0
err = 0.0
N =0
index = 0 # index into the path

while not myrobot.check_goal(goal, accuracy) and N < timeout:

if len(spath) - 1 <= index:
robotPos_x = []
robotPos_y = []
robot_orientation = []
steeringAngles = []

myrobot.set(init[0], init[1], orientation)

cte = 0.0
err = 0.0
N =0
index = 0 # index into the path

else:
diff_cte = - cte
inti_cte = 0.0

# ----------------------------------------
# compute the CTE
robotPose = [myrobot.x, myrobot.y, myrobot.orientation]

#some basic vector calculations:

dx = spath[index + 1][0] - spath[index][0]
dy = spath[index + 1][1] - spath[index][1]
drx = robotPose[0] - spath[index][0]
dry = robotPose[1] - spath[index][1]

51
 #u is the robot estimate projectes onto the path segment:
u = (drx * dx + dry * dy) / (dx**2 + dy**2)

#u is the cte estimate projectes onto the normal path segment:

cte = (dry * dx - drx * dy) / sqrt(dx**2 + dy**2)

# spath_x = spath[index][0]
# spath_y = spath[index][1]
if u >1.0 :
index += 1
# ---------------------------------------

diff_cte += cte #diff_cte = (cte(new) - cte(old) ) / dt ###suppose dt=1

inti_cte += cte #intigral = SUM(cte)

#PID
steer = - params[0] * cte - params[1] * diff_cte - params[2] * inti_cte

myrobot, steeringAngle = myrobot.move(grid, steer, distance)

robotPos_x.append(myrobot.x)
robotPos_y.append(myrobot.y)
robot_orientation.append(myrobot.orientation * 180/pi)

steeringAngles.append(steeringAngle)

if not myrobot.check_collision(grid):
print('##### Collision ####')

err += (cte ** 2)
N += 1

if printflag:
print(myrobot, cte, index, u)

print([myrobot.check_goal(goal), myrobot.num_collisions,
myrobot.num_steps])
return [myrobot.check_goal(goal), robotPos_x, robotPos_y,
robot_orientation, steeringAngles]

52
 4. The robot checks for goal at every step or grid point, the check_goal code is shown
below:
def check_goal(self, goal, threshold = 1.0):
dist = sqrt((float(goal[0]) - self.x) ** 2 + (float(goal[1]) - self.y) ** 2)
return dist < threshold
The full code of the mobile robot is given in appendix B.

53
 CHAPTER 6
PRACTICAL RESULTS

6.1 Introduction
Engineering projects usually have a difference between real behavior and simulation
because of errors caused by environment and hardware equipment so the practical tests are necessary
to evaluate the system performance and improve until getting the best match between simulated and
real results.

6.2 Testing the manipulator positioning accuracy

While testing the reach position of the manipulator’s gripper, the performance was found accurate
enough. This was expected since the 1k was derived mathematically and only 3 joints equipped with
servomotors were used. Since speed of the second manipulator joints was high could damage the
mechanism, it was adjusted using the common delay function in the programming code for the servo
motors.

6.3 Testing the mobile robot positioning accuracy

The motion of the mobile robot was tested to extract the positioning accuracy of the car. The
tests were repeated number of times to extract a general performance parameter. While commanding
the car to move on straight line of 5 meters length, the maximum deviation from the lint was 30 cm,
Figure (6.1 a). Also. While commanding the car to move on a circular path, the error on the steering
angle was 11 degrees, Figure (6.1 a).

Figure (6.1) (a) Steering angle error (b) Distance error

54
6.3.1 Analysis the Errors
1. The error of the distance is mostly caused by the variance of the battery voltage. This affects
the motor speed and results eventually in a deviation from the given path.
2. A considerable reason for the error in the steering angle is the inequality of mass distribution
of the vehicle components that results in an inequality of the moment of inertia about the
vehicle axes.

6.3.2 Possible solutions

1. Distance error: We solve this error by increasing the speed of the car so the error
could be reduced to 10 cm. The error perhaps could be reduced further if a new good
quality 12v battery is used.
2. Steering angle: The steering angle error could be remedied eliminated or using a
software calibration to correct the angle error.

55
 CHAPTER 7
CONCLUSION AND FUTURE DEVELOPMENT

This report described the design and fabrication processes of an autonomous mobile SCARA
manipulator. A mobile car equipped with PID control was developed to carry a 3 DOF SCARA
manipulator and move autonomously on the floor. A*search algorithm was implemented to allow the
car to choose the best possible path in terms of the shortest distance. The manipulator was designed
grip and release objects on a certain height. A raspberry-pi controller was programmed via python to
coordinate the overall motion of the car and the manipulator. The kinematic and dynamic models for
the car and the manipulator were derived and the performance was simulated in MATLAB and
python environments. In practical experimentation, the overall performance was tested and found
satisfactory. We hope this graduation project will open a wide door for the potential research
capabilities in robotics available in our department and serve a forward step towards the development
of the industrial society surrounding us.

The project could be enhanced further by considering the following issues:

1- Equipping the robot with a camera, stationary or attached to the robot, to extract the map
instead of entering the data manually.
2- The accuracy of the movement of the car can be enhanced by adding encoders to the
motors of the car.

Finally, it is our wished that the project may get developed further and finds an upcoming group to
work on it.

56
 REFRENCES
[1] Introduction to robotics, [https://en.wikibooks.org/wiki/Robotics/Introduction] accessed October-
15-2017.

[2] Peter Corke, 2011. Robotics, Vision & Control, Springer.

[3] S K Saha, 2008.Introduction to Robotics, New Delhi, India: Tata Mcgraw-Hill.

[4] Ganesh S. Hegde, (2006), A Text Book On Industrial Robotics,1st Edition, Laxim Publication.

[5] Serial manipulators, [https://en.m.wikipedia.org], accessed January 5 2018.

[6] Merlet, J.P. (2008). Parallel Robots, 2nd Edition, Springer.

[7] Parallel manipulators, [https://en.m.wikipedia.org], accessed January 5 2018.

[8] Robot Configurations, [http://nptel.ac.in/courses/112103174/module7/lec5/3.html], accessed

January 6 2018

[9] All Types of Robots - By Locomotion, http://www.robotpark.com/All-Types-Of-Robots,

Accessed December 14 2017.

[10] Whatis.com website, May 2016, definition of robot,

[http://whatis.techtarget.com/definition/robot-insect-robot-autonomous-robot]. Accessed December
10 2018

[11] John J. Craig, 2005. Introduction to Robotics Mechanics and Control, 3rd Edition. Boston, Ma:
Addison Wesley.

[12] Forward Kinematics, [online] available at: https://en.m.wikipedia.org, accessed January 4 2018.

[13] Inverse Kinematics, [online] available at: https://en.m.wikipedia.org, accessed January 4 2018.

[14] F. C. Park and K. M. Lynch, 2012. Introduction to Robotics Mechanics, Planning, And Control,
Cambridge University Press.

[15] Firebots: autonomous Fire Detecting Robots report, Waterloo university, [online] available at:
https://eng.uwaterloo.ca/~smasiada/FirebotsReport.

[16] Mark W. Spong, Seth Hutchinson, And M. Vidyas,1989, Robot Dynamics and Control 2nd
Edition, Wiley and Sons.

57
[17] Introducing the raspberry pi 3 [https://hackaday.com/2016/02/28/introducing-the-raspberry-pi
3/]

[18] Servo-driver, [online] available at: [[https://www.adafruit.com/product/815]

[19] Robot-gripper, [online] available at: [http://store.makeblock.com/robot-gripper], Accessed may

14 2017.

[20] ADS115, [online] available at: [https://www.adafruit.com/product/1085]

[21] Rabu, 17 May 2017 [online] available at: http://jessicasiboro.blogspot.com/2017/05/servo-

service-on-output-and-application.html, Accessed October 2 2017.

[22] Tooth-spline-servo-to-shaft-couplers, [online] available at: https://www.servocity.com/25-tooth-

spline-servo-to-shaft-couplers, Accessed October 2 2017.

[23] Sharp IR Distance Sensor, available at [https://www.bananarobotics.com/shop/Sharp-

GP2Y0A21YK0F-IR-Distance-Sensor?gclid=EAIaIQobChMIz_fahJye2AIVjMBkCh3e4A-
dEAQYAiABEgI1UfD_BwE], Accessed October 2 2017.

[24] Hc-sr04, Datasheet, available at: [http://www.electroschematics.com/8902/hc-sr04-datasheet/].

[25] SLAM Course, Udacity Community, [online] available at:

[Https://Classroom.Udacity.Com/Courses/Cs373], Accessed June 14 2017

[26] Python distribution, [https://en.wikipedia.org/wiki/Anaconda_ (Python distribution)].

58
 Appendix A
Robotics Arm Codes

1. Robotic Arm kinematics and trajectory planning code:

import math

import numpy as np

np.set_printoptions(threshold=np.nan)

pi = math.pi

cos = math.cos

sin = math.sin

sqrt = math.sqrt

atan2 = math.atan2

deg = 180/pi

rad = pi/180

a1 = 28

a2 = 32 #link = 22 , gripper = 10

d2 = 5

d4 = 5

def myMap(x, in_min, in_max, out_min, out_max):

return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min

class RobotArm:

def __init__(self, q1=0.0, q2=0.0, d3=0.0):

59
self.q1 = q1

self.q2 = q2

self.d3 = d3

T = self.forwardK(q1, q2, d3)

self.x = T[0]

self.y = T[1]

self.z = T[2]

def joints_update(self, q1, q2, d3):

self.q1 = q1

self.q2 = q2

self.d3 = d3

def coord_update(self, x, y, z):

self.x = x

self.y = y

self.z = z

def forwardK(self, q1, q2, d3):

q1 = q1 * rad

q2 = q2 * rad

d3 = d3 * rad

DH = np.matrix([[0, 0, 0, q1],

[0, a1, d2, q2],

[pi, a2, d4 + d3, 0 ]])

Y = DH[2, :]

60
 Z = DH[1, :]

C = DH[0, :]

Tr3 = np.matrix([[cos(Y[0, 3]), -sin(Y[0, 3]), 0, Y[0, 1]],

[sin(Y[0, 3])*cos(Y[0, 0]), cos(Y[0, 3])*cos(Y[0, 0]), -sin(Y[0, 0]), -sin(Y[0, 0])*Y[0, 2]],

[sin(Y[0, 3])*sin(Y[0, 0]), cos(Y[0, 3])*sin(Y[0, 0]), cos(Y[0, 0]), cos(Y[0, 0])*Y[0, 2]],

[0, 0, 0, 1]])

Tr2 = np.matrix([[cos(Z[0, 3]), -sin(Z[0, 3]), 0, Z[0, 1]],

[sin(Z[0, 3])*cos(Z[0, 0]), cos(Z[0, 3])*cos(Z[0, 0]), -sin(Z[0, 0]), -sin(Z[0, 0])*Z[0, 2]],

[sin(Z[0, 3])*sin(Z[0, 0]), cos(Z[0, 3])*sin(Z[0, 0]), cos(Z[0, 0]), cos(Z[0, 0])*Z[0, 2]],

[0, 0, 0, 1]])

Tr1 = np.matrix([[cos(C[0, 3]), -sin(C[0, 3]), 0, C[0, 1]],

[sin(C[0, 3])*cos(C[0, 0]), cos(C[0, 3])*cos(C[0, 0]), -sin(C[0, 0]), -sin(C[0, 0])*C[0, 2]],

[sin(C[0, 3])*sin(C[0, 0]), cos(C[0, 3])*sin(C[0, 0]), cos(C[0, 0]), cos(C[0, 0])*C[0, 2]],

[0, 0, 0, 1]])

Tr = Tr1*Tr2*Tr3

Roll = q1+q2

Roll = Roll*deg

Tr = np.around(Tr, decimals=4)

T = [Tr[0,3], Tr[1,3], Tr[2,3], Roll]

return T

def Inv_K(self , X, Y, Z, Config = 'R', angle = 'd'):

if sqrt(X**2 + Y**2) > 50 or sqrt(X**2 + Y**2) < 31.2 or Z > (d2-d4) or Z < -30:

print("Values exceds min/max reachable points ! ")

61
 return

else:

#Calculation of Theta(2)

Cos_q2 = ((X**2)+(Y**2)-(a1**2)-(a2**2))/(2*a1*a2)

if(sqrt((X**2)+(Y**2)) >= 50):

Sin_q2a = 0

Sin_q2b = 0

else:

Sin_q2a = sqrt(1-(Cos_q2**2))

Sin_q2b = -sqrt(1-(Cos_q2**2))

Theta2_a = atan2(Sin_q2a,Cos_q2)

Theta2_b = atan2(Sin_q2b,Cos_q2)

#Calculation of Theta(1)

K1 = a1+(a2*Cos_q2)

K2_a = a2*Sin_q2a

K2_b = a2*Sin_q2b

Theta1_a = atan2(Y, X)-atan2(K2_a, K1)

Theta1_b = atan2(Y, X)-atan2(K2_b, K1)

#Calculation of d3

d3 = d2 - d4 - Z

62
 if angle == 'r':

if (Config=='R'):

T = [Theta1_a + (pi/2), Theta2_a + (pi/2), d3]

b = (T[0] - 0) * (0 - pi) / (pi - 0) + pi

T[0] = b

elif (Config=='L'):

T = [Theta1_b + (pi/2), Theta2_b + (pi/2), d3]

b = (T[0] - 0) * (0 - pi) / (pi - 0) + pi

T[0] = b

elif angle == 'd':

if (Config=='R'):

T = [Theta1_a*deg+(90), Theta2_a*deg+(90), d3]

b = (T[0] - 0) * (0 - 180) / (180 - 0) + 180

T[0] = b

elif (Config=='L'):

T = [Theta1_b*deg+(90), Theta2_b*deg+(90), d3]

b = (T[0] - 0) * (0 - 180) / (180 - 0) + 180

T[0] = b

return T

def moveRobot(self, space, x_q1, y_q2, z_d3, t, traj, config = 'R'):

# Check the type of space (cartisian or Joints)

tf = max(t)

63
if space == 'cart':

x, y, z = x_q1, y_q2, z_d3

q0 = [self.q1 * rad, self.q2 * rad, self.d3]

qf_deg = self.Inv_K(x, y, z, config)

qf = self.Inv_K(x, y, z, config, 'r')

elif space == 'joint':

T = self.forwardK(x_q1, y_q2, z_d3)

x, y, z = T[0], T[1], T[2]

q0 = [self.q1 * rad, self.q2 * rad, self.d3]

qf_deg = [x_q1, y_q2, z_d3]

qf = [x_q1 * rad, y_q2 * rad, z_d3]

# Check the type of Trajectory (quantic or quibic)

if traj == 5:

##### Joint (1) #####

a0_1 = q0[0]

a3_1 = (10*(qf[0]-q0[0]))/(tf**3)

a4_1 = (-15*(qf[0]-q0[0]))/(tf**4)

a5_1 = (6*(qf[0]-q0[0]))/(tf**5)

q1 = a0_1 + (a3_1*(t**3)) + (a4_1*(t**4)) + (a5_1*(t**5))

q1 = q1*deg

v1 = (3*a3_1*(t**2)) + (4*a4_1*(t**3)) + (5*a5_1*(t**4))

64
 a1 = (6*a3_1*t) + (12*a4_1*(t**2)) + (20*a5_1*(t**3))

##### Joint (2) #####

a0_2 = q0[1];

a3_2 = (10*(qf[1]-q0[1]))/(tf**3)

a4_2 = (-15*(qf[1]-q0[1]))/(tf**4)

a5_2 = (6*(qf[1]-q0[1]))/(tf**5)

q2 = a0_2 + (a3_2*(t**3)) + (a4_2*(t**4)) + (a5_2*(t**5))

q2 = q2*deg

v2 = (3*a3_2*(t**2)) + (4*a4_2*(t**3)) + (5*a5_2*(t**4))

a2 = (6*a3_2*t) + (12*a4_2*(t**2)) + (20*a5_2*(t**3))

##### Joint (3) #####

a0_3 = q0[2];

a3_3 = (10*(qf[2]-q0[2]))/(tf**3)

a4_3 = (-15*(qf[2]-q0[2]))/(tf**4)

a5_3 = (6*(qf[2]-q0[2]))/(tf**5)

#convert the z axis reading to ultrasonic reading

qf[2] = myMap(qf[2], 30, 0, 0, 30)

q3 = (len(q2)) * [qf[2]]

q = np.array([q1, q2, q3])

q = q.transpose()

65
 q = np.around(q, decimals = 1)

v = np.array([v1, v2])

v = v.transpose()

v = np.around(v, decimals = 4)

a = np.array([a1, a2])

a = a.transpose()

a = np.around(a, decimals = 4)

elif traj == 3:

##### Joint (1) #####

a0_1 = q0[0]

a2_1 = (qf[0]-q0[0])*(3/(tf*tf))

a3_1 = -(qf[0]-q0[0])*(2/(tf*tf*tf))

q1 = a0_1 + (a2_1*(t**2)) + (a3_1*(t**3))

q1 = q1 * deg

v1 = (2*a2_1*t) + (3*a3_1*t**2)

a1 = (2*a2_1) + (6*a3_1*t)

##### Joint (2) #####

a0_2 = q0[1]

a2_2 = (qf[1]-q0[1])*(3/(tf*tf));

a3_2 = -(qf[1]-q0[1])*(2/(tf*tf*tf));

66
 q2 = a0_2 + (a2_2*(t**2)) + (a3_2*(t**3))

q2 = q2*deg

v2 = (2*a2_2*t) + (3*a3_2*t**2)

a2 = (2*a2_2) + (6*a3_2*t)

##### Joint (3) #####

a0_3 = q0[2]

a2_3 = (qf[2]-q0[2])*(3/(tf*tf));

a3_3 = -(qf[2]-q0[2])*(2/(tf*tf*tf));

#convert the z axis reading to ultrasonic reading

qf[2] = myMap(qf[2], 30, 0, 0, 30)

q3 = (len(q2)) * [qf[2]]

q = np.array([q1, q2, q3])

q = q.transpose()

q = np.around(q, decimals = 1)

v = np.array([v1, v2])

v = v.transpose()

v = np.around(v, decimals = 4)

a = np.array([a1, a2])

a = a.transpose()

a = np.around(a, decimals = 4)

67
 # Update the current pos and joint angle

self.joints_update(qf_deg[0], qf_deg[1], qf_deg[2])

self.coord_update(x, y, z)

return (q, v, a)

def __repr__(self):

return 'Robot pose:\n\t[q1=%.6s q2=%.6s d3=%.6s]\n\t[x=%.6s y=%.6s z=%.6s]' \

% (str(self.q1), str(self.q2), str(self.d3), \

str(self.x), str(self.y), str(self.z))

2. Servo Code:

from __future__ import division

import time

import math

# Import the PCA9685 module.

import Adafruit_PCA9685

# Initialise the PCA9685 using the default address (0x40).

pwm = Adafruit_PCA9685.PCA9685()

# Set frequency to 60hz, good for servos.

frequency = 60

pwm.set_pwm_freq(frequency)

68
#%%%%%%%%%%%

#Finding the min and max pulse length for the servo by (Mohammed Rizq)

# Knowing that 0.5 ms will move the servo to 0 position

# and 1.5 ms will move the servo to 90 position

# and 2.5 ms will move the servo to 180 position

#Finding The time per tick = time(1/freq) divided by teh Resolution(2^(12))

t = 1/frequency

time_per_tic = t/4096

#So, the min and max pulse length can be found as follows:

servo_min = math.ceil(0.0005/time_per_tic) # Min pulse length out of 4096

servo_max = math.floor(0.0025/time_per_tic) # Max pulse length out of 4096

#%%%%%%%%%%%

def myMap(x, in_min, in_max, out_min, out_max):

return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min

def moveServo(deg, channel):

pulseWidth = myMap(deg, 0, 180, 155, servo_max)

pwm.set_pwm(channel, 0, int(pulseWidth))

69
3. Ultrasonic Code:

#Libraries

import RPi.GPIO as GPIO

import time

GPIO.setwarnings(False)

#GPIO Mode (BOARD / BCM)

GPIO.setmode(GPIO.BCM)

#set GPIO Pins

GPIO_TRIGGER = 16

GPIO_ECHO = 20

#set GPIO direction (IN / OUT)

GPIO.setup(GPIO_TRIGGER, GPIO.OUT)

GPIO.setup(GPIO_ECHO, GPIO.IN)

def getDistance():

# set Trigger to HIGH

GPIO.output(GPIO_TRIGGER, True)

# set Trigger after 0.01ms to LOW

time.sleep(0.00001)

GPIO.output(GPIO_TRIGGER, False)

70
 StartTime = time.time()

StopTime = time.time()

# save StartTime

while GPIO.input(GPIO_ECHO) == 0:

StartTime = time.time()

# save time of arrival

while GPIO.input(GPIO_ECHO) == 1:

StopTime = time.time()

# time difference between start and arrival

TimeElapsed = StopTime - StartTime

# multiply with the sonic speed (34300 cm/s)

# and divide by 2, because there and back

distance = (TimeElapsed * 34300) / 2

#get 10 results and find the avarage to reduce the random error

Ultrasonic_Resolution = 10

distMatrix = []

for i in range(Ultrasonic_Resolution):

dist = distance

distMatrix.append(dist)

time.sleep(0.04)

dist = sum(distMatrix)/Ultrasonic_Resolution

return dist

71
4. DC-Motor Code:

import RPi.GPIO as GPIO

from time import sleep

from ultrasonic import getDistance

GPIO.setmode(GPIO.BCM)

#Linear

IN_1 = 24

IN_2 = 25

#gripper

IN_3 = 18

IN_4 = 23

GPIO.setwarnings(False)

GPIO.setup(IN_1, GPIO.OUT)

GPIO.setup(IN_2, GPIO.OUT)

GPIO.setup(IN_3, GPIO.OUT)

GPIO.setup(IN_4, GPIO.OUT)

GPIO.output(IN_1, GPIO.LOW)

GPIO.output(IN_2, GPIO.LOW)

GPIO.output(IN_3, GPIO.LOW)

GPIO.output(IN_4, GPIO.LOW)

72
def forward():

GPIO.output(IN_1, GPIO.HIGH)

GPIO.output(IN_2, GPIO.LOW)

def reverse():

GPIO.output(IN_2, GPIO.HIGH)

GPIO.output(IN_1, GPIO.LOW)

def motorStop():

GPIO.output(IN_1, GPIO.LOW)

GPIO.output(IN_2, GPIO.LOW)

def gripper(mtype = 'Open'):

if mtype == 'Open': #Open

GPIO.output(IN_3, GPIO.HIGH)

GPIO.output(IN_4, GPIO.LOW)

elif mtype == 'Close': #Close

GPIO.output(IN_4, GPIO.HIGH)

GPIO.output(IN_3, GPIO.LOW)

elif mtype == 'Stop':

GPIO.output(IN_4, GPIO.LOW)

GPIO.output(IN_3, GPIO.LOW)

def linearMotion(goal):

init = getDistance()

73
 if goal < init:

while goal < init:

init = getDistance()

print(init)

sleep(0.5)

forward()

if goal >= init:

motorStop()

elif goal > init:

while goal > init:

init = getDistance()

print(init)

sleep(0.5)

reverse()

if goal <= init:

motorStop()

elif goal == init:

motorStop()

74
5. Raspberry piCode for the Robotics Arm:

import servoLibrary

from time import sleep

from RobotArm import *

import DC_Motor

import RPi.GPIO as GPIO

#Linear

IN_1 = 24

IN_2 = 25

#gripper

IN_3 = 18

IN_4 = 23

#car

IN_5 = 22

IN_6 = 27

GPIO.setwarnings(False)

GPIO.setup(IN_1, GPIO.OUT)

GPIO.setup(IN_2, GPIO.OUT)

GPIO.setup(IN_3, GPIO.OUT)

GPIO.setup(IN_4, GPIO.OUT)

GPIO.setup(IN_5, GPIO.OUT)

GPIO.setup(IN_6, GPIO.OUT)

75
#Function(1)

def armModule(desiredHigh, moduleType = 'pick'): #module Types [pick, drop]

if moduleType == 'pick':

#First open the gripper

print("module is pick")

print("opening the gripper...")

DC_Motor.gripper('Open')

sleep(1)

print("gripper Stop...")

DC_Motor.gripper('Stop')

#Move the linear motor to the desired high

print("Moving the linear motor...")

DC_Motor.linearMotion(desiredHigh)

#Now close the gripped

print("Closing the gripper...")

DC_Motor.gripper('Close')

sleep(1)

print("gripper Stop...")

DC_Motor.gripper('Stop')

#Return the linear motor to initial high

print("Return the linear to orginal position...")

DC_Motor.reverse()

76
sleep(3)

elif moduleType == 'drop':

print("module is drop")

#Move the linear motor to the desired high

print("Moving the linear motor...")

DC_Motor.linearMotion(desiredHigh)

#Now open the gripper

DC_Motor.gripper('Open')

print("opening the gripper...")

sleep(1)

print("gripper Stop...")

DC_Motor.gripper('Stop')

print("Return the linear to orginal position...")

DC_Motor.reverse()

sleep(6)

#Function(2)

def btw_twoPos(motor_1_Channel, motor_2_Channel, desiredPos_1, desiredPos_2):

#Move the robot from initial pos. to desired pos. 1

q,v,a = scara.moveRobot('joint', desiredPos_1[0], desiredPos_1[1], desiredPos_1[2], time,

5)

for i in range(1, len(q)):

servoLibrary.moveServo(q[i-1, 0], motor_1_Channel)

servoLibrary.moveServo(q[i-1, 1], motor_2_Channel)

77
 sleep(servoSpeed)

#Now pick the object

armModule(desiredPos_1[2], 'pick')

## sleep(15)

#Move the robot to desired pos. 2

q2,v,a = scara.moveRobot('joint', desiredPos_2[0], desiredPos_2[1], desiredPos_2[2],

time, 5)

for i in range(1, len(q2)):

servoLibrary.moveServo(q2[i-1, 0], motor_1_Channel)

servoLibrary.moveServo(q2[i-1, 1], motor_2_Channel)

sleep(servoSpeed)

#Now drop the object

armModule(desiredPos_2[2], 'drop')

## btw_twoPos(15, 14, [0, 90, 0], [0, 180, 0])

#Function

def btw_twoPosCart(motor_1_Channel, motor_2_Channel, desiredPos_1, desiredPos_2):

#Move the robot from initial pos. to desired pos. 1

q,v,a = scara.moveRobot('cart', desiredPos_1[0], desiredPos_1[1], desiredPos_1[2], time, 5,

'L')

for i in range(1, len(q)):

servoLibrary.moveServo(q[i-1, 0], motor_1_Channel)

78
 servoLibrary.moveServo(q[i-1, 1], motor_2_Channel)

sleep(servoSpeed)

#Now pick the object

armModule(7, 'pick')

## sleep(15)

#Move the robot to desired pos. 2

q2,v,a = scara.moveRobot('cart', desiredPos_2[0], desiredPos_2[1], desiredPos_2[2],

time, 5, 'L')

for i in range(1, len(q2)):

servoLibrary.moveServo(q2[i-1, 0], motor_1_Channel)

servoLibrary.moveServo(q2[i-1, 1], motor_2_Channel)

sleep(servoSpeed)

#Now drop the object

armModule(7, 'drop')

## btw_twoPosCart(15, 14, [30, -18, -25], [30, 18, -25])

#Function(3)

def cartMotion():

x = input("Enter X in cm: ")

y = input("Enter Y in cm: ")

z = input("Enter Z in cm: ")

79
q,v,a = scara.moveRobot('cart', x, y, z, time, 5, 'L')

for i in range(1, len(q)):

servoLibrary.moveServo(q[i-1, 0], 15)

servoLibrary.moveServo(q[i-1, 1], 14)

## print(q[i-1, 0])

sleep(servoSpeed)

## print(q[len(q)-1, 2])

DC_Motor.linearMotion(q[len(q)-1, 2])

#Function(5)

def linearTest():

desiredHigh = input("Enter the desired High: ")

DC_Motor.linearMotion(desiredHigh)

#Function(6)

def robotLinearMotion():

Xi = input("Enter the initial X point: ")

Yi = input("Enter the initial Y point: ")

Xf = input("Enter the final X point: ")

Yf = input("Enter the fianl Y point: ")

z = input("Enter Z: ")

q,v,a = scara.moveRobot('cart', Xi, Yi, z, time, 5)

for i in range(len(q)):

80
 servoLibrary.moveServo(q[i-1, 0], 15)

servoLibrary.moveServo(q[i-1, 1], 14)

DC_Motor.linearMotion(z)

sleep(servoSpeed)

q2,v2,a2 = scara.moveRobot('cart', Xf, Yf, z, time, 5)

for i in range(len(q2)):

servoLibrary.moveServo(q2[i-1, 0], 15)

servoLibrary.moveServo(q2[i-1, 1], 14)

sleep(servoSpeed)

q1 = input("Enter the initial angle of link (1): ")

q2 = input("Enter the initial angle of link (2): ")

init = [q1, q2, 0]

time = np.arange(0, 100, 0.5)

servoSpeed = 0.02

scara = RobotArm(init[0], init[1], 0)

## q1 = input("Enter the angle of link (1): ")

## q2 = input("Enter the angle of link (2): ")

## q,v,a = scara.moveRobot('joint', q1, q2, 0, time, 5)

## for i in range(1, len(q)):

81
## servoLibrary.moveServo(q[i-1, 0], 15)

## servoLibrary.moveServo(q[i-1, 1], 14)

## sleep(servoSpeed)

#main loop

try:

while True:

btw_twoPosCart(15, 14, [40, 22, -25], [40, -22, -25])

## cartMotion()

## linearTest()

print(scara)

# Reset by pressing CTRL + C

except KeyboardInterrupt:

print("program stopped by User")

GPIO.output(IN_1, GPIO.LOW)

GPIO.output(IN_2, GPIO.LOW)

GPIO.output(IN_3, GPIO.LOW)

GPIO.output(IN_4, GPIO.LOW)

GPIO.cleanup()

82
 Appendix B:
Mobile Robot Codes

1. Mobile Robot search algorithm, path smoothing and PID control

Codes:

from math import *

import random
from time import sleep

#class plan: used for finding the best and cheap path from start to goal
#By the use of A star Search (Artificail Inteligence)
class plan:

# --------
# init:
# creates an empty plan
#

def __init__(self, grid, init, goal, cost = 1):

self.cost = cost
self.grid = grid
self.init = init
self.goal = goal
self.make_heuristic(grid, goal, self.cost)
self.path = []
self.spath = []

# --------
#
# make heuristic function for a grid

83
 def make_heuristic(self, grid, goal, cost):
self.heuristic = [[0 for row in range(len(grid[0]))]
for col in range(len(grid))]
for i in range(len(self.grid)):
for j in range(len(self.grid[0])):
self.heuristic[i][j] = abs(i - self.goal[0]) + \
abs(j - self.goal[1])

# ------------------------------------------------
#
# A* for searching a path to the goal
#
#

def astar(self):

if self.heuristic == []:
raise ValueError("Heuristic must be defined to run A*")

# internal motion parameters

delta = [[-1, 0], # go up
[ 0, -1], # go left
[ 1, 0], # go down
[ 0, 1]] # do right

# open list elements are of the type: [f, g, h, x, y]

closed = [[0 for row in range(len(self.grid[0]))]

for col in range(len(self.grid))]
action = [[0 for row in range(len(self.grid[0]))]
for col in range(len(self.grid))]

closed[self.init[0]][self.init[1]] = 1

84
 x = self.init[0]
y = self.init[1]
h = self.heuristic[x][y]
g=0
f=g+h

open = [[f, g, h, x, y]]

found = False # flag that is set when search complete

resign = False # flag set if we can't find expand
count = 0
while not found and not resign:

# check if we still have elements on the open list

if len(open) == 0:
resign = True
print('###### Search terminated without success')

else:
# remove node from list
open.sort()
open.reverse()
next = open.pop()
x = next[3]
y = next[4]
g = next[1]

# check if we are done

if x == self.goal[0] and y == self.goal[1]:

found = True
# print '###### A* search successful'

85
 else:
# expand winning element and add to new open list
for i in range(len(delta)):
x2 = x + delta[i][0]
y2 = y + delta[i][1]
if x2 >= 0 and x2 < len(self.grid) and y2 >= 0 \
and y2 < len(self.grid[0]):
if closed[x2][y2] == 0 and self.grid[x2][y2] == 0:
g2 = g + self.cost
h2 = self.heuristic[x2][y2]
f2 = g2 + h2
open.append([f2, g2, h2, x2, y2])
closed[x2][y2] = 1
action[x2][y2] = i

count += 1

# extract the path

invpath = []
x = self.goal[0]
y = self.goal[1]
invpath.append([x, y])
while x != self.init[0] or y != self.init[1]:
x2 = x - delta[action[x][y]][0]
y2 = y - delta[action[x][y]][1]
x = x2
y = y2
invpath.append([x, y])

self.path = []
for i in range(len(invpath)):
self.path.append(invpath[len(invpath) - 1 - i])

86
 # this is the smoothing function

def smooth(self, weight_data = 0.1, weight_smooth = 0.1,

tolerance = 0.000001):

if self.path == []:
raise ValueError("Run A* first before smoothing path")

self.spath = [[0 for row in range(len(self.path[0]))] \

for col in range(len(self.path))]
for i in range(len(self.path)):
for j in range(len(self.path[0])):
self.spath[i][j] = self.path[i][j]

change = tolerance
while change >= tolerance:
change = 0.0
for i in range(1, len(self.path)-1):
for j in range(len(self.path[0])):
aux = self.spath[i][j]

self.spath[i][j] += weight_data * \
(self.path[i][j] - self.spath[i][j])

self.spath[i][j] += weight_smooth * \
(self.spath[i-1][j] + self.spath[i+1][j]
- (2.0 * self.spath[i][j]))
if i >= 2:
self.spath[i][j] += 0.5 * weight_smooth * \
(2.0 * self.spath[i-1][j] - self.spath[i-2][j]
- self.spath[i][j])
if i <= len(self.path) - 3:
self.spath[i][j] += 0.5 * weight_smooth * \
(2.0 * self.spath[i+1][j] - self.spath[i+2][j]
87
 - self.spath[i][j])

change += abs(aux - self.spath[i][j])

# ------------------------------------------------

# this is the robot class

#Used to set robot Coordinate
#and to move the robot by specific distance and orientation

class robot:
# init:
# creates robot and initializes location/orientation to 0, 0, 0

def __init__(self, length = 0.54):

self.x = 0.0
self.y = 0.0
self.orientation = 0.0
self.length = length
self.steering_noise = 0.0
self.distance_noise = 0.0
self.measurement_noise = 0.0
self.num_collisions =0
self.num_steps =0
# set:
# sets a robot coordinate
def set(self, new_x, new_y, new_orientation):

self.x = float(new_x)
self.y = float(new_y)
self.orientation = float(new_orientation) % (2.0 * pi)

# set_noise:
# sets the noise parameters
#
88
 def set_noise(self, new_s_noise, new_d_noise, new_m_noise):
# makes it possible to change the noise parameters
# this is often useful in particle filters
self.steering_noise = float(new_s_noise)
self.distance_noise = float(new_d_noise)
self.measurement_noise = float(new_m_noise)

# --------
# check:
# checks of the robot pose collides with an obstacle, or
# is too far outside the plane

def check_collision(self, grid):

for i in range(len(grid)):
for j in range(len(grid[0])):
if grid[i][j] == 1:
dist = sqrt((self.x - float(i)) ** 2 +
(self.y - float(j)) ** 2)
if dist < 0.5:
self.num_collisions += 1
return False
return True

def check_goal(self, goal, threshold = 1.0):

dist = sqrt((float(goal[0]) - self.x) ** 2 + (float(goal[1]) - self.y) ** 2)
return dist < threshold
# --------
# move:
# steering = front wheel steering angle, limited by max_steering_angle
# distance = total distance driven, most be non-negative

def move(self, grid, steering, distance,

tolerance = 0.001, max_steering_angle = pi / 6.0):
89
 if steering > max_steering_angle:
steering = max_steering_angle
if steering < -max_steering_angle:
steering = -max_steering_angle
if distance < 0.0:
distance = 0.0

# make a new copy

res = robot()
res.length = self.length
res.steering_noise = self.steering_noise
res.distance_noise = self.distance_noise
res.measurement_noise = self.measurement_noise
res.num_collisions = self.num_collisions
res.num_steps = self.num_steps + 1

# apply noise
steering2 = random.gauss(steering, self.steering_noise)
distance2 = random.gauss(distance, self.distance_noise)

steerAngle = 90 + steering2*180/pi
steerAngle = (steerAngle - 60) * (60 - 120) / (120 - 60) + 120
print(steerAngle)
# Execute motion
turn = tan(steering2) * distance2 / res.length #from corke equation 4.2

if abs(turn) < tolerance:

# approximate by straight line motion

res.x = self.x + (distance2 * cos(self.orientation))

res.y = self.y + (distance2 * sin(self.orientation))
res.orientation = (self.orientation + turn) % (2.0 * pi)
90
 else:

# approximate bicycle model for motion

radius = distance2 / turn #from corke equation 4.1

cx = self.x - (sin(self.orientation) * radius)
cy = self.y + (cos(self.orientation) * radius)
res.orientation = (self.orientation + turn) % (2.0 * pi)
res.x = cx + (sin(res.orientation) * radius)
res.y = cy - (cos(res.orientation) * radius)

# check for collision

# res.check_collision(grid)

## print(res)
return res, steerAngle

# --------
# sense:
#

def sense(self):

return [random.gauss(self.x, self.measurement_noise),

random.gauss(self.y, self.measurement_noise),
random.gauss(self.orientation, self.measurement_noise)]

# --------
# measurement_prob
# computes the probability of a measurement
#

def measurement_prob(self, measurement):

91
 # compute errors
error_x = measurement[0] - self.x
error_y = measurement[1] - self.y

# calculate Gaussian
error = exp(- (error_x ** 2) / (self.measurement_noise ** 2) / 2.0) \
/ sqrt(2.0 * pi * (self.measurement_noise ** 2))
error *= exp(- (error_y ** 2) / (self.measurement_noise ** 2) / 2.0) \
/ sqrt(2.0 * pi * (self.measurement_noise ** 2))

return error

def __repr__(self):
return '[x=%.5f y=%.5f orient=%.5f]' % (self.x, self.y, self.orientation * 180 / pi)
## return '[%.5f, %.5f]' % (self.x, self.y)

#__________________________________
#------------------------------------------

# run: runs control program for the robot (PID CONTROL)

def run(grid, init, orientation, goal, spath, params, printflag = False, distance = 1, timeout =
100):

myrobot = robot()
myrobot.set(init[0], init[1], orientation)
myrobot.set_noise(steering_noise, distance_noise, measurement_noise)

robotPos_x = []
robotPos_y = []
robot_orientation = []

steeringAngles = []
92
cte = 0.0
err = 0.0
N =0
index = 0 # index into the path

while not myrobot.check_goal(goal, accuracy) and N < timeout:

if len(spath) - 1 <= index:

robotPos_x = []
robotPos_y = []
robot_orientation = []
steeringAngles = []

myrobot.set(init[0], init[1], orientation)

cte = 0.0
err = 0.0
N =0

index = 0 # index into the path

else:
diff_cte = - cte
inti_cte = 0.0

# ----------------------------------------
# compute the CTE

robotPose = [myrobot.x, myrobot.y, myrobot.orientation]

#some basic vector calculations:

dx = spath[index + 1][0] - spath[index][0]
dy = spath[index + 1][1] - spath[index][1]
drx = robotPose[0] - spath[index][0]
93
 dry = robotPose[1] - spath[index][1]

#u is the robot estimate projectes onto the path segment:

u = (drx * dx + dry * dy) / (dx**2 + dy**2)

#u is the cte estimate projectes onto the normal path segment:

cte = (dry * dx - drx * dy) / sqrt(dx**2 + dy**2)

# spath_x = spath[index][0]
# spath_y = spath[index][1]

if u >1.0 :
index += 1
# ----------------------------------------

diff_cte += cte #diff_cte = (cte(new) - cte(old) ) / dt ###suppose dt=1

inti_cte += cte #intigral = SUM(cte)

#PID
steer = - params[0] * cte - params[1] * diff_cte - params[2] * inti_cte

myrobot, steeringAngle = myrobot.move(grid, steer, distance)

robotPos_x.append(myrobot.x)
robotPos_y.append(myrobot.y)
robot_orientation.append(myrobot.orientation * 180/pi)

steeringAngles.append(steeringAngle)

if not myrobot.check_collision(grid):
print('##### Collision ####')

err += (cte ** 2)
94
 N += 1

if printflag:
print(myrobot, cte, index, u)

print([myrobot.check_goal(goal), myrobot.num_collisions, myrobot.num_steps])

return [myrobot.check_goal(goal), robotPos_x, robotPos_y, robot_orientation,
steeringAngles]
#________________________
#_------------------------------------

# this is our main routine

def mainRun(grid, init, orientation, goal, steering_noise, distance_noise, measurement_noise,

weight_data, weight_smooth, p_gain, d_gain, i_gain, speed):

path = plan(grid, init, goal)

path.astar()
path.smooth(weight_data, weight_smooth)

check_goal, x, y , orint, steeringAngles = run(grid, init, orientation, goal, path.spath,

[p_gain, d_gain, i_gain], False, speed)
return check_goal, x, y, orint, steeringAngles

steering_noise = 0.0 #12*pi/180

distance_noise = 0.0
measurement_noise = 0.3

accuracy = 0.1

95
2. Raspberry pi Code for the mobile robot:

from math import *

from mobileRobot import *
import RPi.GPIO as GPIO
from time import sleep
#import servoLibrary
from IRsensor import *

GPIO.setmode(GPIO.BCM)
IN_1 = 22
IN_2 = 27

GPIO.setwarnings(False)

GPIO.setup(IN_1, GPIO.OUT)
GPIO.setup(IN_2, GPIO.OUT)
GPIO.output(IN_1, GPIO.LOW)
GPIO.output(IN_2, GPIO.LOW)

f = GPIO.PWM(IN_1, 50)
r = GPIO.PWM(IN_2, 50)
f.start(0)
r.start(0)
def moveForward(maxSpeed = 30):
f.start(0)
print("Moving")
f.ChangeDutyCycle(maxSpeed)
##___________________##
def pause():
print("Pause")
f.ChangeDutyCycle(0)
r.ChangeDutyCycle(0)

96
##__________________##
def stop():
print("Stoping")
f.stop()
r.stop()
##_________________##
def moveReverse(speed):
r.start(0)
print("Reverse")
r.ChangeDutyCycle(speed)
##_________________##
def check_obstacles(channel_1, channel_2, channel_3, threshold = 15):
dist_1 = IRreading(channel_1)
if dist_1 <= threshold:
return True
else:
dist_2 = IRreading(channel_2)
if dist_2 <= threshold:
return True
else:
dist_3 = IRreading(channel_3)
if dist_3 <= threshold:
return True
##_________________##
def returnTo(Angles, maxSpeed, delay):
Angles.reverse()

for i in range(len(Angles)):
print(Angles[i])
servoLibrary.moveServo(Angles[i] - 10, 13)
moveReverse(maxSpeed-15)
sleep(delay)

if i == len(Angles) -1:
97
stop()
##_________________##

def chooseNextAction():
print("___________________________________")
print("Choose the next action: ")
print(" 1 - Return to previous point")
print(" 2 - Go to new postion")
print(" 3 - nothing")
x = input("Enter the number of the action : ")
return x
##____________________##

grid = [[0, 0, 0, 0], # y >, x down

[0, 0, 0, 0],
[1, 0, 1, 0],
[0, 1, 0, 0]]

steering_noise = 0.0
distance_noise = 0.1
measurement_noise = 0.3

weight_data = 0.1
weight_smooth = 0.2
p_gain = 2.0
d_gain = 6.0

i_gain = 0.0

distance = 0.1
speed = 0.18/1 # 0.12 m/sec
delay = distance/speed
maxSpeed = 30

98
try:
x=2
while x == 2:
init = []
goal = []
xi = input("Enter initial x : ")
init.append(xi)
yi = input("Enter initial y : ")
init.append(yi)
xf = input("Enter final x : ")
goal.append(xf)
yf = input("Enter final y : ")
goal.append(yf)
myPath = plan(grid, init, goal)
myPath.astar()
myPath.smooth(weight_data, weight_smooth)
mySmothPath = myPath.spath
slop = ((mySmothPath[1][1]) - (mySmothPath[0][1])) / ((mySmothPath[1][0]) -
(mySmothPath[0][0])+0.0000001)

if((mySmothPath[1][0]) < (mySmothPath[0][0])):

best_orient = 180 + atan(slop) * 180/pi
else:
best_orient = atan(slop) * 180/pi

print("Note: Best value for Orientation = ", best_orient)

orient = input("Enter initial orientation in deg: ")
orientation = orient * pi/180
check_goal, robotPos_x, robotPos_y, orint, steeringAngles = mainRun(grid, init,
orientation, goal, steering_noise,\
distance_noise,measurement_noise,\
weight_data,weight_smooth,p_gain,\
d_gain, i_gain, distance)
99
 if(check_goal):
for i in range(len(steeringAngles)):
## while check_obstacles(0, 1, 2, 25): #(distance, Analog channel)
## pause()
## print("Obstacles found")

print(steeringAngles[i])
print("[x=%.5f y=%.5f orient=%.5f]" % (robotPos_x[i], robotPos_y[i], orint[i]))
# servoLibrary.moveServo(steeringAngles[i] - 10, 13)
moveForward(maxSpeed)
sleep(delay)

if i == len(steeringAngles) -1:
stop()
x = chooseNextAction()
if(x == 1):
returnTo(steeringAngles, maxSpeed, delay)

elif(x >= 3):

print("### The End ###")
else:
print("Could not find a solution !")

# Reset by pressing CTRL + C

except KeyboardInterrupt:
print("program stopped by User")
f.stop()
r.stop()
GPIO.output(22, GPIO.LOW)
GPIO.output(27, GPIO.LOW)
GPIO.cleanup()

100