Omnidirectional pong playing robot

IN

DEGREE PROJECT TECHNOLOGY, FIRST CYCLE, 15 CREDITS

,

STOCKHOLM SWEDEN 2019

Omnidirectional pong

playing robot

Pong playing robot using kiwi drive and a PID

controller

FILIP BJÖRKLUND

Omnidirectional pong playing robot

Pong playing robot using kiwi drive and a PID controller

FILIP BJ ¨ORKLUND CHRISTOPHER STRAND

Bachelor’s Thesis at ITM Supervisor: Nihad Subasic

Abstract

This project goal was to determine the flexibility of an om-nidirectional robot with a physical implementation of the video game pong. A robot was created to follow and catch a ball and could play against a human player. The challenge of the project was to create a stable system that could move in a straight path and catch the ball within a reasonable distance from the other player.

A camera was used to implement an image recognition system that could determine the two-dimensional position of the ball and hard coded values for the size of the ball was used to simulate a three-dimensional position. Given these values, the robot was able to follow the ball and push the ball when close. For the omnidirectional system, so-called kiwi drive with three DC motors and omnidirectional wheels was used. Ultrasonic sensors were also used to stop the robot if a nearby wall was too close.

To make the robot move in a straight path, control the-ory together with a compass module was used to measure the angular error which was fed as feedback to the system. This enabled the robot to travel in a straight path and catch the ball.

The results of the project showed that it is possible to con-trol an omnidirectional robot with concon-trol theory in a stable manner. Using image recognition with a web camera to-gether with OpenCV is fast enough to create a fast robotic system that can successfully complete a given task.

Referat

Flerdimensionell pongrobot

Detta projekts m˚al var att analysera hur flexibel det g˚ar att g¨ora en robot med flerdimensionella hjul, det vill s¨aga en robot som har hjul som g¨or att den kan r¨ora sig med tre frihetsgrader. Detta gjordes genom att implementera en fy-sisk version av datorspelet pong. I projektet byggdes en robot som kunde f¨olja och f˚anga en boll samt spela mot en m¨ansklig spelare. Utmaningen i projektet var att skapa ett stabilt system som kunde m¨ojligg¨ora f¨or roboten att f¨ardas en rak v¨ag och f˚anga bollen inom ett rimligt avst˚and fr˚an motspelaren.

En webbkamera anv¨andes f¨or att implementera ett bildi-genk¨anningssystem som kunde avg¨ora den tv˚adimensionella positionen f¨or bollen och h˚ardkodade v¨arden p˚a bollens storlek anv¨andes f¨or att simulera en tredimensionell posi-tion. Givet dessa v¨arden lyckades roboten f¨olja efter bollen och trycka ifr˚an den n¨ar bollen n¨armade sig. Tre stycken DC-motorer med tillh¨orande hjul anv¨andes f¨or att skapa en treaxlig konfiguration f¨or det flerdimensionella systemet. Ultraljudssensorer anv¨andes f¨or att stanna roboten om den kom f¨or n¨ara en v¨agg i spelplanen.

F¨or att f˚a roboten att r¨ora sig l¨angs en rak linje anv¨andes en kompassmodul f¨or att m¨ata vinkelfelet som uppstod n¨ar roboten k¨orde p˚aett felaktigt s¨att. Detta vinkelfel anv¨andes som˚aterkoppling f¨or en PID-regulator vilket i sin tur m¨ojlig-gjorde f¨or roboten att kunna f¨olja och f˚anga bollen l¨angs en rak linje.

Resultaten fr˚an projektet visade att en flerdimensionell ro-bot g˚ar att kontrollera p˚a ett stabilt s¨att genom en PID-regulator och bildigenk¨anning med hj¨alp av en webbkamera och OpenCV ¨ar tillr¨ackligt snabbt f¨or att kunna skapa ett robotsystem som kan l¨osa en given uppgift.

Nyckelord: Mekatronik, Bildigenk¨anning, Flerdimensio-nell robot, Reglerteknik

Acknowledgements

We would like to thank our supervisor Nihad Subasic for necessary guidance and introduction into the world of mechatronics. We would also like to thank the team of course assistants and Staffan Qvarnstr¨om for their invaluable help on the con-struction of the prototype for this thesis. A special thanks to the course assistants who has stayed long after scheduled time and provided aid to the project. Further-more, we would like to thank our fellow peer students for opposition and discussion on our project.

Contents

1 Introduction 1 1.1 Background . . . 1 1.2 Purpose . . . 1 1.3 Scope . . . 2 1.4 Method . . . 2 2 Theory 3 2.1 Motors . . . 3 2.1.1 DC motor . . . 3 2.1.2 H-bridge . . . 3 2.1.3 PWM . . . 4 2.2 Microcontroller . . . 4 2.3 Object recognition . . . 4 2.4 Omnidirectional drive . . . 5 2.4.1 Kiwi drive . . . 5 2.5 Sensors . . . 7 2.5.1 Ultrasonic sensors . . . 7

2.5.2 Electronic compass and magnetic fields . . . 7

2.6 PID controller . . . 8

2.6.1 Theory behind the PID controller . . . 8

2.6.2 PID in this project . . . 8

3 Demonstrator 11 3.1 Setup . . . 11

3.2 Hardware . . . 11

3.2.1 Electronics . . . 11

3.2.2 Motors . . . 13

3.2.3 Wheels and chassis . . . 13

3.3 Software . . . 14

3.3.1 Image recognition . . . 14

3.3.2 Movement control . . . 15

4 Results 17

4.1 Stability and PID values . . . 17 4.2 Image recognition and speed of the system . . . 18 4.3 Sensors . . . 19

5 Discussion and conclusion 21

5.1 Discussion . . . 21 5.2 Conclusion . . . 22

Bibliography 23

Appendices 24

A Python code for Raspberry Pi 25

B Datasheet for Ultrasonic sensor SRF05-HY 49

C Datasheet for Logitech C270 HD Webcam 55

D Datasheet for H-bridge L9997ND 59

List of Figures

2.1 Omnidirectional wheel with freerollers, edited with Photoshop . . . 5 2.2 The kiwi configuration, made in Solid Edge + Keyshot . . . 6 2.3 The geometry of a kiwi drive robot, created in Adobe llustrator . . . 6 3.1 An ultrasonic sensor set up with two resistors, made in Adobe Illustrator 12 3.2 Image of PCB designed in Autodesk Eagle and edited in Adobe Photoshop 13 3.3 A flowchart showing the workflow of the robot, drawn in draw.io . . . . 14 4.1 A plot showing how the robot moves back after being physically pushed,

List of Tables

3.1 Electronic components . . . 11 4.1 PID values for the first test and controller . . . 18

List of Acronyms

CPU Central processing unit DC Direct current

DoF Degrees of freedom FPS Frames per second HSV Hue Saturation Value PCB Printed circuit board

PID Proportional–integral–derivative (controller) PLA Polyactic Acid

PWM Pulse-width modulation RGB Red Green Blue

Nomenclature

A Ampere θ Angle ω Angular velocity ° Degree e Error

KD Factor on the derivative term in a PID controller KI Factor on the integral term in a PID controller KP Factor on the proportional term in a PID controller R Radius of the robot

r Setpoint value in a PID controller W Resistance

τ Momentary time interval

u Input from a PID controller to the system v Velocity

Chapter 1

Introduction

1.1

Background

Robots are increasingly used in industries and home applications. These applica-tions will play an important role in future society. Automation is already common in the industrial sector and there is little indication that this trend will decrease. Artificial intelligence often has a common basis both with robotics and software applications, such as video games. The results of developing artificial intelligence in video games can often be used in robotic applications. One of the first video games was pong. Pong is basically a two-dimensional version of table tennis. A simple game like pong can be used as a starting point for further analysis and research for a robots ability to perform a task. Omnidirectional robots, with wheels allowing for a flexible movement, have the potential to be used in a wide range of applications and research and developments of these robots can lead to many interesting products. The question that initiated this project was to analyze how stable one can make an omnidirectional robot using control theory. The simple rules of pong were chosen as a starting point for this analysis. If it is possible to create competing robots playing a game with the simple rules of pong then the results of this easily can be extrapolated to more complex systems and tasks.

1.2

Purpose

The purpose of this project was to see the stability of a proportional-integral-derivative (PID) controlled omnidirectional robot, using a physical version of the video game pong as an implementation. Image recognition was also used for detect-ing the ball. Improvements on omnidirectional robots can lead to new possibilities for the robot industry. These types of robots are specially useful in applications were there is a lack of space and fast travel time is required. Examples of this can be robot vacuum cleaners or delivery robots. From this purpose of the project, the

CHAPTER 1. INTRODUCTION

following research questions came to mind:

• How stable can one make a PID controlled omnidirectional robot?

• How well and fast can an omnidirectional robotic system react to a ball using image recognition?

• What sensors are able to be used, and which are best suited for this project?

