IN DEGREE PROJECT TECHNOLOGY,FIRST CYCLE, 15 CREDITS

, STOCKHOLM SWEDEN 2016

Human controlled robotic armImproving usability with haptic feedback

SIMON AHLIN HÖGFELDT

DANIEL SÖDERMAN

KTH ROYAL INSTITUTE OF TECHNOLOGYSCHOOL OF INDUSTRIAL ENGINEERING AND MANAGEMENT

Human controlled robotic arm

SIMON AHLIN HÖGFELDTDANIEL SÖDERMAN

Bacherlor’s Thesis in Mechatronics

Supervisor: Fariba RahimiExaminer: Martin Edin GrimhedenApproved: 2016-06-07

TRITA MMKB 2016:25 MDAB086

AbstractIn our modern society, the usage of robotic arms are increasing. Much of thework in the industry is now done by robots. Even though they are able to dovery precise work, difficulties appear when trying to do some of the tasks thathumans do. This can be changed by making it easy for a human to control therobotic arm and to "teach it" how it’s done.

The purpose of this project is to develop a robotic arm that is easily con-trolled by the user. This is done by using the users own arm movement tocontrol the robot. To make the usage more intuitive, a simple haptic feedbacksystem will be implemented. This creates a greater experience where the useris able to "feel what the robot feels". To be able to create such a system,development of an easy control unit, robotic arm and feedback system has tobe made. The steering of the robotic arm is created from reading the user’sarm movement with potentiometers, and mapping these values to servos onthe robotic arm. Sensors on the robotic claw and on the user’s hand makes itpossible for a DC motor to create a counter torque that gives a feel for the sizeof the object being lifted.

The feedback system seemed to improve the overall experience of using arobotic arm and with further work, today’s robotic industry could be improved.

iii

SammanfattningRobotarm styrd av människans arm

Allt mer arbete inom industrin utförs av robotarmar. Fördelen är att de klararav att utföra arbeten med hög precision. Däremot finns det problematik dårobotar ska utföra vissa specifika arbeten som människor utför. Genom attpå ett enkelt sätt kunna styra en robotarm med sin egen armrörelse skulleanvändaren enkelt kunna lära roboten att göra de rörelser som annars varitsvåra att programmera för dess rörelsemönster.

Syftet med detta projekt är att utveckla en robotarm som underlättar sty-randet. För att göra detta till en mer naturlig upplevelse, implemeteras ett feed-backsystem som gör att användare i viss mån kan "känna vad roboten känner".För att kunna skapa ett sådant system behövdes en robotarm, en styrenhetoch ett feedbacksystem utvecklas. Robotarmen styrs genom att potentiomet-rar registrerar användarens rörelse. Dessa värden från potentiometrarna skickassedan till servomotorer som styr robotarmen. Kraftsensorer på robotklon ochanvändarens egen hand tillåter oss att styra en DC motor som kan tillföra ettvisst moment till användarens hand. Detta gör att man kan känna ungefärligstorlek på det objekt som robotarmen greppar.

Feedbacksystemet verkade förbättra användandet av robotarmen. Med fram-tida utveckling av detta projekt skulle framtidens robotindustri kunna förbätt-ras avsevärt.

v

Preface

We would like to thank our class mates for all the helpful and valuable discussions.Staffan Qvarnström provided us with the electronic parts needed. Tomas Östbergfor providing us with the tools needed for the construction. Fariba Rahimi for helpwith the project and PID control.

Daniel Söderman, Simon Ahlin Högfeldt,Stockholm, May, 2016

vii

Contents

Abstract iii

Sammanfattning v

Preface vii

Contents ix

Nomenclature xi

1 Introduction 11.1 Background . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11.2 Purpose . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21.3 Scope . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21.4 Method . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3

1.4.1 Haptic feedback . . . . . . . . . . . . . . . . . . . . . . . . . 31.4.2 Robotic arm . . . . . . . . . . . . . . . . . . . . . . . . . . . 31.4.3 Arm unit . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41.4.4 Measuring the real step response . . . . . . . . . . . . . . . . 41.4.5 Application testing . . . . . . . . . . . . . . . . . . . . . . . . 4

2 Theory 52.1 Past Research . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52.2 PID-control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5

3 Demonstrator 73.1 Problem formulation . . . . . . . . . . . . . . . . . . . . . . . . . . . 73.2 Hardware . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8

3.2.1 Chosen parts . . . . . . . . . . . . . . . . . . . . . . . . . . . 83.2.2 Robotic arm . . . . . . . . . . . . . . . . . . . . . . . . . . . 83.2.3 Arm unit . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11

3.3 Electronics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 143.4 Software . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16

3.4.1 PID . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16

ix

3.5 Measuring step response . . . . . . . . . . . . . . . . . . . . . . . . . 173.6 Application testing . . . . . . . . . . . . . . . . . . . . . . . . . . . . 193.7 Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19

3.7.1 PID . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 203.7.2 Application testing . . . . . . . . . . . . . . . . . . . . . . . . 22

4 Discussion and conclusions 234.1 Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 234.2 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25

5 Recommendations and future work 275.1 Recommendations . . . . . . . . . . . . . . . . . . . . . . . . . . . . 275.2 Future work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27

Bibliography 29

Appendices

A Datasheets 31

B Schematics 37

C Arduino Code 39

D Matlab Code 49

E Plots 57

x

Nomenclature

SymbolsSymbols Description

P Proportional part parameterI Integral part parameterD Derivative part parameterJ Rotor inertia [kg·m2]b Motor viscous friction constant [N ·m·s]K Torque constant [Nm/A]R Electric resistance [Ω]L Electric inductance [H]Gmotor Transfer function for DC motorF Transfer function for controller

AbbreviationsAbbreviation Description

FSR Force Sensitive ResistorPID controller Proportional–Integral–Derivative controllerDC motor Direct Current motorCAD Computer Aided DesignGND Ground

xi

Chapter 1

Introduction

1.1 BackgroundIn our modern society, the development of technological solutions has been in greatfocus. Robotic arms has revolutionized the industry by making it more cost ef-ficient, improving build quality, decreasing production time.[Keystone ElectronicsCorp, 2013] In the future, robotic arms will be even more important in manufactur-ing.[HAGERTY, 2015] At BMW there are robots doing the big jobs of fastening thebody panels and doing the welding. However what remains is to use those robots onthe assembly line where workers assemble the more complex parts.[IndustryWeek,2012] Robotic arms are improving and the precision required for assembly may beachieved. But to control the movement of the robotic arm, an engineer is oftenrequired to program its software. A way to make robotic arms easier to control isto allow the robotic arm to be controlled by the user’s own arm.

You could almost say that we have mastered the technique of controlling ourown arm movement. Because a robotic arm’s construction is similar to the humanarm, it makes sense for the user to control it with her own arm. This would makeit easy to understand its movement limitations.

When you move your own arm you get feedback as to what the arm is experi-encing. Without some type of feedback the feeling of controlling the robotic arm isdiminished. For full control of the robotic arm some type of force feedback wouldbe helpful.

Research into robotic arms with feedback can often be seen in the surgical in-dustry. However these are often built with a narrow purpose in mind and are alsovery expensive since surgical precision is needed.[Okamura, 2009] This project ex-plores cheaper and simpler options that don’t have to follow the same standard asthe medical industry.

1

CHAPTER 1. INTRODUCTION

1.2 PurposeThe purpose of this project is to improve the application of a standard roboticarm by implementing a control of the robotic arm with an arm unit that followsyour arm’s movement. Simple haptic feedback will be implemented that transmitsthe force that the robotic arm is feeling back to your own hand that is connectedto the arm unit. It should be possible for a person without any knowledge aboutrobotic control to be able to control the robotic arm’s movement. This means thatthe controller should be made in an intuitive way i.e controlling it with your ownarm. The haptic feedback will be constructed using a force sensor on the roboticarm’s claw and one on the arm unit. By implementing a motor on the arm unit,the force that the user feels in their hand can be altered to be equal to the forcethat is measured at the gripping claw. This requires an optimized PID controller.The system leads to the interesting question on how the real system differs fromthe theoretical calculations. Therefore, comparison of the step response for the realsystem and the one in theory will be done.

The research questions of this study are:

• "Which PID control parameters fits better to the designed system with feed-back?"

• "How does the feedback system differ to the simulated model?"• "How will the implemented haptic feedback affect the application of the robotic

arm?"