1.3

Scope

The scope of this project was to construct a robot implemented with a variety of sensors and a working PID controller, with limited time, money and knowledge. This means that this project did not aim to make a perfectly stable system as an end product, however still try to accomplish the main goal and research questions that was proposed in section 1.2.

1.4

Method

Information gathering was conducted at the beginning of the project about motors, sensors, microcontrollers, theory about omnidirectional drive and image recogni-tion. This research helped form a demonstrator that could help answer the research questions mentioned in section 1.2. A single board computer, Raspberry Pi, was chosen due to the fact that image recognition was chosen for the robots ability to see the ball. Omnidirectional wheels driven by direct current (DC) motors were chosen to make it possible for the robot to move in all two-dimensional directions in a easy way. Different sensors was placed on the robot to make it aware on its surroundings and its own movement. The demonstrator was then tested and improved based on its performance. These results from the testing was used to draw conclusions about the robots performance and how a omnidirectional robot can be used to perform a relative simple task of playing pong.

Chapter 2

Theory

2.1

Motors

2.1.1 DC motor

The DC motor is a fairly simple motor to use, by connecting a voltage (V) source and ground, the motor starts to rotate, and by changing the polarity of the voltage, the rotational direction is changed. A DC motor consists of a stator, rotor and a commutator. The stator consists of permanent magnets, and the rotor is usually built from a isolated metal core, wrapped in spools that are winded by isolated copper wire. The commutator is made from two or more conducting parts, isolated from each other, and connected to the spools on the rotor. When the rotor is turning, the commutator is changing the direction current is running through the spools, creating a variable magnetic field. This magnetic field has opposite polarity to the permanent magnets that repels each other and thus starts to rotate [4].

2.1.2 H-bridge

To help control the direction the DC motor is rotating, an H-bridge can be used [7]. An H-bridge is a circuit which consists of transistors and often diodes. A transistor is an electrical component that in this case works as an electrical switch, controlled by the microcontroller and are able to turn on and off very fast. A diode has the functionality to limit the direction the current can flow to one way only [4]. The control of the DC motors by the H-bridge, is accomplished by using four transistors, which are turned on or off in a specific way, allowing the current to flow through the motor in both directions. Usually diodes are used in parallel to the transistors to prevent the huge voltage spikes that are being created by the motor when the power is turned off, otherwise components in the microcontroller can be damaged [7].

CHAPTER 2. THEORY

2.1.3 PWM

If a DC motor has its power supply from a non-variable source, such as a battery, one single voltage is applied over the motor. Since one voltage leads to a single rotation speed on the DC motor, only one velocity is achieved on the wheels. If different speeds are required from the motors, a method called pulse-width modu-lation (PWM) can be used. This method creates a pulse of the high voltage value for a period of time instead of a steady high voltage value. During a set period of time, this pulse can vary in length. If the DC motor is on a high voltage state for 75 % of the period, 75 % of the maximum speed with that specific voltage is achieved. The length of the period can be described as the frequency. The PWM method can therefore be used as a way to control the speed of a motor without changing the voltage supply [4].

2.2

Microcontroller

Two different microcontrollers were considered for this project, the Raspberry Pi and the Arduino. For this particular project, a Raspberry Pi was chosen because it has a faster central processing unit (CPU) than the Arduino, and has the ability to use any Universal Serial Bus (USB) web camera. Because the project needs to process a lot of data in form of images, a faster CPU is key to allow the robot to respond as fast as possible.

The negatives with Raspberry Pi is that it is a bit difficult to run on batteries, that is because it has to run in a fairly narrow voltage range, and can draw up to 2.4 amperes (A) see appendix E. To solve this, a buck converter can be used to deliver the desired voltage and ampere, or a simpler solution is to use a power bank with the right outputs. The Raspberry Pi has pins that can supply 5V digital output. However, the same pins are not designed for using 5V inputs [3]. If the pins on the Raspberry Pi should be used as inputs, these should deliver 3.3V instead of the 5V. Any higher value than 3.3V risk damaging the Raspberry Pi.

2.3

Object recognition

In order to know where an object is, a robot can use different methods and sensors. One way for a robot to know where an object is located is to use a camera and read from the resulting image. When the image is captured by the camera, it is transformed into pixels which is stored on the computer of the robot. These pixels can be later used to get information about what is in front of the robot [1].

2.4. OMNIDIRECTIONAL DRIVE

2.4

Omnidirectional drive

Omnidirectional wheels are wheels designed to allow for movement in both transver-sal and rotational directions, making the wheels able to move in three degrees of freedom (DoF). This type of wheel, displayed at figure 2.1, makes it easy for a robotic system to be used in a wide range of applications. The wheel is built as a normal wheel but with smaller decoupled wheels, called freerollers, on the structure [9]. These freerollers rotates when the entire wheel is pushed sideways, allowing for movement to this side without slippage from the wheels.

Figure 2.1. Omnidirectional wheel with freerollers, edited with Photoshop

2.4.1 Kiwi drive

By using three omnidirectional wheels, which is the least amount of wheels needed to make the configuration work, a configuration called Kiwi drive is created [9]. In kiwi drive, the wheels are orientated 120° from each other. Each wheel is driven by a motor connected to that specific wheel. This allows each wheel to be rotated at different speeds and directions. The resulting force from the three wheels in this configuration is the force that makes the robot move in a particular direction. The kiwi configuration is shown in figure 2.2.

Since the wheels have freerollers connected to the wheels, each wheel will be able to move both sideways and forwards, allowing for the three DoF drive. This type of drive system can be described mathematically, allowing for easy control over the system. The geometry of a kiwi drive robot can be described by figure 2.3.

With θn referring to each angle between the x-axis and the n:th motor axis. If

each axis from the motor to the centre of the robot is R long, we are able to de-scribe each wheel velocity v1, v2, v3 with equation 2.1 [10].

CHAPTER 2. THEORY

Figure 2.2. The kiwi configuration, made in Solid Edge + Keyshot

Figure 2.3. The geometry of a kiwi drive robot, created in Adobe llustrator

(v1, v2, v3)T =    −sinθ1 cosθ1 1 −sinθ2 cosθ2 1 −sinθ3 cosθ3 1   (vx, vy, Rω) T _(2.1)

With vx, vy being the velocities of the robot in a coordinate system defined by

2.5. SENSORS

the robot and ω the angular velocity of the robot. The equation 2.1 turns into equations 2.2, 2.3 and 2.4 when expanded:

v1=−sinθ1vx+ cosθ1vy+Rω (2.2) v2=−sinθ2vx+ cosθ2vy+Rω (2.3) v3=−sinθ3vx+ cosθ3vy+Rω (2.4)

The Kiwi configuration puts the different axis of the wheels 120° between each other and with a coordinate system defined in figure 2.3 gives θ1 = 30, θ2 = 150

and θ3 = 270. This simplifies the motor equations into the following:

v1 =−1₂vx+ √ 3 2 vy+Rω (2.5) v2 =−1 2vx− √ 3 2 vy+Rω (2.6) v3=vx+Rω (2.7)

These equations can therefore be used for controlling each motor speed with respect to the desired velocities of the robot.

2.5

Sensors

2.5.1 Ultrasonic sensors

Ultrasonic sensors were used in this project to gather information about the robots location. This sensor works by sending a pulse of sound and then register the time it takes the sound to reach back to the sensor. By knowing the approximately speed of soundv and the time tit takes to get back, the distance d to the object can be calculated with equation 2.8.

d= vt

2 (2.8)

2.5.2 Electronic compass and magnetic fields

A magnetometer can be used as a way to measure the surrounding magnetic fields in a similar way to how a compass works. This can be used to get a bearing of the robot with respect to the cardinal directions.

If the robot is supposed to move in a straight direction, for example straight along the X-axis, no angular velocity is wanted and therefore ω should be equal to 0 in

CHAPTER 2. THEORY

equation 2.1. If however the robot rotates, this can be measured by a compass module as an error between the target degree value and the current value according to equation 2.9:

θerror =θtarget−θcurrent (2.9)

The target degree value is defined as the initial value measured by the robot before any movement has been done.

2.6

PID controller

There is a high risk of robotic systems to show differences between the desired output and actual speeds. This can result in an unstable system that deviates from the desired task. Control theory can be used to solve this problem. A proportional-integral-derivative controller (PID), is a relative simple controller within control theory.

2.6.1 Theory behind the PID controller

A PID controller uses a feedback signaly(t) and a setpoint r(t) as input from the system and uses this to calculate a new output signal u(t). The error between the feedback signal and the setpoint is defined in equation 2.10.

e(t) =r(t)−y(t) (2.10) The error is in a PID used to calculate the the signalu(t), which is later used as an input value for the system. The equation that describes a PID controller is given by equation 2.11 [2]: u(t) =KPe(t) +KI Z t 0 e(τ)dτ +KD d dte(t) (2.11) Well chosen values ofKP,I,D for one particular system creates an input signal u(t)

which allows for a stable system. The three different terms in 2.11 correspond to the P, I and D part in the PID controller. The P term applies a factor Kp on

the current error which leads to a new signal u. High values on Kp creates fast

control over the error, meaning that a given error will create a new high signal to the system, for example more voltage over a DC motor leading to a faster motor velocity. Too high values onKp will lead to decreased stability in the system. The

I-part of 2.11 can eliminate errors that are constant over time and the D-part can increase the stability of the system.

2.6.2 PID in this project

For an omnidirectional robot a PID controller can for example be used as a way to make sure no rotation is being applied to the robot. In this project, this was used

2.6. PID CONTROLLER

to get the angular error received from a compass module as defined in equation 2.9 and usingθerror as the errore(t):

e(t) =θerror(t) =θtarget(t)−θcurrent(t) (2.12)

This corresponds to a input signal urobot given from 2.11 and 2.12: urobot(t) =KPθerror(t) +KI Z t 0 θerror(τ)dτ+KD d dtθerror(t) (2.13) Since no rotation corresponds toω= 0 in equation 2.1, the signalurobot can be used

to control the different motor velocities of the omnidirectional robot. Following the same derivation from 2.1 shown in section 2.5.1, the third term in equation 2.5 to 2.7 can be replaced by urobot sinceθerror will not effect the values of vx and vy in

any meaningful way. Furthermore, urobot tries to achieve θerror = 0 and since ω is

the derivative of θerror, a value of 0 onθerror will result in a value of 0 on ω, thus

allowing for following equations: v1,P ID=−1 2vx+ √ 3 2 vy+urobot (2.14) v2,P ID=−1 2vx− √ 3 2 vy+urobot (2.15) v3,P ID =vx+urobot (2.16)

These equations can then be used to control each motor with a common PID con-troller for the entire robot, resulting in the robot compensating for unwanted rota-tion.

A second PID controller was also implemented for the image recognition system. The camera detects the distance from the center of the frame to the center-point of the ball. This distance was used as an error to the PID, essentially controlling vx

Chapter 3

Demonstrator

3.1

Setup

For testing the robot an environment was set up to allow a physical representation of the game of pong. A ball was needed to simulate the pong ball and walls was necessary to make it possible for the ball to bounce and not drift away. A web camera was used to get information on where the ball was located and ultrasonic sensors were used to make sure that the robot did not crash into any walls. The kiwi drive configuration was used, which in turn required omnidirectional wheels, DC motors and motor drivers. A compass module was used to measure the angular movements of the robot, that was used as inputs for the PID controller.

3.2

Hardware

3.2.1 Electronics

The electronic parts that were used in this project are listed in table 3.1. Electronic components

Component Data

3x DC motors Maxon Motor 3x Ultrasonic sensors HY-SRF05 3x Motordrivers L9997ND Web camera Logitech c270

Compass CMPS03

Singleboard computer Raspberry Pi 3A+ 10x Batteries AA 1.5V

Power bank 5V, 3.2A, 20000 mAh

3x Resistors 178 ohm

3x Resistors 330 ohm

CHAPTER 3. DEMONSTRATOR

Datasheets for each components can be found in appendix B - F. As described in section 2.2, a Raspberry Pi should not have more than 3.3V on its input pins, since this risk damaging the microcontroller. The ultrasonic sensors were supplied with 5V and the output from these sensors were leveled down to minimize any risk of damaging the Raspberry Pi. Resistors was used to allow for the voltage drop, shown in figure 3.1. A 330 W resistor was chosen as the resistor closest to the ground and the other resistorR1 was calculated with equation 3.1 [4]:

3,3 5 =

330

R1+ 330 (3.1)

That gives the value of R1 = 170. From this a 178 W resistor was chosen to drop

the voltage to the desired value.

Figure 3.1. An ultrasonic sensor set up with two resistors, made in Adobe Illustrator

The components were soldered together via a printed circuit board (PCB) that was milled for the project. The electric schematic that was used to connect all the com-ponents with power supply and the Raspberry Pi is shown in figure 3.2:

3.2. HARDWARE

Figure 3.2. Image of PCB designed in Autodesk Eagle and edited in Adobe Photo-shop

3.2.2 Motors

The motors where chosen mostly by the torque they were able to produce, because some of the directions the robot needs to be able to travel in requires far more torque from the motors than a conventional drive setup would require. This is due to the fact that the motor equations 2.5 to 2.7 show that in a straightvx direction, v1 and v2 will be half of the v3 motor speed. This means that the PWM-signal

on the first motors will be half of the ones on the third motor. Strong motors are required for the relatively heavy robot to be able to move with Kiwi drive. Stronger voltage over the motors also leads to more torque being drawn from the motors at low PWM-signals. 15V with AA batteries were chosen to be sufficient for this purpose.

3.2.3 Wheels and chassis

The majority of the robots components, everything from chassis to the different mounts for the sensors were designed in Solid edge and 3D-printed in Polyactic Acid (PLA) plastic. The base of the robot was designed with a hole-system, allowing for a more customizable use with sensors and opens up the ability to further build on

CHAPTER 3. DEMONSTRATOR

top and upgrading the robot, like how LEGO is used.

3.3

Software

In order to control the robot, a Raspberry Pi was used and the program was coded in Python. The code was structured with a robot class, containing methods and attributes necessary to control the robot. The methods and attributes that were created for this project in the robot class was then used to play the game of pong according to figure 3.3.

Figure 3.3. A flowchart showing the workflow of the robot, drawn in draw.io

3.3.1 Image recognition

The basic principles on how the image recognition software works is the following, Firstly a photo is taken by the camera and loaded in to the software, the image is then converted from the mostly used color space Red, Green and Blue (RGB), to Hue, Saturation and Value (HSV), which is better suited for filtering the image by color [8]. Then, a color range is then defined depending on the wanted color, all other colors is then filtered away leading to an image only showing the colors

3.3. SOFTWARE

previously defined. These steps are carried out in order to easily distinguish the contours and shape of the object in the image. To help reduce unwanted noise created around the object, a function in the OpenCV library called Morphological transformation was used [6].

Now that contours of the wanted object can be easily distinguished, another function of the OpenCV library, called Hough transform, is used to create a circle around the object if the edge of the object is similar to a circle [5]. From this function, the center of the circle is calculated and returned as a vector with two values, x and y. These vectors contain which pixel the center is located in, and can then be used to calculate where in the photo the object currently is by knowing the amount of pixels the photo contains. This can be used as a signal to the robot, telling it whether to go left or right. The radius of the circle is also calculated and can be used in a similar fashion, the bigger the radius, the closer the object is to the camera, and with some measurements, can be used to calculate the distance between the camera and the object.

3.3.2 Movement control

The equations 2.5, 2.6 and 2.7 show that it is possible to describe the velocities of each wheel only depending on the robots desired movement. This was implemented on the Raspberry Pi as a function, with vx,y as inputs. Both of these inputs were

received from the camera and were simplified as the distance between the center points of the robot and the ball, and the distance from the robot to the ball. In mathematical terms, this simplification would mean, if x is referring to the dis-tance between the center points, that vx = x. This simplification is possible since

the velocity and distance are linearly dependent, according to x = vxt, only

de-pending on the time factort. The time factor is irrelevant in this scenario since the robot was using the maximal possible values from this function to the PWM signals. The different motor velocities were received using the values from equations 2.5, 2.6 and 2.7 and using these as the duty cycle for each respective motor. This means that ifv1 = 100, the duty cycle is put to 100%. For the duty cycle to be valid,v1,2,3

therefore has to be in range of−100 to 100. The fact thatvnboth can have positive

and negative values is due to the fact that each wheel needs to be able to rotate either clockwise or counter-clockwise at any given time. Since the signal to the DC motor only allows one current at a time, two different pins were used to control the different rotations of the wheels, via the motor drivers. Before setting the duty cycle for the motor, the sign ofvnwas checked in order to determine which pin to use.

Since the robot should avoid hitting the walls, the distance from the robot and the walls was continually calculated using ultrasonic sensors. These sensors gath-ered distance measures from the right, left and back side of the robot after each change in output velocity. If these sensors detect any wall to be close, the robot

CHAPTER 3. DEMONSTRATOR

drives in the opposite direction until the wall is a safe distance from the robot. 3.3.3 Control system

With the compass module a control system could be used to make the robot drive in a straight path. A PID controller was used in this project and the feedback used was the current robot angle received from the compass. Object oriented programming became useful to calculate the PID values over time. A PID class was created to keep track of the updated inputs from the PID. The PID controller needs to keep track of both the time difference between two different feedback inputs and the total sum of the error to calculate theI- andD-parts of the PID respectively. This was solved by storing the previous error and the sum of the error as two attributes of the PID objects and also keeping the last recorded time as an attribute. This allowed for a simple PID to be implemented through only using the current error as an input to the calculation.