1.3 ScopeThis report covers a semester work for a Bachelors thesis at KTH with limited timeand resources. Due to this, there must be some limitations to the construction.Focus will be on the haptic feedback and its functionality. The construction of therobotic arm and arm unit will therefore be as simplified as possible. The main goalfor the robotic arm is to be able to grip an object and move it. The weight of theobject and the distance is not important as this does not affect the main purposeof this project. Limitations of the robotic arm and arm unit is set to three degreesof freedom as shown in Figure 1.1. The feedback system will affect the whole handand will not be different for the separate fingers. This means that the robotic clawwill be a single flat surface, corresponding to a human hand with all their fingersclosed together. The robotic arm will only be able to grab objects with simplerectangular shapes. When calculating the transfer function for the PID controller,some frictions will be neglected. Due to the fact that the purpose of the units arefor scientific testing, the appearance and practical use is not in focus.

2

1.4. METHOD

Figure 1.1. Sketch showing the degrees of freedom. a) Side view where upwardsrotation is shown. b) View from above where the opening and closing of the claw aswell as the horizontal rotation of the base is shown.

1.4 Method

1.4.1 Haptic feedbackThe main goal of the design is that the user should be able to feel when the roboticarm grabs an object. A general feel of the object size should also be possible. Forthis, the robotic arm should be able to register the normal force from the objectbeing grabbed. The same force should occur on the user’s hand. The arm unittherefore needs to register the force between the user’s hand and the unit. This isdone by using a FSR on the robotic arm’s claw and one FSR on the arm unit. Forthe user to be able to feel the same force as the robotic arm, the signal from eachFSR should be the same.

1.4.2 Robotic armThe robotic arm is constructed with some simplifications. Most of its parts are 3Dprinted using a Ultimaker,[Ultimaker BV, 2016]. The less complex parts are createdin wood using a laser printer Epson,[Epilog Laser, 2016]. For reducing the errorsduring the construction, most parts has been modeled using Solidedge,[SiemensPLM Software, 2016]. To make the controlling of the arm as intuitive as possible,the robotic arm should have the same limitations as a human arm, i.e the samedegrees of freedom. The movement of the robotic arm is created using three servos,one for each degree of freedom, see Figure 1.1. The claw will be built as plates,therefore it will be hard to grab circular objects.

3

CHAPTER 1. INTRODUCTION

1.4.3 Arm unitBecause of its simple design, the arm unit is mostly created in wood using a laserprinter Epson,[Epilog Laser, 2016]. The movement is registered using three poten-tiometers, one for each degree of freedom. These should correspond to the samedegrees as on the robotic arm, see Figure 1.1. Using the signal from the poten-tiometers, the angles can be calculated. Knowing this angle, the same angle can besent to the servo on the robotic arm. A DC motor is used to create the countertorque needed on the user’s hand. This torque makes the user feel a force, thus thevalue of the FSR will increase. This enables us to create a simple feedback system,where the signal to the motor is controlled to give different torques depending onhow hard the robotic arm is grabbing an object.

1.4.4 Measuring the real step responseTo get a measurement of the real step response, a real test was made. The testwas conducted by having the user’s hand connected to the arm unit. The roboticarm was then given a predefined force. The feedback system will then try to getthe value of the FSR on the arm unit to be the same as on the robotic arm. Thisis done via the DC motor mounted on the arm unit. By saving the values of thearm unit’s FSR during this procedure, the step response of the real system couldbe plotted.

1.4.5 Application testingAn interesting aspect of the haptic feedback system is how it affects the applicationfor the user. To answer this question, a small amount of test subjects did multiplepredefined tasks and then answered some questions about the application with thefeedback system.

4

Chapter 2

Theory

2.1 Past ResearchThere have been projects where the robotic arm is controlled by the human arm.One uses accelometers to register the movement of the human arm. [PATTNAIK,2013]. Another solution is to use gyroscopes and bendable potentiometers[bluebean,2012]. These methods are unnecessarily complex where some sort of filter is needed.The signal from a potentiometer is stable and will provide for easier control withless fluctuations. Therefore simple potentiometers will be used for registering themovement of the user.

2.2 PID-controlFor a system to be able to read a reference value and then do something to makethe system get to that value, the system needs to continuously calculate an errorvalue. This is the difference between the desired value and the measured value. Thesystem will then try to minimize this error over time by adjusting the system. ThePID controller is made up of three parts. The proportional (P) part corresponds tothe gain of the controller. If this part is increased the system gets a faster responseat the cost of stability. Using only a P-controller has some drawbacks. The errorwill always be greater than zero. For a fast system (where the proportional constantvalue is high) the system will get large overshoots. Overshoot is when the systemexceeds its final steady-state value. To overcome this problem, an integral part (I)needs to be implemented. Increasing the I part yields a smaller steady state errorbut once again at the cost of reduced stability. Adding a derivative part (D) part isused for better stability. An increased D gives greater stability. [Glad and Ljung,2006] To apply this controller to a DC motor, the transfer function from voltage tospeed in the laplace domain must be known. This is received from [Univeristy ofMichigan, 2016]

5

CHAPTER 2. THEORY

Gmotor = K

((J ·s + b)·(L·s + R) + K2) . (2.1)

Equation 2.1 does not consider friction forces. It only consider the DC motor.The system is now mathematically expressed and can be simulated using MATLABsimulink,[MathWorks Nordic, 2015b]. This makes the closed loop system

F ·Gmotor

1 + F ·Gmotor. (2.2)

Where F is the PI or PID controller in laplace domain. The PI controller is writtenin laplace domain as

F = Ps + I

s. (2.3)

The PID controller can be written as

F = Ps + Ds2 + I

s. (2.4)

For tuning the controller, pole placement is used. This is done by placing thepoles of the denominator of (2.2) and solving for the P,I, and D part. The closedloop system will be of second order and have two poles. Which gives the followingequation for the P,I,and D parameters

(s + pole)2 = 1 + F ·Gmotor. (2.5)

Note that one or more of the parameters P, I and D can be set to zero. Thisgives different controllers, such as P, PI, PD and PID. Using the step response, theparameters of the controller can be tuned to give a desirable behavior.

6

Chapter 3

Demonstrator

3.1 Problem formulationThe robotic system can be divided into two major parts, the arm unit and therobotic arm. The arm unit will read the human arm movement. The robotic armis the robot itself, which will do the movement given by the user. Each systemrequires a number of technical solutions. The requirements for the arm unit are:

• Easy to fasten user’s arm.• Intuitive to use.• Read the movement with potentiometers.• Read the normal force created from a motor.• Transfer torque from DC motor to user’s hand.

The requirements for the robotic arm are:

• Movement created by servos.• Create same movement as read by the arm unit.• Able to lift and move objects.• Read the normal force of grabbed object.

To be able to create the correct counter torque from the motor on the user’s hand,a PID controller needs to be implemented. The problem to be solved is to optimizethe values of the controller’s constants, but also to see what kind of controller isbest for our purpose. To be able to optimize the PID, a set of requirements needsto be decided for the system. The requirements for the PID controller are:

• Stable system.• Small to no overshoot.• Settling time less than 0.5 seconds.

7

CHAPTER 3. DEMONSTRATOR

3.2 Hardware

3.2.1 Chosen partsThese are the chosen parts for our prototype.

• 3 Servos (Parallax Standard Servo 900-00005)• 1 Motor (Maxon motor 110147 with gears 232766)• H-bridge (L298)• 2 FSR (Interlink 402)• 3 Potentiometers 10KΩ (PIHER PC16SH10IP06-103A2020–TA)

When deciding which servos to buy, small servos was preferred, while being strongenough for lifting objects. The robot should be able to lift 0.5 kilograms. The servo’sstrength is given in torque. To choose an appropriate servo, a simple calculation forthe required torque was made. The largest strain is for the lifting movement. Thedistance from the servos rotational center point to the end of the gripping claw isapproximately 0.35m. The weight of the arm and claw itself is approximately 0.2kgLifting a weight of 0.5kg would require a torque of approximately

(0.5 + 0.2)·0.35 = 0.245Nm. (3.1)

As mentioned above, the chosen servo should be able to create a torque ofapproximately 0.26 Nm.

For the haptic feedback a DC motor was chosen because it is easy to controlspeed and torque compared to servos or stepper motors. When choosing the motor,the strength and required speed was unknown. The feedback system was built andthe DC motor was chosen with trial and error. One important aspect was that themotor should not be too heavy. This would make the whole arm unit heavy andhard to use for the user. The motor mentioned above was both strong and fastenough to fulfill the requirements for the feedback system and was of a reasonablesize.