Chapter 4

Results

4.1

Stability and PID values

One test was made when only checking theurobot value, with no camera input. This

was to see which PID values created a stable system without the effect of the camera and its X and Y output. A manual method was used to find the optimal values. This method consisted of checking each part of the controller separately. The first

Figure 4.1. A plot showing how the robot moves back after being physically pushed, made with matplotlib on the Raspberry Pi.

step of this method was to find a high enough value ofKp so that the robot started

CHAPTER 4. RESULTS

part just enough to make the oscillations disappear. LastlyKI was increased until

no long-term error was found after one rotation of the robot and few initial oscilla-tions occurred. A plot showing the errors over time during a test with good values are shown in figure 4.1. The resulting values after this process are listed in table 4.1.

Optimal PID values without camera inputs PID factor Value

KP 3.6

KI 0.5

KD 0.0725

Table 4.1. PID values for the first test and controller

Another PID was implemented on the image recognition system. This PID re-ceived the distance from the cameras center point as an error and used this error as a feedback signal to the PID. With the aid of the previous rotational PID controller, optimal values was found for the image recognition system. The same method as with the previous PID was used to find optimal values, with the difference of plac-ing a ball in front of the robot, creatplac-ing a error in the X direction and inferring a change in vx of the robot. The optimal values for this PID controller are listed in

table 4.2.

Optimal PID values for the ball detection PID factor Value

KP,camera 1.3

KI,camera 0.23

KD,camera 0.038

Table 4.2. PID values for the second test and controller

4.2

Image recognition and speed of the system

Using a camera for image recognition requires a fair amount of computing power, while the Raspberry Pi has limited computing power. To get the program run-ning smoothly but still not loosing out on necessary data from the camera, some optimization is required. Depending on what the system is going to be used for, the two biggest factors regarding data usage from a camera is the resolution of the image and frames per second (FPS) of the camera. The higher resolution used, the more data every image contains, and thus more of the CPU is needed to run the program. If there is too much data for the CPU to handle, the program may start to stutter and the frame rate will be lowered. Because this project only had to be able to distinguish a ball but most importantly have a high FPS, the resolution was

4.3. SENSORS

lowered, but not to low so that the robot can see the ball even when is it farther away.

4.3

Sensors

By using ultrasonic sensors and a compass module the robots odometry information could be determined. The robot in this project reacted fast to nearby walls due to the placements of the ultrasonic sensors.

The interference from magnetic fields from the robots electronic components and other electrical devices in the testing environment turned out to be a problem. A placement of the compass module near any other electrical components interfered with the magnetometer in the compass module. If the robot moved through a strong magnetic field in the room, this also effected the output value from the compass, creating invalid errors for the PID controller. To avoid this problem, the compass was placed far away from any electrical components used in the robot. A close placement of the compass to the other components was tried with the intention of calibrating the compass given the circumstances. It was found however that the other components affected the compass too much for a calibration to be effective.

Chapter 5

Discussion and conclusion

5.1

Discussion

This project was carried out to analyze the stability of an omnidirectional robot with a PID controller and to implement an image recognition system that could create a pong playing robot. Due the motor equations found in section 2.4.1, it was found that a PID controller can be relatively easy to implement for an omni-directional system as long as some sensor can be used to get feedback information on the specific directional term in the motor equations 2.5 to 2.7. In this project, a compass was used as a PID controller for controlling the error on the rotational term of the robot, leading to a stable path. It would be possible to use, for example, an accelerometer to measure the change in acceleration in the X and Y directions and implement this for a PID controller on these terms as well.

This could be used to create a more stable path in these directions. An accelerome-ter measuring the changing accelerations in theX and Y directions could lead to a more efficient way to generate a stable path and also allows for more control options for the omnidirectional robot. It would also be possible to use the ultrasonic sensors for the same purpose. Since the wheels of an omnidirectional robot allow for trans-verse movement in both the X and Y directions, it is possible that the compass measures a constant angle but the robot itself moves slightly in another unwanted direction. However, with a well optimized PID controller for the rotational term, any rotational error of the robot is eliminated. This eliminates the most dominant stability error of the robot and allows for a relative stable path during each step of the movements.

It was found that the interference from surrounding magnetic fields affected the reli-ability of the compass module. This was solved in a relatively simple way by putting the compass module far away from any other electrical components. Another solu-tion to this could be to enclose the component in a electromagnetic shielding. It is not possible to eliminate the effect from magnetic fields on the component, but it is

CHAPTER 5. DISCUSSION AND CONCLUSION

possible to redirect the magnetic field with a material with high permeability. This was however out of scope of this project and the compass worked sufficiently well by placing it far away from any other components. To calibrate the compass given the circumstances of its placement on the robot could also been used as a way to gain accurate measurements from the compass. However, if placed too close to the other components, the surrounding magnetic fields was too strong and no reliable measurements could be used. Therefore, the simple solution of placing the compass farther away was determined as the best solution for this project.

The robot was able the detect the ball quickly with the help of the web camera and image recognition software. The camera PID controller worked well to move the robot to the ball. Optimization of this code helped increase the speed of the system that in turn made it possible for the robot to move quickly to intercept the ball. Calibrating the color recognition given a new environment was effective to identify the ball and ignore all other surrounding objects. Image recognition with this setup and OpenCV as guiding image recognition software is effective to use for an omnidirectional pong playing robot.

5.2

Conclusion

Omnidirectional robots can, with the right choice of sensors, relatively easily be-come stable with a PID controller. This is due to the fact that each motor of the robot can be controlled via the overall desired velocity of the robot. Each term in the motor equations can be used in the feedback system to get a stable overall path. A PID can also be used in a image recognition system to obtain a fast ball detection. With optimized code, this allows the robot to move fast against the ball making it able to play the game of pong. Ultrasonic sensors are well suited to avoiding nearby obstacles and a compass module is possible to use to control the rotational error. A further development of the robot described in this project could be to also use an accelerometer to more accurately measure the transversal error. Electromagnetic shielding could also used as a way to shield the compass module from outside interference.

Bibliography

[1] Simon Calminder and Mattew K¨allstr¨om Chittum. Object Tracking andIn-terception System : Mobile Object Catching Robot using StaticStereo Vision.

2018. url: http : / / urn . kb . se / resolve ? urn = urn : nbn : se : kth : diva -233135.

[2] Torkel Glad and Lennart Ljung. Reglerteknik : grundl¨aggande teori. swe. 4.,

[omarb.] uppl. Lund: Studentlitteratur, 2006.isbn: 9144022751.

[3] GPIO. _url: https : / / www . raspberrypi . org / documentation / hardware / raspberrypi/gpio/README.md(visited on 05/05/2019).

[4] Hans Johansson.Elektroteknik. swe. Stockholm: Institutionen f¨or

maskinkon-struktion, Tekniska h¨ogsk., 2006.

[5] Jeisung Lee, Chang-Ho Hyun, and Mignon Park. “A Vision-Based Automated Guided Vehicle System with Marker Recognition for Indoor Use”. English. In:

SENSORS 13.8 (AUG 2013), 10052–10073. _issn: 1424-8220. _doi: 10.3390/ s130810052.

[6] Cui Liu and Lianming Wang. “Multi-scale Fuzzy Color Recognition and Seg-mentation of Color Image”. English. In:2016 12TH INTERNATIONAL CON-FERENCE ON NATURAL COMPUTATION, FUZZY SYSTEMS AND KNOWL-EDGE DISCOVERY (ICNC-FSKD). Ed. by Li, MZ and Xiong, N and Tong,

Z and Du, J and Liu, C and Li, KL and Wang, L. 12th International Con-ference on Natural Computation, Fuzzy Systems and Knowledge Discovery (ICNC-FSKD), Changsha, PEOPLES R CHINA, AUG 13-15, 2016. IEEE; IEEE Circuits & Syst Soc; Nanyang Technol Univ; Hunan Univ; State Univ New York; Hunan Normal Univ; Natl Univ Defense Technol. 345 E 47TH ST, NEW YORK, NY 10017 USA: IEEE, 2016, 984–988.isbn: 978-1-5090-4093-3. doi:10.1109/FSKD.2016.7603312.

[7] Mechatronic systems, sensors, and actuators : fundamentals and modeling.

eng. 2. ed.. Electrical engineering handbook series. Boca Raton: Taylor Fran-cis, 2008.isbn: 0-8493-9258-6.

[8] Kusuma Wardhani Mega, Xiangru Yu, and Jinping Li. “Comparative Anal-ysis of Color Edge Detection for Image Segmentation”. English. In: PRO-CEEDINGS OF 2018 INTERNATIONAL CONFERENCE ON COMPUT-ING AND PATTERN RECOGNITION (ICCPR 2018). International

Confer-BIBLIOGRAPHY