The H-bridge was chosen with simplicity in mind. The component had to beeasy to use, cheap and available. The purpose is to be able to control a DC motor.

The FSR needed to be cheap but reliable with good accuracy. The chosen FSRfulfilled these requirements. They were also easy to fastened.

Potentiometers were chosen to track the arm movement because they provide alogical connection with the angles of the servos. The resistance of 10KΩ is chosento provide a more stable signal.

3.2.2 Robotic armThe construction of the robotic arm uses a combination of 3D printing and lasercutting. The base is 3D printed to fit one servo. Its main purpose is to rotate the

8

3.2. HARDWARE

whole robotic arm. Wooden feet were laser cut and mounted to the base, see Figure3.1.

Figure 3.1. The base of the robotic arm. Servo is mounted and fastened with fourbolts.

The servo mounted in the base is connected to another housing. The torque istransferred via a connection to the servo arm. This is the part being horizontallyrotated, see Figure 3.2.

Figure 3.2. The rotational part of the robotic arm mounted on the base. Torque istransferred via a tight fitting shape around the servo arm.

The second servo takes care of the second degree of freedom, which is lifting thearm vertically. This servo is mounted in the mentioned housing, see Figure 3.3.On this part, two wooden arms are connected via an attachment part. This partis a 3D printed U-shaped part that connects the two wooden arms to the servo. Asimilar connection to the servo arm is implemented for transferring its torque.

9

CHAPTER 3. DEMONSTRATOR

Figure 3.3. The arm part is mounted via a 3D printed U-shape connector. Thisconnects to the servo and transfers the torque. The servo is fastened with four bolts.

The wooden arm parts are attached to a 3D printed claw connector. The thirdservo is fastened underneath this connector, see Figure 3.4. The claws are 3Dprinted with the gears designed to create movement in both claw parts with onlyone servo. For a claw with a bigger surface area, sheets of wood were laser cut asthe final piece of the claw for better grip. A FSR is mounted on one of the woodenpieces. This will sense the force on the robotic arm.

Figure 3.4. The claw connected to the servo. It is fastened to the servo with ascrew. The rotational movement transfers to the other claw via gears. The claws are3D printed. The gripping part is made out of wooden pieces. A FSR is mounted onone of the plates.

The complete robotic arm, see Figure 3.5, is a result of solutions for transferringthe torques from the three servos. It uses a total of three servos, one for each degreeof freedom, see Figure 3.6.

10

3.2. HARDWARE

Figure 3.5. The final version of the robotic arm

Figure 3.6. The movements of the robotic arm with three degrees of freedom

3.2.3 Arm unitThe arm unit is constructed by sheet wood which has been laser cut using CADdesigns. There are three major parts, as seen in Figure 3.7. One part for eachdegree of freedom with potentiometers paired with each servo.

11

CHAPTER 3. DEMONSTRATOR

Figure 3.7. An overview of the arm unit.

The function of the base is to follow the user’s horizontal rotation of the arm.This is registered by a potentiometer. Its shaft is mounted to the rotational plate.The potentiometer itself is fastened to another plate underneath the rotational plate,see Figure 3.8. For stability this part has circular stands attached.

Figure 3.8. The potentiometer for measuring the horizontal rotation is fastenedunderneath the rotational plate

The user’s arm is fastened to a long wooden piece that connects the rotationalbase to the hand part. This piece is connected to a potentiometer that reads therotation from lifting the hand, see Figure 3.9.

12

3.2. HARDWARE

Figure 3.9. The potentiometer is fastened via a bend metal piece. It registers therotation of the vertical movement of the arm.

The hand part is a complex system that was built as the project progressed. Thehand piece has two major functions. It measures how the user opens and closes herhand and transfers the torque produced by a mounted DC motor. The measuringof the user’s hand movement is done with a potentiometer, see Figure 3.10.

Figure 3.10. The potentiometer is fastened to the wooden piece at the bottom.This piece is fastened to a metal plate which is fastened to the rigid part of the handunit. A similar part is connected to the potentiometer shaft, but connected to theouter part of the hand unit.

The haptic feedback is constructed by fastening the DC motor to the rigid partof the hand. The motor shaft is coupled with the closing part of the hand. Thisis done by laser cutting a part that fits around the shaft to transfer torque. Hereacrylic glass, as shown in Figure 3.11, is used since sheet wood is too soft and will

13

CHAPTER 3. DEMONSTRATOR

not maintain the shape around the shaft when torque is applied. When the DCmotor applies a torque, the hand unit will open, thus giving a counter force to theuser that is trying to close her hand.

Figure 3.11. The feedback construction with acrylic glass.

A FSR is mounted to the hand unit, see Figure 3.12. This reads the force onthe user’s hand produced by a torque from the DC motor.

Figure 3.12. A FSR is fastened to the outer part of the hand unit. A finger of theuser is placed on the sensor. This registrered the force on the hand produced fromthe torque in the DC motor.

3.3 ElectronicsThe entire system is run by an Arduino Uno.[Arduino, 2016] This is a micro con-troller that is controlled by a given code. The system uses three smaller PCBs, see

14

3.3. ELECTRONICS

Figure 3.13. The purpose for this was to try and make the connections intuitive,where one PCB connects all the parts for the robotic arm and another for the armunit. GND and 5V from each PCB are then connected to a third PCB that containsa button and a LED. This card has two functions. It connects all the 5V and GND,which then goes to the Arduino Uno. The button and LED will give the user theoption to toggle the feedback system ON and OFF. When the LED is turned ON,the feedback system is ON. The button requires a pull-down resistor, in this case,10KΩ is used. For the LED a 330Ω resistor is used.

Analog potentiometers are used for reading the position of the users arm. Throughthe software, the angle is mapped and fed to the servo motors via the Arduino Uno.The servos are powered by an external source to provide enough power. Accordingto the datasheet for the servos, see Appendix A, maximum voltage was used, around9V.

To implement the PID control, two force sensitive sensors are used. To work,they need a pull-down resistor. A 10KΩ resistor was used for each sensor. Thereference value is received from the sensor on the robotic arm. The output value isreceived from the sensor on the hand unit. The PID controller regulates the PWMsignal to the DC motor which corresponds to a force at the output sensor. The DCmotor is connected via a H-bridge to the Arduino Uno. The DC motor needs a sep-arate voltage source. According to the datasheet for the DC motor, see AppendixA, the DC motor needs to be connected to 15V. For schematics of the system, seeappendix A.

Figure 3.13. Connection diagram for the system

15

CHAPTER 3. DEMONSTRATOR

3.4 SoftwareThe Arduino Uno reads the values from the potentiometers. These values aremapped so the angle of the servo corresponds to the same angle as the poten-tiometer. This means that the servos will follow the same angle movement as theinput from the potentiometers, see Figure 3.14. The values from the two FSRsare measured by the Arduino Uno. The value from the robotic arm is seen as thesetpoint signal while the value from the arm unit is the output. Using the PIDlibrary,[Beauregard, 2016] it calculates the required signal to the DC motor viathe H-bridge. The state of the button controls whether or not the PID State isON which turns the haptic feedback on and off. The LED is also turned on or offdepending on the PID state.

Figure 3.14. Program flowchart

3.4.1 PIDWhen calculating the step response for the feedback system, MATLAB Simulink isused. When inserting values for the DC motor given by the datasheet, see AppendixA, into Equation 2.1, we get the closed loop system as shown in Figure 3.15. Byinserting different values for the parameters P,I and D, the different step response

16

3.5. MEASURING STEP RESPONSE

for each calculated system can be plotted. By inspecting these plots, it’s possibleto see if the calculated values fulfills the requirements given in chapter 3.1 Problemformulation.

Figure 3.15. Closed loop for the feedback system.

3.5 Measuring step responseTo evaluate the response of the feedback system, a simple measuring method wasimplemented. The main idea was to apply a force to the robotic claw and measurehow the feedback system responds to the given force. A hand was placed in thearm unit and force was applied to the robotic claw. The feedback system wouldthen make the FSR on the arm unit go from zero to the setpoint given by theforce applied to the robotic arm. A problem with this method was that the armunit would start twitching. This was because of the user could not keep the handsteady. A solution to this problem was to start the measuring from a small forceinstead of starting from no force. By doing this, the twitches were reduced a lot.The changed test method was therefore:

• Apply small force to the robotic claw.• Let this value be the start point. For the step. function, set this value as the

zero point.• Rapidly increase the force and try to keep the force as constant as possible.• Measure the values of the FSR on the arm unit.