ence on Computing and Pattern Recognition (ICCPR), Harbin Inst Technol, Shenzhen, PEOPLES R CHINA, JUN 23-25, 2018. 1515 BROADWAY, NEW YORK, NY 10036-9998 USA: ASSOC COMPUTING MACHINERY, 2018, 93–101.isbn: 978-1-4503-6471-3.doi:10.1145/3232829.3232845.

[9] Levente Raj and Andras Czmerk. “Modelling and simulation of the drivetrain of an omnidirectional mobile robot”. English. In:AUTOMATIKA58.2 (2017),

232–243.issn: 0005-1144. doi:10.1080/00051144.2017.1391612.

[10] Raul Rojas. “Omnidirectional control”. eng. In: (2005). url: http://ftp.

itam.mx/pub/alfredo/ROBOCUP/SSLDocs/PapersTDPs/omnidrive.pdf.

Appendix A

Python code for Raspberry Pi

1 ””” Omnidirectional pong playing robot 2

3 Authors : F i l i p Bjorklund and Christopher Strand 4 Email : ” f i l i p b j o @ k t h . se ” , ” chstrand@kth . se ” 5 Date : 05/2019

6

7 Omnidirectional pong playing robot

8 Bachelor ’ s Thesis P r o j e c t in Mechatronics at KTH (ITM) 9 Course Code : MF133X

10

11 D e s c r i p t i o n : This code uses a robot c l a s s to d e f i n e a o m n i d i r e c t i o n a l

robot with a Kiwi d r i v e c o n f i g u r a t i o n using Raspberry Pi .

12 I t a l s o d e f i n e s a program in which the robot i s able to f o l l o w a b a l l

and simulate the video game pong .

13 14 Requirements 15−−−−−−−−−−−− 16 17 HARDWARE: 18 1 x Raspberry Pi

19 1 x Web camera connected via USB 20 3 x DC Motors with motor d r i v e r s

21 3 x U l t r a s o n i c s e n s o r s (SRF05 used here ) 22 1 x Compass/Magnetometer module (CMPS03) 23 24 SOFTWARE: 25 Python 26 numpy 27 OpenCV 28 Rpi .GPIO 29 smbus

30 Python time module 31 Python math module

32 m a t p l o t l i b ( f o r p l o t t i n g PID e r r o r ) 33−−−−−−−−−−−−

34 ”””

APPENDIX A. PYTHON CODE FOR RASPBERRY PI 36 # Importing modules

37 import sys 38 import cv2 as cv 39 import numpy as np 40 import RPi .GPIO as GPIO 41 import time

42 import math 43 import smbus

44 import m a t p l o t l i b . pyplot as p l t 45

46 GPIO. setwarnings ( False ) 47

48 ################## ROBOT CODE ######################

49 # This part o f the code d e f i n e s a robot c l a s s that can be used f o r an

o m n i d i r e c t i o n a l robot with a webcamera f o r image r e c o g n i t i o n .

50 # 51 52 c l a s s Robot : 53 54 def i n i t ( s e l f ) : 55 56 #Enviroment

57 s e l f . CrashDistance = 10 # Distance from US s e n s o r . Values from

s e n s o r below t h i s r i s k o f chrashing the robot

58 s e l f . backDistance = 12 # Distance from back wall to robot 59

60 #Defiene the wheels 61 s e l f . w1 = 0 . 0 62 s e l f . w2 = 0 . 0 63 s e l f . w3 = 0 . 0 64

65 #For Raspberry . S e t t i n g pins . 66 GPIO. setmode (GPIO.BOARD) 67

68 # frequency f o r PWM 69 s e l f . f r e q = 1600 70

71

72 #CAMERA− S t a r t i n g camera and d e f i n e d e s i r e d r e s o l u t i o n 73 s e l f . s r c = cv . VideoCapture ( 0 )

74 s e l f . s r c .s e t(3 ,1280) 75 s e l f . s r c .s e t( 4 , 2 40 ) 76

77 # Camera c a l i b r a t i o n values , t h e s e v a l u e s change deping on what (

c o l o u r o f the ) b a l l i s being tracked

78 # These are d e f i n e d l a t e r from s t o r e d v a l u e s in a f i l e 79 s e l f . HUE L = 0 80 s e l f .HUE H = 0 81 s e l f . SL = 0 82 s e l f .SH = 0 83 s e l f .VL = 0 84 s e l f .VH = 0 85 s e l f . Xcal = 0 86 s e l f . Ycal = 0 26

87 s e l f . parameter1cal = 0 88 s e l f . parameter2cal = 0 89

90

91 #Define output pins : Motor r i g h t r o t a t i o n 92 s e l f . motor1PINR = 8

93 s e l f . motor2PINR = 16 94 s e l f . motor3PINR = 38 95

96 GPIO. setup ( s e l f . motor1PINR ,GPIO.OUT) 97 GPIO. setup ( s e l f . motor2PINR ,GPIO.OUT) 98 GPIO. setup ( s e l f . motor3PINR ,GPIO.OUT) 99 100 s e l f .pwm1R = GPIO.PWM( s e l f . motor1PINR , s e l f . f r e q ) # I n i t i a l i z e PWM 101 s e l f .pwm2R = GPIO.PWM( s e l f . motor2PINR , s e l f . f r e q ) 102 s e l f .pwm3R = GPIO.PWM( s e l f . motor3PINR , s e l f . f r e q ) 103

104 #Define output pins : Motor l e f t r o t a t i o n 105 s e l f . motor1PINL = 10

106 s e l f . motor2PINL = 18 107 s e l f . motor3PINL = 36 108

109 GPIO. setup ( s e l f . motor1PINL ,GPIO.OUT) 110 GPIO. setup ( s e l f . motor2PINL ,GPIO.OUT) 111 GPIO. setup ( s e l f . motor3PINL ,GPIO.OUT) 112 113 s e l f .pwm1L = GPIO.PWM( s e l f . motor1PINL , s e l f . f r e q ) # I n i t i a l i z e PWM 114 s e l f .pwm2L = GPIO.PWM( s e l f . motor2PINL , s e l f . f r e q ) 115 s e l f .pwm3L = GPIO.PWM( s e l f . motor3PINL , s e l f . f r e q ) 116

117 #ULTRASOUND − INPUT pins (ECHO) 118 s e l f . UPREcho = 11

119 s e l f . UPLEcho = 29 120 s e l f . UPBEcho = 37 121

122 GPIO. setup ( s e l f . UPREcho ,GPIO. IN ) 123 GPIO. setup ( s e l f . UPLEcho ,GPIO. IN ) 124 GPIO. setup ( s e l f . UPBEcho ,GPIO. IN ) 125

126

127 #ULTRASOUND − OUTPUT pins (TRIGGER) 128 s e l f . UPRTrig = 7

129 s e l f . UPLTrig = 23 130 s e l f . UPBTrig = 35 131

132 GPIO. setup ( s e l f . UPRTrig ,GPIO.OUT) 133 GPIO. setup ( s e l f . UPLTrig ,GPIO.OUT) 134 GPIO. setup ( s e l f . UPBTrig ,GPIO.OUT) 135

136 #COMPASS − data from I2C , from address 60 137 s e l f . bus = smbus . SMBus( 1 )

APPENDIX A. PYTHON CODE FOR RASPBERRY PI 139

140 # Camera method , reads inputs from camera

141 # Outputs : X − a d i s t a n c e to the b a l l from a c e n t e r point and Y− a

value r e l a t e d to the r a d i u s o f the b a l l

142 def camera ( s e l f , argv ) :

143 # These v a l u e s are parameters used from the c a l i b r a t e d image

r e c o g n i t i o n . 144 HL = s e l f . HUE L 145 HH = s e l f .HUE H 146 SL = s e l f . SL 147 SH = s e l f .SH 148 VL = s e l f .VL 149 VH = s e l f .VH 150 Xcal = s e l f . Xcal 151 Ycal = s e l f . Ycal 152 p1 = s e l f . parameter1cal 153 p2 = s e l f . parameter2cal 154 # F i r s t time 155 d i s t a n c e = 0 156 157 158 159 #I n i t i a l i z e d i s t a n c e to 0

160 , Image = s e l f . s r c . read ( ) #reads the frames from the camera 161

162 k e r n e l = cv . getStructuringElement ( cv .MORPH ELLIPSE, ( Xcal , Xcal ) )

#Sets k e r n e l to a e l l i p s e shape o b j e c t

163

164 HSV = cv . cvtColor ( Image , cv .COLOR BGR2HSV) #c o n v e r t s

the c o l o r s p a c e from BGR to HSV

165

166 color lowerRange = np . array ( [ HL, SL , VL] ) #Define

lower and upper range to the d e s i r e d c o l o r in hsv c o l o r s p a n

167 color upperRange = np . array ( [HH, SH, VH] ) 168

169 c o l o r f i l t e r = cv . inRange (HSV, color lowerRange , color upperRange )

#f i l t e r s out a l l c o l o r s that are out o f the d e s i r e d c o l o r range ,

170

171 open morph = cv . morphologyEx ( c o l o r f i l t e r , cv .MORPH OPEN, k e r n e l )

#reduces n o i s e around the f i l t e r e d image

172

173 rows = HSV. shape [ 0 ] #Takes

out a l l rows c r e a t e d by the image

174

175 c i r c l e s = cv . HoughCircles ( open morph , cv .HOUGH GRADIENT, 1 , rows /

1 , param1=p1 , param2=p2 , minRadius=0, maxRadius=0)

176 r a d i u s = 0 177

178 # I f c i r c l e s are found , a d i s t a n c e X i s mapped out 179 i f c i r c l e s i s not None :

180 c i r c l e s = np . uint16 ( np . around ( c i r c l e s ) ) 181 f o r i in c i r c l e s [ 0 , : ] :

182 c e n t e r = ( i [ 0 ] , i [ 1 ] )

183

184 # c i r c l e c e n t e r

185 cv . c i r c l e ( open morph , center , 1 , ( 0 , 100 , 100) , 3) 186 # c i r c l e o u t l i n e

187 r a d i u s = i [ 2 ]

188 cv . c i r c l e ( open morph , center , radius , (255 , 0 , 255) , 3) 189 d i s t a n c e = ( c e n t e r [ 0 ] − 272) #c a l c u l a t e the d i s t a n c e from

the c e n t e r o f the c i r c l e to the middle o f the camera

190 191

192 # This e n a b l e s a l i v e view o f the detected c i r c l e s 193 cv . imshow (” detected c i r c l e s ”, open morph )

194

195 X = d i s t a n c e /3.6 #s c a l i n g the d i s t a n c e too 100 , so that the

maximum PWM s i g n a l w i l l not exceed 100%

196 i f r a d i u s i s 0 :

197 Y = 0

198 e l s e:

199 Y = r a d i u s

200 #Used to p r e s s ESC to c l o s e the program while in the camera window 201 k = cv . waitKey ( 5 ) & 0xFF 202 i f k == 2 7 : 203 204 cv . destroyAllWindows ( ) 205 s e l f . motorStop ( ) 206 GPIO. cleanup ( ) 207 208 return X, Y 209 210 211

212 # Functions o f wheel movement

213 # Calcuates the v e c t o r s f o r each wheel on a robot with a Kiwi c o n f i g . 214

215 # IN : X, Y and R. Desiered d i r e c t i o n s o f the robots movement .

Optional : r a d i u s from robot c e n t e r to wheels .

216 # OUT: Val between −100 and 100 f o r wheel motor movement . 217 def wheels ( s e l f ,X,Y,R, r a d i u s = 1) :

218 # Boolean r e l a t e d to i f the e q u t i o n s g e t s l a r g e r than 100 219 Bool = False 220 221 i f X == None : 222 X = 0 223 i f Y == None : 224 Y = 0 225 i f R == None : 226 R = 0 227 228 # Kiwi d r i v e e q u t i o n s 229 s e l f . w1 = (−1/2∗X) + ( math . s q r t ( 3 ) /2∗Y) + ( r a d i u s ∗R) 230 s e l f . w2 = (−1/2∗X) − ( math . s q r t ( 3 ) /2∗Y) + ( r a d i u s ∗R) 231 s e l f . w3 = X + ( r a d i u s ∗R) 232

APPENDIX A. PYTHON CODE FOR RASPBERRY PI 234 i f s e l f . w1 > 100: 235 Bool = True 236 i f s e l f . w1 < −100: 237 Bool = True 238 239 i f s e l f . w2 > 100: 240 Bool = True 241 i f s e l f . w2 < −100: 242 Bool = True 243 244 i f s e l f . w3 > 100: 245 Bool = True 246 i f s e l f . w3 < −100: 247 Bool = True 248 249 i f Bool == True : 250 w1 = s e l f . w1 251 w2 = s e l f . w2 252 w3 = s e l f . w3 253 254 w1 = abs(w1) 255 w2 = abs(w2) 256 w3 = abs(w3) 257

258 #One i s b i g g e r than abs (100) 259 W = [ w1 , w2 , w3 ]

260 Wsort = np . s o r t (W) # S o r t i n g W 261

262 b i g g e s t = W. pop ( ) 263

264 # Making sure the b i g g e s t wheel g e t s max o f 100 PWM 265 f a c t o r = 100/ b i g g e s t 266 267 s e l f . w1 = ((−1/2∗X) − ( math . s q r t ( 3 ) /2∗Y) + ( r a d i u s ∗R) ) ∗ f a c t o r 268 s e l f . w2 = ((−1/2∗X) + ( math . s q r t ( 3 ) /2∗Y) + ( r a d i u s ∗R) ) ∗ f a c t o r 269 s e l f . w3 = (X + ( r a d i u s ∗R) ) ∗ f a c t o r 270

271 # I f anything went wrong , f o r exemple i f one wheel turned out to be

100.0001 272 273 i f s e l f . w1 > 100: 274 s e l f . w1 = 100 275 276 i f s e l f . w1 < −100: 277 s e l f . w1 = −100 278 279 i f s e l f . w2 > 100: 280 s e l f . w2 = 100 281 282 i f s e l f . w2 < −100: 283 s e l f . w2 = −100 284 285 i f s e l f . w3 > 100: 286 s e l f . w3 = 100 30

287 288 i f s e l f . w3 < −100: 289 s e l f . w3 = −100 290 291 return s e l f . w1 , s e l f . w2 , s e l f . w3 292 293 # Y f u l l , p o s i t i v e or n e g a t i v e d i r e c t i o n . D i r e c t i o n 1 i s s e t as d e f a u l t . 294 def driveY ( s e l f , d i r e c t i o n = 1) : 295 296 s e l f . wheels ( 0 , d i r e c t i o n , 0 ) 297 return s e l f . w1 , s e l f . w2 , s e l f . w3 298 299 # X f u l l , p o s i t i v e or n e g a t i v e d i r e c t i o n . D i r e c t i o n 1 i s s e t as d e f a u l t . 300 def driveX ( s e l f , d i r e c t i o n = 1) : 301 302 s e l f . wheels ( d i r e c t i o n , 0 , 0 ) 303 return s e l f . w1 , s e l f . w2 , s e l f . w3 304 305 # R f u l l , p o s i t i v e or n e g a t i v e d i r e c t i o n . D i r e c t i o n 1 i s s e t as d e f a u l t . 306 def r o t a t e ( s e l f , d i r e c t i o n = 1) : 307 308 s e l f . wheels ( 0 , 0 , d i r e c t i o n ) 309 return s e l f . w1 , s e l f . w2 , s e l f . w3 310 311

312 #Movement f u n c t i o n . Stop the robot i s s e t as d e f a u l t .

313 def g e n e r a l ( s e l f , d i r e c t i o n X = 0 , d i r e c t i o n Y = 0 , rotateMov = 0) : 314

315 s e l f . wheels ( directionX , directionY , rotateMov ) 316 return s e l f . w1 , s e l f . w2 , s e l f . w3

317

318 # This i s the method that d r i v e s the motors . I t uses c a l c u t e d v a l u e s

from the wheels f u n c t i o n as input

319 def motorDrive ( s e l f , motor1 , motor2 , motor3 , ramp = True ) : 320 #motorXabs i s the d e s i r e d v e l o c i t e s o f each motor

321 motor1abs = abs( motor1 ) 322 motor2abs = abs( motor2 ) 323 motor3abs = abs( motor3 ) 324

325 # Stoping motor and r e s e t i n g pins b e f o r e a s s i g n i n g new ones 326 s e l f . motorStop ( )

327

328 # The f o l l o w i n g chooses which pins to use depending on what

d i r e c t i o n each motor should r o t a t e

329 i f motor1 <= 0 : 330 #Negative value 331 m1 = s e l f .pwm1L # S e t t i n g high to l e f t d i r e c t i o n 332 e l i f motor1 > 0 : 333 m1 = s e l f .pwm1R # S e t t i n g high to r i g h t d i r e c t i o n 334 335 i f motor2 <= 0 :

APPENDIX A. PYTHON CODE FOR RASPBERRY PI 336 #Negative value 337 m2 = s e l f .pwm2L # S e t t i n g high to l e f t d i r e c t i o n 338 e l i f motor2 > 0 : 339 m2 = s e l f .pwm2R # S e t t i n g high to r i g h t d i r e c t i o n 340 341 i f motor3 <= 0 : 342 #Negative value 343 m3 = s e l f .pwm3L # S e t t i n g high to l e f t d i r e c t i o n 344 e l i f motor3 > 0 : 345 m3 = s e l f .pwm3R # S e t t i n g high to r i g h t d i r e c t i o n 346