The code for the Arduino Uno was modified to a code for the measuring, seeAppendix C for code and Figure 3.16 for flowchart diagram. This code began themeasuring when the FSR on the robotic claw was given a force signal larger than350. When measuring, values from the FSR of the arm unit were saved and thenprinted when the measuring was finished. The test were done ten times.

17

CHAPTER 3. DEMONSTRATOR

The printed values were then imported into a MATLAB script for further cal-culations, see Appendix D. This script scaled the values so the start values were setto zero and the reference were set to one. Plotting of the step response for each testwas made, but also a plot with the averaged step response.

Figure 3.16. Flowchart for measuring the step response.

18

3.6. APPLICATION TESTING

3.6 Application testingA small amount of randomly picked people tried the robotic arm. They had sometime to feel different sized objects before the real testing began. The testing wasdone in two steps. The first was to feel two different objects while having theireyes shut. The test subject would then try to guess what object was the largest ofthe two. Different objects were used with the main focus to have two objects withlarge difference in size and two objects with a small difference in size. One of thetests was with the same object twice to see if they noticed it. This test was mademultiple times, where two of the times, the difference in size was big, two timessmall and one time without difference.

Next test was to move an object once with the feedback turned ON and oncewith the feedback turned OFF. This was done with their eyes open. After the testthe people had to answer the following questions:

1. Did the feedback system make the task easier?2. Did the feedback system improve the experience?

The meaning of the second question is to see if the user feels like there is a betterexperience when being able to somewhat feel what the robotic arm is feeling.

Some weeks after the application testing was done, then idea of being able tofeel objects with different softness was brought up. This test was done by grabbinga hard object and then a soft foam ball to see if a difference in feel could be noted.

3.7 ResultsThe robotic arm and the arm unit were placed on a wooden piece. This made thewhole test station more compact and easy to move, see Figure 3.17.

Figure 3.17. Final design.

19

CHAPTER 3. DEMONSTRATOR

3.7.1 PIDThe theoretically calculated step response was made in MATLAB Simulink. Usingpole placement calculations, different values for the parameters P, I and D werereceived. The overall best step response was found for a pole at s = −90, see Figure3.18. This system does not use the D part, but fulfills the given requirements, seechapter 3.1 Problem formulation. The system has a very small overshoot with shortsettling time and almost no oscillations. Comparing this step response to the onescalculated with the pole placed at other positions did not give as good results, seeAppendix E. The controllers using a D part did not improve the step response Asmentioned, this did not give as good result as for the PI controller with the pole ats = -90. The best result for a PID was for the one given in Figure 3.19.

Figure 3.18. Step response function with Simulink for the pole s = -90. The valuesare: P = 0.0536, I = 2.4215

20

3.7. RESULTS

Figure 3.19. Step response function with Simulink for the pole s = -0.001). Thevalues are: P = 1.4387, I = 0.0072, D = 71.9424. This system is very fast and has noovershoot. The problem with this system is that it has a constant error.

When measuring the step response for the real system, we got the results seen inFigure 3.20. The system is much slower than the theoretically calculated system.

Figure 3.20. Step response from measuring the real system. The measuring wasdone ten times. Each blue plot corresponds to one measurement. The red plotcorresponds to a step function.

These measurements was then averaged to provide a more reliable result as seen

21

CHAPTER 3. DEMONSTRATOR

in Figure 3.21. This step response reduces the errors from the measuring itself.

Figure 3.21. Step response function from measuring the feedback system. This isthe mean value of all measured values. The red plot corresponds to a step function.

3.7.2 Application testingThe results from having a small amount of people do some application testing isgiven in Table 3.1. Every test subject could feel the difference of object size whenthe two objects differs a lot in size.

Big and small refers to the difference between the size of the grabbed objects.Big means that the two objects being grabbed has a big difference in size. Smallmeans the opposite. No refers to having the subject grabbing the same objecttwice. The number "1" means that the answer is yes and "0" means no. For columns"Big","Small" and "No", "1" means that the test subject felt the difference and cor-rectly guessed what object was the largest.

Subject Big Small Small No BigDoes thismake the taskeasier?

Does thisimprove theexperience?

Questions Result Percentage

Person 1 1 1 1 1 1 0 1 Big 5 100Person 2 1 1 1 0 1 0 1 Small 4 80Person 3 1 0 0 1 1 0 1 No 3 60Person 4 1 1 1 0 1 0 1 Task easier? 1 20Person 5 1 1 1 1 1 1 1 experience? 5 100

Table 3.1. Test results from having test subjects using the robotic arm.

22

Chapter 4

Discussion and conclusions

4.1 DiscussionThe servos did not seem to be able to create the torque given by the manufacturer.While it should be able to lift a weight of 0.5kg without any problem, it couldhardly lift itself without added weight. This made us shorten the arm as much aspossible (approximately 0.2m). This would require a torque around 0.14Nm. Therobotic arm is still very weak and can hardly lift 0.1kg. Because of the design ofthe robotic arm mostly being created in CAD and 3D printed, buying a larger servowould require us to re-design most of the CAD models and 3D print them all again.This would be very time consuming. Because of the research question not being sodependent of the lifting part, we decided to leave the model as it was and focus onthe feedback system itself.

The best step response was for a PI controller with poles at s = -90. With avery fast settling time, almost no overshoot, and very small oscillations, this systemfulfilled our requirements from chapter 3.1. However with PID controller we get anerror that is too large and also oscillations. This is probably due to the I part beingtoo small. But with pole placement the parameters of the PID controller can’t beindividually tuned which means that the inclusion of a D part causes the I part toshrink.

The response function of the real system differed a lot from the theoretical cal-culation that was made. As can be seen in Figure 3.21, the plot corresponds to astep response, but is much slower than the theoretical as can be seen in Figure 3.18.There are many reasons to why this is the case. When calculating the transfer func-tion, friction and inertia of the construction were neglected. The transfer functiononly takes the DC motor into account. The test method for calculating the realsystem’s step response is also a great factor of why the result differs so much fromthe theoretical calculations. The measuring was done mostly by "feel" and each testcould differ a lot. The start and end value of the FSR of the robotic hand differedfor each test. This would affect the amount of the error, thus the time would differ.When testing, ones hand was placed in the arm unit. Because of the human factor,

23

CHAPTER 4. DISCUSSION AND CONCLUSIONS

it was very difficult to be able to have a constant reaction to the torque from themotor. Also, to get the reference value, the FSR value on the robotic arm wasprinted. This value was equal to the value of the FSR at the end of the test. Thismeans that the value could have differed a lot during the test. By doing several testsand calculating the mean value, we were able to reduce the error of the measuringtechnique. For more accurate results, a test rig should have been built, to minimizethe factors which are affecting the result. In hindsight, it would have been muchbetter if we had inserted a value to the FSR on the robotic arm, instead of applyingthe force ourselves. This would have removed the variance of the input FSR signal.

We had a very small amount of people to test the feedback system and answersome questions about its functionality. The reason of not having more people totest the unit was because of time limitations, but also that other areas felt morerelevant. Even though the test data may not be sufficient, some interesting resultsmay be considered. Every test subject were able to feel size difference in objectsdiffering a lot in size, but also when the difference were not so large. However,when having the subjects grab the same objects twice, 60% thought that there is adifference in size. One reason for this may be that when testing, we might not havebeen clear enough that we would test the same object twice. Another aspect of thisresult is the construction itself. The connection between the DC motor and FSRwith PID may lead to that the user’s hand closes a bit different every time grabbingan object. The system does only react to the reaction force from the grabbed objectand not its actual size. If the user keep closing her hand when the counter torque isproduced, the torque will increase but the hand will still be able to close. To be ableto have a correct feeling of the object’s size, the user has to close the hand slowlyand may have to decrease the force. 20% percent of the test subjects experiencedthat the moving task were easier with the feedback control. Perhaps this is becauseof the small distance between the user and the robotic arm. If the robotic arm werecontrolled from a greater distance via a screen, the result might have been different.Although the subjects did not feel that the design is making the task easier, 100%believed that it improved the experience i.e, the connection between the robot andthe user, since they got a better feeling for what the robotic arm were grabbing.

When testing to grab objects with different softness, the difference was minimal.This is probably due to how the FSR work. It only registers the applied force. Whengrabbing the soft foam ball, the user were able to close her hand a bit more becauseof the compression of the ball. This made the user to believe that the object beinggrabbed is smaller.

The softness test was very simply executed. If more sensors were implementedon the users hand and the robotic arm, the chance of feeling different softness wouldhave increased.