347 #S t a r t i n g the PWM s i g n a l s on each motor 348 #Motor 1 349 dc=0 350 # S t a r t i n g PWM with 0% duty c y c l e 351 m1. s t a r t ( dc ) 352 # Motor 2 353 m2. s t a r t ( dc ) 354 # Motor 3 355 m3. s t a r t ( dc ) 356

357 # S e t t i n g duty c y c l e to each c a l c u l a t e d value f o r the motors 358 m1. ChangeDutyCycle ( motor1abs )

359 m2. ChangeDutyCycle ( motor2abs ) 360 m3. ChangeDutyCycle ( motor3abs ) 361

362 #f u n c t i o n to stop the motors by s e t t i n g Dutycyle to 0 363 def motorStop ( s e l f , option = 1) :

364 365 i f option == 1 : 366 s e l f .pwm1L. stop ( ) 367 s e l f .pwm1R. stop ( ) 368 s e l f .pwm2L. stop ( ) 369 s e l f .pwm2R. stop ( ) 370 s e l f .pwm3L. stop ( ) 371 s e l f .pwm3R. stop ( ) 372

373 # Option to a l l o w s f o r ramping down 374 i f option == 2 :

375

376 f o r dc in range(95 , 0 , −5) : # Loop 95 to 5 stepping dc down by

5 each loop

377 pwm. ChangeDutyCycle ( dc ) 378 pwm. ChangeDutyCycle ( dc ) 379 time . s l e e p ( 0 . 0 5 )

380

381 # Method to get the d i s t a n c e in cm from a s p e c i f i c u l t r a s o n i c s e n s o r 382 def USDistance ( s e l f , s e n s o r ) :

383

384 # Pin i s being chosen here depending on input . 385 # Each U l t r a s o n i c s e n s o r f u n c t i o n in the same way 386 i f s e n s o r == ” back ”:

387 Trig = s e l f . UPBTrig 388 Echo = s e l f . UPBEcho

389 390 e l i f s e n s o r == ” r i g h t ”: 391 Trig = s e l f . UPRTrig 392 Echo = s e l f . UPREcho 393 394 e l i f s e n s o r == ” l e f t ”: 395 Trig = s e l f . UPLTrig 396 Echo = s e l f . UPLEcho 397 398 e l s e: 399 return False 400

401 #Trigger pin to High 402 GPIO. output ( Trig , True ) 403

404 #Trigger Low

405 time . s l e e p ( 0 . 0 0 0 0 1 ) 406 GPIO. output ( Trig , False ) 407

408 StartTime = time . time ( ) 409 StopTime = time . time ( ) 410

411 #StartTime

412 # This part checks f o r a e r r o r that occurs sometimes i f the Echo

s e n s o r does not f i n d the t r i g g e r output

413 t e s t t i m e = time . time ( ) 414 while GPIO.input( Echo ) == 0 : 415 StartTime = time . time ( )

416 t e s t E l a p s e d = StartTime−t e s t t i m e 417 i f t e s t E l a p s e d > 1 :

418 break

419

420 #C a l c u l a t i n g d i s t a n c e 421 while GPIO.input( Echo ) == 1 : 422 StopTime = time . time ( ) 423

424 # e l a p s e d time

425 TimeElapsed = StopTime − StartTime 426

427 # Getting the d i s t a n c e according to c a l c u t e d value . 428 # The speed o f sound i s used here as v .

429 d i s t a n c e u s = ( TimeElapsed ∗ 34300) / 2 430

431 return d i s t a n c e u s 432

433

434 # Getting degree from compass module connected via I2C p r o t o c o l 435 def getDegree ( s e l f ) :

436 # This g a t h e r s informatio n from the I2C p r o t o c o l and turns t h i s

i n t o a value

437 # between 0 and 360. The input from the I2C i s a compass module

connected to the Raspberry Pi .

438 degree1 = s e l f . bus . r e a d b y t e d a t a ( s e l f . address , 2) 439 degree2 = s e l f . bus . r e a d b y t e d a t a ( s e l f . address , 3)

APPENDIX A. PYTHON CODE FOR RASPBERRY PI 440

441 # C a l c u l a t i n g degree from I2c input 442 bearing = ( degree1 << 8) + degree2 443 bearing = bearing /10.0

444 return bearing

445 446

447 #This f u n c t i o n reads v a l u e s from f i l e . This i s used

448 # f o r s e t t i n g the c a l i b r a t e d v a l u e s f o r the image r e c o g n i t i o n . 449 def readCalibratedImage ( s e l f , f i l e n a m e ) : 450 451 # Opening f i l e 452 f i l e = open( filename , ” r ”) 453 454 # Reading a l l o f the f i l e v a l u e s 455 r e a d F i l e V a l u e s = f i l e. read ( ) 456 457 # Removing \n 458 f i l e V a l u e s = r e a d F i l e V a l u e s . s p l i t ( ) 459 460 # I n i t a l i z i n g a v e c t o r 461 v a l u e s = [ ] 462 # S t o r i n g a l l o f the c a l i b r a t r e d v a l u e s in an array 463 f o r value in f i l e V a l u e s :

464 # Input value are s t r i n g s . Turing thee i n t o i n t s and appending

i n t o array .

465 vInt = i n t( value ) 466 v a l u e s . append ( vInt ) 467

468 # S e t t i n g the parameters f o r the image r e c o g n i t i o n program 469 s e l f . HUE L = v a l u e s [ 0 ] 470 s e l f .HUE H = v a l u e s [ 1 ] 471 s e l f . SL = v a l u e s [ 2 ] 472 s e l f .SH = v a l u e s [ 3 ] 473 s e l f .VL = v a l u e s [ 4 ] 474 s e l f .VH = v a l u e s [ 5 ] 475 s e l f . Xcal = v a l u e s [ 6 ] 476 s e l f . Ycal = v a l u e s [ 7 ] 477 s e l f . parameter1cal = v a l u e s [ 8 ] 478 s e l f . parameter2cal = v a l u e s [ 9 ] 479 480 # Closing f i l e 481 f i l e. c l o s e ( ) 482 483 def back ( s e l f ) : 484 # S e t t i n g backDistance 485 s e l f . backDistance = 25 486 487 488################## PONG CODE ######################

489# This part o f the code uses the robot c l a s s and uses f u n c t i o n s that

simulate a game o f pong .

490# When main ( ) runs , the robot s t a r t s to f o l l o w the b a l l and i n t e r c e p t s

i t when c l o s e .

491# The robot avoids c o l l i d i n g with w a l l s and uses a PID c o n t r o l l e r to

stay on a s t r a i g h t l i n e .

492

493# Drive to b a l l i s run from main ( ) when the b a l l g e t s c l o s e to the

robot .

494# I t uses the PID o b j e c t s to keep track o f the e r r o r s and the robot

o b j e c t to send

495# the robot through the code .

496# The f u n c t i o n makes the robot d r i v e to the b a l l and then back . I t

outputs the updated o b j e c t s and d i s t a n c e s .

497 def driveToBall ( argv , robot , PIDcontroler , PIDcamera , PIDsensor ,

distanceToCenterpoint , distanceToBall ) :

498

499 # Tracking time

500 p r i n t(” Drive to b a l l ”) 501 startTime = time . time ( ) 502 urobot = 0

503

504 # Close enough value to back wall 505 closeEnough = 40

506

507 # I n i t v a l u e s f o r p o s i t i o n , in order to get back to t h i s p o s i t i o n

a f t e r d r i v i n g

508 InitBack = robot . USDistance (” back ”) 509 I n i t R i g h t = robot . USDistance (” r i g h t ”) 510 I n i t L e f t = robot . USDistance (” l e f t ”) 511 512 513 #C a l c u l a t e v e c t o r to b a l l 514 x = distanceToCenterpoint 515 y = distanceToBall 516 517 r i g h t D i s t a n c e = I n i t R i g h t 518 l e f t D i s t a n c e = I n i t L e f t 519 520 # Get d i s t a n c e to backwall

521 backDistance = robot . USDistance (” back ”) # Function 522

523 524

525 # While loop to d r i v e to b a l l

526 #while backDistance < distanceToBall : 527 while True :

528 #Use v e c t o r to get wheel d r i v e s

529 #x , y = PIDcamera . regulateCamera ( argv , robot ) # Getting robot

v e c t o r s X and Y

530 urobot = PIDcontroler . r e g u l a t e ( robot ) 531

532 currentTime = time . time ( )

533 deltaTime = currentTime − startTime 534

535 # Checks so the robot does not d r i v e f o r a long time 536 i f deltaTime > 1 :

APPENDIX A. PYTHON CODE FOR RASPBERRY PI 538

539 i f r i g h t D i s t a n c e < 1 0 : 540

541 # I f the r i g h t wall i s too c l o s e , the robot d r i v e s away from i t 542 wh1 , wh2 , wh3 = robot . wheels (−80 ,0 , urobot )