24

4.2. CONCLUSIONS

4.2 ConclusionsA PI controller with the values P = 0.0536 and I = 2.4215 seems to be best for ourfeedback system.

The PID system differed quite a lot from the simulated model. The reasons aremany. We neglected some friction forces, but also the measuring of the feedbacksystem had some drawbacks.

For improving the control of robotic arms, using a control method based on yourown arm movement is a concept that seems to be beneficial. Controlling a roboticclaw with haptic feedback where the user can get some kind of feeling for the sizeof the object being lifted, improves the application of controlling a robotic arm. Asimple feedback system for robotic arms could be very beneficial for the future ofrobotics in the industry. When the user and robot are in different places this couldimprove the application a lot. Using potentiometer for registering the user’s armmovement and mapping this to servos seem to be simple but yet a good way ofcontrolling a robotic arm. If the robotic arm would be of greater size, servos mightnot be the best solution.

25

Chapter 5

Recommendations and future work

5.1 RecommendationsWe would recommend getting bigger and preferably higher quality servos for therobotic arm.

The measuring for the feedback system in reality should be done better. Theinput value for the FSR on the robotic arm should be inserted as an input value inthe code rather than by pressing the FSR.

5.2 Future workSince this unit was made for testing the feedback system, it fulfilled its requirements.For future testings, the robotic arm should be designed to fit stronger servos. Thisenables testing of moving larger objects.

The gripping claw could be improved by making it more similar to a hand withfive fingers. This means having one FSR for each finger on the robotic arm and thearm unit. This would enable the testing of objects with different shapes.

One thing that we wanted to do was to be able to record the movement of therobotic arm. This would enable the user to do a task once, and the robotic armwould then be able to do the same task on its own. This leads to the fact that a userwith no robotic experience could easily set up the robotic arm’s movement. Thiswould be very beneficial to the industry, where you would not need an engineer tomake a change in the settings for the robotic arm to do the new task.

The feedback system could be improved so that the user could be able to feelthe softness of the object being grabbed. This may be done by building a grippingclaw that has five fingers and five sensors. The arm unit would have one sensor andone dc motor for each finger. This might improve the aspect of being able to feeldifferent softness.

In the future, a larger system could be built to be applied in industry.

27

Bibliography

[Arduino, 2016] Arduino. Arduino - arduinoboarduno [online]. (2016) [cited 2016-05-07]. Available from: https://www.arduino.cc/en/Main/ArduinoBoardUno.

[Beauregard, 2016] Beauregard, B. Arduino pid library [online]. (2016) [cited 2016-04-20]. Available from: http://playground.arduino.cc/Code/PIDLibrary.

[bluebean, 2012] bluebean. Motion controlled robotic arm [online]. (2012) [cited2016-05-24]. Available from: http://www.instructables.com/id/Intro-53/.

[Epilog Laser, 2016] Epilog Laser. Epilog fusion m2 laser series [online]. (2016)[cited 2016-05-07]. Available from: https://www.epiloglaser.com/products/fusion-laser-series.htm.

[Glad and Ljung, 2006] Glad, T. and Ljung, L. (2006). Reglerteknik - Grundläg-gande teori. Studenlitteratur, 4th edition.

[HAGERTY, 2015] HAGERTY, J. R. (2015). Meet the newgeneration of robots for manufacturing. Wall street jour-nal. Available from: http://www.wsj.com/articles/meet-the-new-generation-of-robots-for-manufacturing-1433300884[cited 2016-03-06].

[IndustryWeek, 2012] IndustryWeek, Josh, C. The future of robotics in man-ufacturing: Moving to the other side of the factory [online]. (2012)[cited 2016-03-06]. Available from: http://www.industryweek.com/robotics/future-robotics-manufacturing-moving-other-side-factory.

[Keystone Electronics Corp, 2013] Keystone Electronics Corp. How robots havechanged manufacturing [online]. (2013) [cited 2016-03-06]. Available from: http://www.keyelco.com/blog-details.cfm?blog_id=40.

[Margolis, 2009] Margolis, M. Arduino servo library [online]. (2009) [cited 2016-03-07]. Available from: https://www.arduino.cc/en/Reference/Servo.

[MathWorks Nordic, 2015a] MathWorks Nordic. Matlab - the language of tech-nical computing [online]. (2015) [cited 2016-05-07]. Available from: http://se.mathworks.com/products/matlab/.

29

BIBLIOGRAPHY

[MathWorks Nordic, 2015b] MathWorks Nordic. Simulink - simulation and model-based design [online]. (2015) [cited 2016-05-07]. Available from: http://se.mathworks.com/products/simulink/.

[Okamura, 2009] Okamura, A. M. (2009). Haptic feedback in robot-assisted min-imally invasive surgery. US National Library of Medicine National Institutesof Health. Available from: http://www.ncbi.nlm.nih.gov/pmc/articles/PMC2701448/ [cited 2016-03-06].

[PATTNAIK, 2013] PATTNAIK, A. Robotic arm control through human armmovement using accelerometers [online]. (2013) [cited 2016-05-24]. Availablefrom: http://ethesis.nitrkl.ac.in/5283/1/109EI0297.pdf.

[Siemens PLM Software, 2016] Siemens PLM Software. Solid edge. [online]. (2016)[cited 2016-05-07]. Available from: https://www.plm.automation.siemens.com/en_us/products/solid-edge/.

[Ultimaker BV, 2016] Ultimaker BV. Ultimaker 2 [online]. (2016) [cited 2016-05-07]. Available from: https://ultimaker.com/en/products/ultimaker-2-go.

[Univeristy of Michigan, 2016] Univeristy of Michigan. DC Motor Speed:System Modeling [online]. (2016) [cited 2016-03-20]. Available from:http://ctms.engin.umich.edu/CTMS/index.php?example=MotorSpeed&section=SystemModeling.

30

Appendix A

Datasheets

31

APPENDIX A. DATASHEETS

Datasheet for gear and DC Motor

32

33

APPENDIX A. DATASHEETS

Datasheet for servos

34

35

Appendix B

Schematics

37

APPENDIX B. SCHEMATICS

Schematic of the whole system

38

Appendix C

Arduino Code

39

APPENDIX C. ARDUINO CODE

Arduino code with servo control and haptic feedback1 /∗ Desc r ip t i on2 This i s the main code f o r the whole system .3 I t conver t s the s i g n a l from potent iometer s to s e rvo s .4 A PID con t r o l i s implemented f o r c o n t r o l l i n g5 the FSR s i g n a l s . This uses a PID l i b r a r y .6 ∗/7

8 #inc lude <PID_v1 . h> // Import PID l i b r a r y9

10 #inc lude <Servo . h> // import se rvo package11 Servo myservo0 ; // Servo f o r r obo t i c arm ( base )12 Servo myservo1 ; // Servo f o r r obo t i c arm (arm)13 Servo myservo2 ; // Servo f o r r obo t i c arm ( claw )14

15 i n t potPin0 = 0 ; // Potent iometer 0 , arm uni t ( base )16 i n t potPin1 = 1 ; // Potent iometer 1 , arm uni t (arm)17 i n t potPin2 = 2 ; // Potent iometer 2 , arm uni t ( claw )18

19 //Connections to H−br idge20 i n t IN1 = 1 ; //Motor IN121 i n t IN2 = 2 ; //Motor IN222 i n t ENA = 3 ; //Motor ENA23 double PWM;24

25 i n t va l0 ; // I n i t i a t e v a r i ab l e26 i n t va l1 ; // I n i t i a t e v a r i ab l e27 i n t va l2 ; // I n i t i a t e v a r i ab l e28

29 // Var iab l e s f o r PID30 double Actual , Error , I n t eg ra l , P, I , D, Sca leFactor , Drive ,

Di rect ion , Last ;31

32 i n t fsrPinHand = 3 ; //FSR and 10K pulldown connected to a333 i n t fsrPinRobot = 4 ; //FSR and 10K pulldown connected to a434

35 //The analog read ing from the FSR r e s i s t o r d i v i d e r36 double fsrReadingRobot , fsrReadingHand , fsrReadingRobotMap ,

fsrReadingHandMap ;37

38 double Setpoint , Input , Output ;39

40 //This part i s f o r sw i t ch ing the PID on and o f f

40