543 robot . motorDrive (wh1 , wh2 , wh3) 544 time . s l e e p ( 0 . 1 ) 545 546 547 e l i f l e f t D i s t a n c e < 1 0 : 548

549 # I f the l e f t wall i s too c l o s e , the robot d r i v e s away from i t . 550 wh1 , wh2 , wh3 = robot . wheels ( 8 0 , 0 , urobot )

551 robot . motorDrive (wh1 , wh2 , wh3) 552 time . s l e e p ( 0 . 1 ) 553 554 555 e l i f backDistance < 1 0 : 556

557 # I f the back wall i s too c l o s e , the robot d r i v e s away from i t . 558 wh1 , wh2 , wh3 = robot . wheels ( 0 , 8 0 , urobot )

559 robot . motorDrive (wh1 , wh2 , wh3) 560 time . s l e e p ( 0 . 1 )

561

562 e l s e: 563

564 # I f a l l o f the u l t r a s o n i c s e n s o r s does not d e t e c t any c l o s e

o b s t a c t l e , the f o l l o w i n g code i s executed

565

566 # Drive to b a l l

567 wh1 , wh2 , wh3 = robot . wheels ( 0 , 8 0 , urobot ) 568 # Output to motors

569 robot . motorDrive (wh1 , wh2 , wh3) 570

571 # PID compass

572 urobot = PIDcontroler . r e g u l a t e ( robot ) 573

574

575 #####################Drive back ################### 576 p r i n t(” Drive back ”)

577 #Checks d i s t a n c e to w a l l s

578 backDistance = robot . USDistance (” back ”) 579 r i g h t D i s t a n c e = robot . USDistance (” r i g h t ”) 580 l e f t D i s t a n c e = robot . USDistance (” l e f t ”) 581

582 # C a l c u l a t e d i s t a n c e v e c t o r s 583

584 d i f f = backDistance−robot . backDistance 585 secTime = time . time ( )

586 # While the robot i s f a r away from i n i t i a l t a r g e t 587 uY = PIDsensor . regulateUS ( robot )

588 while uY> 5 :

589 backDistance = robot . USDistance (” back ”) 590 r i g h t D i s t a n c e = robot . USDistance (” r i g h t ”)

591 l e f t D i s t a n c e = robot . USDistance (” l e f t ”) 592 593 c u r r e n t t i m e = time . time ( ) 594 deltaTime = c u r r e n t t i m e − secTime 595 596 i f r i g h t D i s t a n c e < 1 0 : 597

598 # I f the r i g h t wall i s too c l o s e , the robot d r i v e s away from i t 599 wh1 , wh2 , wh3 = robot . wheels (−80 ,0 , urobot )

600 robot . motorDrive (wh1 , wh2 , wh3) 601 time . s l e e p ( 0 . 1 )

602

603 e l i f l e f t D i s t a n c e < 1 0 :

604 # I f the l e f t wall i s too c l o s e , the robot d r i v e s away from i t . 605 wh1 , wh2 , wh3 = robot . wheels ( 8 0 , 0 , urobot )

606 robot . motorDrive (wh1 , wh2 , wh3) 607 time . s l e e p ( 0 . 1 )

608

609 e l i f backDistance < 1 0 :

610 # I f the back wall i s too c l o s e , the robot d r i v e s away from i t . 611 wh1 , wh2 , wh3 = robot . wheels (0 ,−80 , urobot )

612 robot . motorDrive (wh1 , wh2 , wh3) 613 time . s l e e p ( 0 . 1 )

614

615 # This loop w i l l only run f o r a maximum o f 1 seconds 616 e l i f deltaTime > 1 :

617 break

618

619 e l s e:

620 # I f a l l o f the u l t r a s o n i c s e n s o r s does not d e t e c t any c l o s e

o b s t a c t l e , the f o l l o w i n g code i s executed

621

622 uY = PIDsensor . regulateUS ( robot ) 623

624 # E s s e n t i a l l y a P−c o n t r o l l e r to d r i v e back to c e n t e r 625 # Which d i r e c t i o n should the robot move in ?

626 i f r i g h t D i s t a n c e > l e f t D i s t a n c e :

627 X = 80 # I f the r i g h t wall i s f u r t h e r away , d r i v e to the r i g h t 628 X = X∗( r i g h t D i s t a n c e−l e f t D i s t a n c e ) 629 e l s e: 630 X = −80 631 X = X∗( l e f t D i s t a n c e−r i g h t D i s t a n c e ) 632 633 Y = abs(X∗2) 634 635 # Drive to b a l l

636 wh1 , wh2 , wh3 = robot . wheels (0 ,uY, urobot ) 637

638 robot . motorDrive (wh1 , wh2 , wh3) 639

640 urobot = PIDcontroler . r e g u l a t e ( robot ) 641

642 d i f f = backDistance−robot . backDistance 643

APPENDIX A. PYTHON CODE FOR RASPBERRY PI 644 # After t h i s loop the robot should be back at the c e n t e r p o s i t i o n 645

646 secTime = time . time ( ) 647

648 vX, y = PIDcamera . regulateCamera ( argv , robot ) # Getting robot v e c t o r s

X and Y

649 urobot = PIDcontroler . r e g u l a t e ( robot ) 650 return vX, urobot

651 652

653# The PID c l a s s i s used as d i f f e r e n t o b j e c t s depending on which s e n s o r

i s used .

654 c l a s s PID :

655 def i n i t ( s e l f ) : 656

657 #Values f o r K p , K i and K d f o r compass , USsensor and camera 658

659 # These v a l u e s are changed when the program reads s t o r e d v a l u e s

from f i l e l a t e r in the program .

660 s e l f . KPcomp = 2.25 661 s e l f . KIcomp = 0 662 s e l f .KDcomp = 0 663 664 s e l f . KPcamera = 1 665 s e l f . KIcamera = 0 666 s e l f . KDcamera = 0.075 667 668 s e l f .KpUS = 30 669 s e l f . KiUS = 1 . 1 670 s e l f .KdUS = 0.06 671

672 # Following v a l u e s are used f o r i n i t i a l z i n g PID c a l c u l a t i o n s 673 s e l f . prevErrorU = 0 674 s e l f . errorSumU = 0 675 676 s e l f . sampletime = 0.01 # Ts 677 678 #I n i t i a l i z i n g necceceary v a l u e s to 0 679 s e l f . prevError = 0 680 s e l f . errorSum = 0 681 682 s e l f . s e t p o i n t = 0 683 s e l f . X Latest = 0 684 685 s e l f . prevErrorcamera = 0 686 s e l f . cameraTime = 0 687 688 s e l f . c u r r e n t t i m e =time . time ( ) 689 s e l f . l a s t t i m e = s e l f . c u r r e n t t i m e 690 691 692 # P l o t t i n g e r r o r 693 s e l f . e r r o r L i s t = [ ]

694 s e l f . plotTime = time . time ( ) 38

695 s e l f . t i m e L i s t = [ ] 696 s e l f . t i t l e = ”” 697 s e l f . count = 1

698 s e l f . dateT = datetime . datetime . now ( ) 699

700 #Sets the f i r s t read value from the compass as r e f e r e n c e value 701 def g e t S e t p o i n t ( s e l f , robot ) :

702 s e l f . s e t p o i n t = robot . getDegree ( ) 703 return s e l f . s e t p o i n t

704

705 # The r e g u l a t e f u n c t i o n r e g u l a t e s r o t a t i o n a l e r r o r r e c i v e d from the

compass module

706 def r e g u l a t e ( s e l f , robot ) : 707 # Getting degree and e r r o r 708 degreeRobot = robot . getDegree ( )

709 degreeError = s e l f . s e t p o i n t − degreeRobot #c a l c u l a t e the

e r r o r

710

711 e r r o r = −1∗ degreeError

712 # Used to l a t e r p l o t the e r r o r i s chosen to .

713 s e l f . p l o t E r r o r ( error ,” Error o f PID from compass ”, show=False ) 714

715 d i f f = e r r o r − s e l f . prevError 716

717 # I f the compass makes a mistake 718 i f d i f f > 130:

719 e r r o r = 0

720 # P o s s i b l e to put in something l i k e in the camera r e g u l a t o r here ,

to a d j u s t f o r s u r r o i n d f i e l d s

721

722 # Getting the change in time s i n c e l a s t c a l c u l a t i o n 723 deltaTime = s e l f . c u r r e n t t i m e − s e l f . l a s t t i m e 724

725 # This i s f o r the f i r s t step 726 i f deltaTime == 0 : 727 deltaTime = s e l f . sampletime 728 729 d i f f a b s = abs( d i f f ) 730 731 # S t o r i n g errorsum 732 s e l f . errorSum += e r r o r ∗ deltaTime 733 734 # Windup guard 735 i f s e l f . errorSum > 2 0 : 736 s e l f . errorSum = 20 737 i f s e l f . errorSum < −20: 738 s e l f . errorSum = 2