41 i n t inPin = 5 ; // Input pin used f o r a on/ o f f switch42 i n t outPin = 4 ; //Output pin used f o r LED43 i n t PIDstate = HIGH; //Curent s t a t e . Turning feedback on/ o f f44 i n t Buttonreading ; //The cur rent read ing from input pin45 i n t prev ious = LOW; //The prev ious read ing from input pin46 long time = 0 ; //The l a s t time the output pin was togg l ed47 long debounce = 200 ; //Debounce time , i n c r e a s e i f output

f l i c k e r s48

49

50 /∗ Se t t i ng up PID us ing the l i b r a r y51 ∗ Input : The va r i ab l e to be c on t r o l l e d ( double )52 ∗Output : The va r i ab l e to ad jus t by the pid ( double )53 ∗ Setpo int : The value we want to Input to maintain ( double )54 ∗Kp, Ki , Kd: Tuning Parameters ( double>=0)55 ∗Dir e c t i on : DIRECT or REVERSE. determines which d i r e c t i o n56 ∗ the output w i l l move when faced with a given e r r o r .57 ∗/58

59 //The ca l c u l a t ed cons tant s f o r PID60 double Kp = 0.0536 , Ki = 2 .4215 , Kd = 0 ;61 PID myPID(&fsrReadingHand , &Drive , &fsrReadingRobot , Kp, Ki ,

Kd, DIRECT) ;62

63

64 void setup ( ) 65

66 myPID. SetMode (AUTOMATIC) ; // Se t t i ng up the PID l i b r a r y67

68

69 pinMode ( inPin , INPUT) ; //The switch i s an input s i g n a l70 pinMode ( outPin , OUTPUT) ; //The LED i s an output s i g n a l71

72 S e r i a l . begin (9600) ; // S e r i a l between Arduino and computer73

74 myservo0 . attach (9 ) ; // D i g i t a l pin 9 f o r se rvo 075 myservo1 . attach (10) ; // D i g i t a l pin 10 f o r se rvo 176 myservo2 . attach (11) ; // D i g i t a l pin 11 f o r se rvo 277

78 pinMode ( IN1 , OUTPUT) ; // D i g i t a l pin = output79 pinMode ( IN2 , OUTPUT) ; // D i g i t a l pin = output80 d i g i t a lWr i t e ( IN1 , LOW) ; //Rotate forward81 d i g i t a lWr i t e ( IN2 , HIGH) ;82

41

APPENDIX C. ARDUINO CODE

83 84 void loop ( )85 86 //On/Off part87 Buttonreading = d ig i ta lRead ( inPin ) ;88 /∗ i f the input j u s t went from LOW and HIGH and we ’ ve waited

long enough89 ∗ to i gnore any no i s e on the c i r c u i t , t o gg l e the output pin

and remember90 ∗ the time91 ∗/92 i f ( Buttonreading == HIGH && prev ious == LOW && m i l l i s ( ) −

time > debounce ) 93 i f ( PIDstate == HIGH)94 PIDstate = LOW;95 e l s e96 PIDstate = HIGH;97 time = m i l l i s ( ) ;98 99

100

101 d i g i t a lWr i t e ( outPin , PIDstate ) ;102

103 prev ious = Buttonreading ;104

105 //SERVO PART106 va l0 = analogRead ( potPin0 ) ; //Read analog 0107 va l1 = analogRead ( potPin1 ) ; //Read analog 1108 va l2 = analogRead ( potPin2 ) ; //Read analog 2109

110 //Change range o f potent iometer s111 va l0 = map( val0 , 0 , 614 , 0 , 179) ;112 va l1 = map( val1 , 0 , 614 , 0 , 179) ;113 va l2 = map( val2 , 791 , 1023 , 50 , 140) ;114

115 //This part c r e a t e s some l im i t a t i o n s f o r the movement .116 // Servo 0 base117 i f ( va l0 < 179)118 119 myservo0 . wr i t e ( va l0 ) ; // Potent iometer to se rvo120 121

122 // servo 1 arm123 i f ( va l1 < 179 && val1 > 80)

42

124 125

126 myservo1 . wr i t e ( va l1 ) ; // Potent iometer to se rvo127 128

129 // servo 2 claw130 i f ( va l2 < 179)131 132 myservo2 . wr i t e ( va l2 ) ; // Potent iometer to se rvo133 134

135 //PID PART136 fsrReadingRobot = analogRead ( fsrPinRobot ) ; //Analog s i g n a l

from robot FSR137 fsrReadingRobotMap = map( fsrReadingRobot , 0 , 995 , 0 , 255) ;

//Change range138

139 fsrReadingHand = analogRead ( fsrPinHand ) ; //Analog s i g n a lfrom hand FSR

140 fsrReadingHandMap = map( fsrReadingHand , 0 , 995 , 0 , 255) ;//Change range

141

142 myPID. Compute ( ) ; // Ca lcu la te PID143

144 i f ( fsrReadingRobot == 0) 145 Drive = 0 ;146 147 ///// PID On or OFF//////148 i f ( PIDstate == HIGH) 149 analogWrite (ENA, Drive ) ; //Send PWM command to motor

board150 S e r i a l . p r i n t ( "PID IS ON! ! ! " ) ;151 152 e l s e 153 Drive = 0 ;154 analogWrite (ENA, Drive ) ; //Send PWM command to motor

board155 S e r i a l . p r i n t ( "PID IS OFF ! ! ! " ) ;156 157

158 //This part p r i n t s important va lue s to the s e r i a l monitor159 S e r i a l . p r i n t ( " Analog read ing Robot = " ) ;160 S e r i a l . p r i n t ( fsrReadingRobot ) ;161

43

APPENDIX C. ARDUINO CODE

162 S e r i a l . p r i n t ( " Analog read ing Hand = " ) ;163 S e r i a l . p r i n t ( fsrReadingHand ) ;164

165

166 S e r i a l . p r i n t ( " PWM = " ) ;167 S e r i a l . p r i n t ( Drive ) ;168

169 S e r i a l . p r i n t ( " Pot0 = " ) ;170 S e r i a l . p r i n t ( va l0 ) ;171

172 S e r i a l . p r i n t ( " Pot1 = " ) ;173 S e r i a l . p r i n t ( va l1 ) ;174 S e r i a l . p r i n t ( " Pot2 = " ) ;175 S e r i a l . p r i n t l n ( va l2 ) ;176

177

44

Arduino code for step response measurement1 /∗ Desc r ip t i on2 ∗ This code c a l c u l a t e s the s tep response3 ∗ f o r the system . When the FSR on the r obo t i c4 ∗ hand i s pre s sed so that the value o f the FSR5 ∗ ge t s over 350 , the measuring s t a r t s . Afterwards6 ∗ the program pr i n t s the FSR va lue s from the7 ∗ arm uni t . This data can be copied and pasted8 ∗ i n t o MATLAB fo r f u r t h e r c a l c u l a t i o n s9 ∗/

10

11 #inc lude <PID_v1 . h> // Import PID l i b r a r y12

13 //Timer f o r the measuring14 i n t time0 = m i l l i s ( ) ;15 i n t time1 = 0 ;16 i n t i n t e r v a l 2 = 50 ;17 i n t i n t e r v a l = 50 ;18 i n t array [ 4 0 ] ; //Empty l i s t f o r measured FSR19

20 i n t l ed = 13 ; //LED pin21

22 //Connections to H−br idge23 i n t IN1=1; //Motor IN124 i n t IN2=2; //Motor IN225 i n t ENA=3; //Motor ENA26 double Drive ;27 double PWM;28

29 i n t fsrPinRobot = 4 ; //FSR and 10K pulldown connected to a330 i n t fsrPinHand = 3 ; //FSR and 10K pulldown connected to a431

32 double fsrReadingRobot , fsrReadingHand , fsrReadingRobotMap ,fsrReadingHandMap ; //The analog read ing from the FSRr e s i s t o r d i v i d e r

33

34 double Setpoint , Input , Output ;35

36 // Spec i f y the l i n k s and i n i t i a l tuning parameters37 double Kp = 0.0536 , Ki = 2 .4215 , Kd = 0 ;38

39 i n t inPin = 5 ; //The number o f the input pin40 i n t outPin = 4 ; //The number o f the output pin

45

APPENDIX C. ARDUINO CODE

41

42 i n t PIDstate = HIGH; //Current s t a t e o f the output pin43 i n t Buttonreading ; //Current read ing from the input pin44 i n t prev ious = LOW; // Previous read ing from the input pin45

46 // the f o l l ow va r i a b l e s are long ’ s because the time ,measured in mi l i s econds ,

47 // w i l l qu i ck ly become a b igge r number than can be s to r ed inan i n t .

48 long time = 0 ; //The l a s t time the output pin was togg l ed49 long debounce = 200 ; //The debounce time , i n c r e a s e i f the

output f l i c k e r s50 i n t p lace = 0 ;51 i n t count = 0 ;52 char s t a r t = ’ 0 ’ ;53 i n t STARTTID;54

55

56 /∗ Se t t i ng up PID us ing the l i b r a r y57 ∗ Input : The va r i ab l e to be c on t r o l l e d ( double )58 ∗Output : The va r i ab l e to ad jus t by the pid ( double )59 ∗ Setpo int : The value we want to Input to maintain ( double )60 ∗Kp, Ki , Kd: Tuning Parameters ( double>=0)61 ∗Dir e c t i on : DIRECT or REVERSE. determines which d i r e c t i o n62 ∗ the output w i l l move when faced with a given e r r o r .63 ∗/64 PID myPID(&fsrReadingHand , &Drive , &fsrReadingRobot ,Kp, Ki ,Kd

, DIRECT) ;65 void setup ( ) 66

67 myPID. SetMode (AUTOMATIC) ; // Se t t i ng up the PID l i b r a r y68

69 pinMode ( inPin , INPUT) ;70 pinMode ( outPin , OUTPUT) ;71 S e r i a l . begin (9600) ; // Set up the s e r i a l72

73 pinMode ( IN1 ,OUTPUT) ; // D i g i t a l pin = output74 pinMode ( IN2 ,OUTPUT) ; // D i g i t a l pin = output75 d i g i t a lWr i t e ( IN1 ,LOW) ; //Rotate forward76 d i g i t a lWr i t e ( IN2 ,HIGH) ;77 S e r i a l . p r i n t l n ( " Begin t e s t i n g " ) ;78

79 80 void loop ( )

46

81 82

83 d i g i t a lWr i t e ( outPin , PIDstate ) ;84

85 //PID PART86 fsrReadingRobot = analogRead ( fsrPinRobot ) ;87 fsrReadingRobotMap = map( fsrReadingRobot , 0 , 995 , 0 , 255) ;

//Change range88

89 fsrReadingHand = analogRead ( fsrPinHand ) ;90 fsrReadingHandMap = map( fsrReadingHand , 0 , 995 , 0 , 255) ; //

Change range91

92 myPID. Compute ( ) ; // Ca lcu la te PID93

94

95 ///// PID On or OFF//////96 i f ( fsrReadingRobot == 0) 97 Drive = 0 ;98 99

100

101 analogWrite (ENA, Drive ) ; // send PWM command to motor board102 S e r i a l . p r i n t l n ( Drive ) ;103 S e r i a l . p r i n t ( fsrReadingRobot ) ;104

105 // Startmeasur ing i f the FSR read ing i s g r e a t e r than 350106 i f ( fsrReadingRobot >=350 && place == 0) 107 STARTTID = m i l l i s ( ) ;108 array [ p lace ] = fsrReadingHand ;109 p lace++;110

111 112 // Startmeasur ing i f the FSR read ing i s g r e a t e r than 350113 i f ( fsrReadingRobot >=350)114 i f ( m i l l i s ( )−STARTTID >= in t e r v a l 2 ) 115 i n t e r v a l 2 = i n t e r v a l 2 + i n t e r v a l ;116 array [ p lace ] = fsrReadingHand ;117 p lace++;118 119 // Pr int the va lue s a f t e r measuring i s done , 2 seconds120 i f ( m i l l i s ( )−STARTTID >= 2000) 121 f o r ( i n t i =0; i <=40−1; i++)122 S e r i a l . p r i n t ( array [ i ] ) ;

47

APPENDIX C. ARDUINO CODE

123 S e r i a l . p r i n t ( " , " ) ;124 125 S e r i a l . p r i n t l n ( " " ) ;126 S e r i a l . p r i n t l n ( "EFTER ARRAY" ) ;127 S e r i a l . p r i n t l n ( fsrReadingRobot ) ;128 delay (100000) ;129 130 131

132 count++;133

48

Appendix D

Matlab Code

49

APPENDIX D. MATLAB CODE

Matlab program for real step response1 %This code c a l c u l a t e s the s tep response f o r the r e a l system .

The system i s2 %measured 10 t imes . The mean value i s then c a l c u l a t ed . The

time o f3 %measurement i s 2 seconds . The va r i ab l e t a r g e t i s the

systems ta r g e t va lue .4 %The measurement va lue s are taken from a t e s t us ing a

Arduino UNO.5 c l e a r a l l , c l c , c l o s e a l l6

7 numberOfMeasurements = 10 ;8 %targe tx = ta rg e t va lue f o r the system .9 %measurex = FSR analog read value .

10 t a r g e t 1 = 574 ;11 measure 1 = [349 , 357 , 344 , 358 , 368 , 376 , 401 , 436 , 492 , 500 , . . .12 517 ,533 , 542 , 556 , 557 , 564 , 567 , 568 , 569 , 572 , . . .13 569 ,568 , 564 , 569 , 571 , 571 , 572 , 572 , 568 , 567 , . . .14 570 ,575 ,576 ,575 ,575 ,578 ,577 ,574 ,576 ,575 ] ;15

16 t a r g e t 2 = 553 ;17 measure 2 = [298 , 325 , 332 , 341 , 362 , 430 , 444 , 460 , 481 , 507 , . . .18 521 ,528 , 542 , 543 , 547 , 546 , 553 , 553 , 555 , 558 , . . .19 563 ,565 , 568 , 574 , 579 , 580 , 574 , 574 , 573 , 574 , . . .20 573 ,576 ,577 ,577 ,577 ,576 ,576 ,573 ,574 ,566 ] ;21

22 t a r g e t 3 = 659 ;23 measure 3 = [350 , 359 , 368 , 402 , 417 , 444 , 472 , 499 , 531 , 547 , . . .24 556 ,566 , 575 , 586 , 594 , 600 , 616 , 624 , 623 , 624 , . . .25 627 ,629 , 632 , 632 , 637 , 644 , 639 , 641 , 644 , 647 , . . .26 655 ,660 ,665 ,665 ,666 ,670 ,671 ,667 ,662 ,662 ] ;27

28 t a r g e t 4 = 593 ;29 measure 4 = [281 , 280 , 271 , 294 , 345 , 427 , 467 , 488 , 508 , 519 , . . .30 534 ,542 , 550 , 557 , 564 , 567 , 573 , 585 , 595 , 596 , . . .31 600 ,601 , 603 , 599 , 609 , 611 , 611 , 609 , 613 , 612 , . . .32 615 ,614 ,609 ,616 ,622 ,623 ,620 ,623 ,630 ,634 ] ;33

34 t a r g e t 5 = 563 ;35 measure 5 = [291 , 293 , 300 , 306 , 329 , 346 , 375 , 440 , 455 , 485 , . . .36 515 ,527 , 546 , 544 , 553 , 556 , 560 , 569 , 564 , 567 , . . .37 564 ,570 , 562 , 568 , 566 , 567 , 566 , 565 , 558 , 547 , . . .38 536 ,527 ,523 ,526 ,524 ,528 ,529 ,537 ,547 ,553 ] ;

50

39

40 t a r g e t 6 = 589 ;41 measure 6 = [254 , 265 , 288 , 364 , 435 , 462 , 483 , 494 , 523 , 529 , . . .42 533 ,540 , 545 , 555 , 563 , 568 , 568 , 572 , 578 , 577 , . . .43 576 ,580 , 586 , 596 , 593 , 599 , 604 , 598 , 596 , 595 , . . .44 603 ,591 ,582 ,577 ,575 ,578 ,579 ,589 ,590 ,584 ] ;45

46 t a r g e t 7 = 573 ;47 measure 7 = [248 , 255 , 329 , 355 , 398 , 432 , 495 , 521 , 561 , 573 , . . .48 566 ,560 , 558 , 557 , 550 , 547 , 546 , 554 , 558 , 560 , . . .49 567 ,566 , 566 , 562 , 559 , 561 , 558 , 559 , 551 , 549 , . . .50 550 ,552 ,552 ,553 ,553 ,559 ,561 ,562 ,563 ,568 ] ;51

52 t a r g e t 8 = 513 ;53 measure 8 = [238 , 260 , 326 , 351 , 383 , 431 , 457 , 503 , 512 , 503 , . . .54 510 ,514 , 509 , 513 , 513 , 508 , 512 , 514 , 512 , 511 , . . .55 506 ,504 , 500 , 502 , 504 , 496 , 500 , 502 , 502 , 500 , . . .56 510 ,514 ,514 ,523 ,524 ,532 ,538 ,538 ,538 ,532 ] ;57

58 t a r g e t 9 = 579 ;59 measure 9 = [281 , 305 , 373 , 392 , 460 , 480 , 512 , 529 , 545 , 567 , . . .60 578 ,591 , 598 , 600 , 599 , 605 , 606 , 610 , 618 , 619 , . . .61 616 ,609 , 612 , 615 , 623 , 626 , 628 , 628 , 627 , 626 , . . .62 626 ,621 ,617 ,613 ,613 ,607 ,607 ,599 ,593 ,574 ] ;63

64 t a r g e t 10 = 558 ;65 measure 10 = [242 , 255 , 287 , 311 , 374 , 400 , 421 , 442 , 464 , 489 , . . .66 493 ,496 , 499 , 502 , 503 , 503 , 505 , 506 , 510 , 511 , . . .67 513 ,517 , 521 , 526 , 525 , 522 , 525 , 525 , 529 , 526 , . . .68 523 ,529 ,532 ,529 ,527 ,535 ,537 ,530 ,534 ,537 ] ;69

70 %Time goes from 1 to 3 seconds71 i n t e r v a l = 0 . 0 5 ;72 measureTime = 1:0 .05 :3 − i n t e r v a l ;73 totalTime = 0 : i n t e r v a l :3−0.0574

75 %Creates a s tep response func t i on f o r r e f e r e n c e s i g n a l76 r e f S i g n a l = [ 0 0 1 1 1 ]77 x=[0 1 1 2 3 ]78 p lo t (x , r e f S i gna l , ’ r ’ )79 hold on80

81 %Creates empty l i s t s .82 s t a r tVa l = [ ] ;

51

APPENDIX D. MATLAB CODE

83 ta rgetVa l = [ ] ;84

85 %Star t va lue f o r t o t a l va lue o f a l l measurements .86 t o ta lVa l = 087 f o r i = 1 : numberOfMeasurements88 %Create l i s t s with s t a r t va lue s and tareg va lue s .89 s t a r tVa l ( i ) = measure i (1) ;90 ta rgetVa l ( i ) = ta rg e t i ;91

92 %To make the measurements s t a r t from zero f o r our93 %t r a n s f e r funct ion , subt rac t the s t a r t i n g value .94 measurementFromZero i = measure i −s t a r tVa l ( i ) ;95

96 %To make the measurement go to 1 ( s tep func t i on ) wes c a l e the

97 %measurement .98 scaledMeasurement i = measurementFromZero i . / (

targetVa l ( i )−s t a r tVa l ( i ) ) ;99

100 %Creates a p l o t with a l l the measurements101 f i g u r e (1 )102 p lo t (measureTime , scaledMeasurement i , ’b ’ )103 t i t l e ( ’ Step response func t i on o f r e a l system ’ )104 x l ab e l ( ’Time [ s ] ’ )105 y l ab e l ( ’ Step response ’ )106 ax i s ( [ 0 3− i n t e r v a l 0 1 . 5 ] )107 hold on108 g r id on109

110 %Calcu la te the t o t a l va lue o f a l l the s c a l edmeasurements . This w i l l be

111 %used f o r c a l c u l a t i o n o f the mean value f o r the measuredsystem .

112 t o ta lVa l = to ta lVa l+scaledMeasurement i ;113 end114

115 %Calcu la te mean value .116 meanVal = to ta lVa l /numberOfMeasurements ;117

118 %Creates a p l o t with the mean value .119 f i g u r e (2 )120 p lo t (measureTime , meanVal )121 t i t l e ( ’ Step response func t i on f o r r e a l system (mean value ) ’ )122 x l ab e l ( ’Time [ s ] ’ )

52

123 y l ab e l ( ’ Step response ’ )124 ax i s ( [ 0 3− i n t e r v a l 0 1 . 5 ] )125 g r id on126 hold on127 %add the r e f e r e n c e s i g n a l in the p l o t128 p lo t (x , r e f S i gna l , ’ r ’ )

53

APPENDIX D. MATLAB CODE

Matlab program for pole placement1 %This program c a l c u l a t e s the appropr ia te cons tant s f o r

d i f f e r e n t2 %co n t r o l l e r s3

4 c l e a r a l l , c l c , c l o s e a l l5 %Values f o r the motor from datasheet (maxon motor 110123)6 J = 4.27 + 0 . 4 ; %gcm^2 ( Rotor i n e r t i a )7 J = J∗1e−3∗1e−4; %kgm^28 b = 0 ; %N∗m∗ s (motor v i s c ou s f r i c t i o n constant )9 K = 13 . 9 ; %mNm/A (Torque constant )

10 K = K∗1e−3;11 R = 8 . 9 ;%Ohm ( e l e c t r i c r e s i s t a n c e )12 L = 0.585∗1 e−3; %e l e c t r i c inductance13 L=0;14

15

16 %Trans f e r func t i on f o r the motor17 s = t f ( ’ s ’ ) ;18 G_motor = K/(( J∗ s+b) ∗(L∗ s+R)+K^2)19

20 SystemPoles = po le (G_motor) ; %The po l e s21 %of the motor t r a n s f e r f u n c t i o n22

23 %Using po le placement to c a l c u l a t e24 %P, I , and D parameters by p l a c ing po l e s25 %fo r the denominator o f the c l o s ed loop system26

27 %Using only PI c o n t r o l l e r28 %Pole at s = −4729 PoleEquation47 = ( s+47)^230 P47 = 94∗ (4 .156 e −6/0.0139) − 0.000193231 I47 = (2209−1) ∗ (4 . 156 e −6/0.0139)32 %Pole at s = −7033 PoleEquation70 = ( s+70) ^2;34 P70 = 140∗ (4 .156 e −6/0.0139) − 0 .0001932 ;35 I70 = (4900−1) ∗ (4 . 156 e −6/0.0139) ;36 %Pole at s = −10037 PoleEquation100 = ( s+100) ^2 ;38 P100 = 200∗ (4 .156 e −6/0.0139) − 0 .0001932 ;39 I100 = (10000−1) ∗ (4 . 156 e −6/0.0139) ;40 %Pole at s = −15041 PoleEquation150 = ( s+150) ^2 ;

54

42 P150 = 300∗ (4 .156 e −6/0.0139) − 0 .0001932 ;43 I150 = (22500−1) ∗ (4 . 156 e −6/0.0139) ;44 %Pole at s = −9045 PoleEquation90 = ( s+90) ^2;46 P90 = 180∗ (4 .156 e −6/0.0139) − 0 .0001932 ;47 I90 = (8100−1) ∗ (4 . 156 e −6/0.0139) ;48

49 %Using the whole PID50 %Pole at s = −4751 P47_2 = 94/0.0139 − 0 .0001932 ;52 I47_2 = 2209/0 .0139 ;53 D47_2 = 1/0.0139 − 4 .156 e−6;54 %Pole at s = −0.155 PoleEquation01 = ( s +0.1) ^2 ;56 P01_2 = 0 .2/0 .0139 − 0 .0001932 ;57 I01_2 = 0 . 01/0 . 0 139 ;58 D01_2 = 1/0.0139 − 4 .156 e−6;59 %Pole at s = −0.0160 PoleEquation001 = ( s +0.01) ^2 ;61 P001_2 = 0.02/0 .0139 − 0 .0001932 ;62 I001_2 = 0 .0001/0 . 0139 ;63 D001_2 = 1/0.0139 − 4 .156 e−6;

55

Appendix E

Plots

57

APPENDIX E. PLOTS

Plots of step response for different poles with PI con-troller

Figure E.1. Step response function with Simulink for the pole s = -47. The valuesare: P = 0.0279, I = 0.6602 The system is quite fast and with no overshoot.

58

Figure E.2. Step response function with Simulink for the pole s = -70). The valuesare: P = 0.0417, I = 1.4648. This system has a shorter settling time than the systemwith the pole s =-47.

59

APPENDIX E. PLOTS

Figure E.3. Step response function with Simulink for the s = -100). The values are:P = 0.0596, I = 2.9896. This system has a shorter settling time than the the systemsfor pole s = -47 and s = -70. The problem with this system is that it produces asmall overshoot.

60

Figure E.4. Step response function with Simulink for the pole s = -150). Thevalues are: P = 0.0895, I = 6.7270. This is a very fast system, but has quite a largeovershoot.

61

TRITA MMKB 2016:25 MDAB086

www.kth.se