IN
DEGREE PROJECT TECHNOLOGY, FIRST CYCLE, 15 CREDITS
,
STOCKHOLM SWEDEN 2019
Tailing Service Droid
DAVID ANDERSSON
SHILWAN PIROTI
Tailing Service Droid
DAVID ANDERSSON SHILWAN PIROTI
Bachelor’s Thesis at ITM Supervisor: Nihad Subasic
Examiner: Nihad Subasic
Abstract
This project aims to build an electric powered robot, that by utilizing infrared light, recognizes a device carried by the user.
The robot built in this project utilizes swivel wheel simi-lar to car wheels for steering over the more conventional differential steering/drive. The testing done in this project proved that this method of steering has difficulties with navigating in small spaces and completing tight turns at low speeds. The result also gives reason to believe that a fixed reference distance between the user and the robot might not be optimal for all instances.
Keywords: - Mechatronics - Carrying - High Power - Infrared
Referat
Efterföljande Robot
Detta projekt går ut på att bygga en eldriven robot som ge-nom att använda infraröda dioder, ser en enhet som utan svårighet bärs av användaren. Roboten som byggs i detta projekts primära användningsområde är att transportera varor.
Roboten är konstruerad för att använda svägande hjul vil-ket påminner om bilstyrning, istället för en mer konventio-nell differentialstyrning. Testen som utfördes i detta pro-jekt visade att denna styrmetod har svårigheter med att navigera i små utrymmen och vid tvära vändingar vid lå-ga hastigheter. Resultatet av tester säger även att ett fast referensavstånd mellan användaren och roboten möjligtvis inte är optimalt vid vissa tillfällen.
Nyckelord: - Mekatronik - Lastbärande - Högeffekt - Infraröd - Tillståndsåterkoppling
Acknowledgements
We want to thank Staffan Qvarnström and Thomas Östberg for giving us the com-ponents necessary, help and guidance throughout this project. We would also like to give special acknowledgement to the assistants Seshagopalan Thorapalli Muralid-haran and Sresht Lyer, for without their help, this project would not have been successful.
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 Infrared tracking . . . 3 2.2 Components . . . 3 2.2.1 Arduino Uno . . . 3 2.2.2 Infrared Camera . . . 3 2.2.3 IR Diode . . . 4 2.2.4 DC Motor . . . 4 2.2.5 Servo Motor . . . 4 2.2.6 Motor Driver . . . 42.2.7 Linear Voltage Regulator . . . 4
2.2.8 Linear Potentiometer . . . 5 2.3 System Control . . . 5 3 Demonstrator 8 3.1 Design . . . 8 3.2 Hardware . . . 9 3.2.1 Wearable lights . . . 10 3.2.2 Infrared-camera . . . 10 3.2.3 Battery . . . 11 3.2.4 Motor driver . . . 11
3.2.5 Voltage regulator/Servo drivers . . . 12
3.2.6 Motors . . . 12
3.3 Software . . . 15
3.3.1 Camera data processing . . . 15
3.4 Body . . . 19
3.4.1 Steering . . . 19
3.4.3 Frame . . . 22 3.4.4 Wheel bearings . . . 22
4 Results and Discussion 23
4.1 Testing methodology . . . 23 4.2 Results . . . 24 4.3 Discussion . . . 26 5 Future work 28 Bibliography 29 Appendices 32 A Measurements 32
B Datasheet for components 37
List of Figures
2.1 Image of a typical potentiometer [1]. . . 5
3.1 Image showing the side of the robot (Edited in Paint.net [2]) . . . 8
3.2 Scheme of the electrical coupling on the robot (Made in Fritzing [3]) . . 9
3.3 Scheme of the electrical coupling of the wearable lights (Made in Fritzing [3]) . . . 9
3.4 Image of the lights (Edited in Paint.net [2]) . . . 10
3.5 Image of the IR-camera mounted . . . 11
3.6 Graphic of the gearing (Created in Paint.net [2]) . . . 13
3.7 The camera mounted on the servo (Edited in Paint.net [2]) . . . 14
3.8 A scheme of the control regulation (made in draw.io [4]) . . . 15
3.9 Figure 1 of the lights (Created in Paint.net [2]) . . . 16
3.10 Figure 2 of the lights (Created in Paint.net [2]) . . . 16
3.11 Graphic of the steering (Created in Paint.net [2]) . . . 19
3.12 Graphic of the axle mount (Created in Paint.net [2]) . . . 20
3.13 Render of the clamp that hold the wheels (Created in Solidedge and Rendered in Keyshot [5]) . . . 21
3.14 Image of the steering and rear wheels . . . 21
3.15 Render of the frame used (Created in Solidedge and Rendered in Keyshot [5]) . . . 22
4.1 One of the measurements of the distance and dutycycle (Created in Mat-lab [6]) . . . 24
4.2 One of the measurements of the servo angle and user angle (Created in Matlab [6]) . . . 25
B.1 Datasheet for Voltage regualtor [7] . . . 37
B.2 Datasheet for Steering Servo [8] . . . 38
B.3 Datasheet for Camera Servo. Image from [9] Text from [10]. . . 39
B.4 Datasheet for Motor Driver. Text from [11], Image form [12]. . . 41
B.5 Datasheet for IR camera [13]. . . 42
B.6 Datasheet for IR diode [14]. . . 43
B.7 Datasheet for battery [15]. . . 44
List of Abbrevations
KTH Kungliga Tekniska Högskolan DC Direct Current
PWM Pulse Width Modulation IR Infrared
IDE Integrated Development Environment kB Kilobyte
I/O Input/Output
Nomenclature
Pwaste Wasted power converted into heat. Uin Voltage supplied to the voltage regulator. Uout The voltage the regulator is specified at. I Current through the voltage regulator. L Distance between the camera and the lights. b Distance between the lights.
α Half of the angle that the camera sees the lights at. β Horizontal view angle of the camera.
γ Angle between the current straight path of the camera and the center of the lights. γreal Angle between the current straight path of the robot and the center of the lights. γservo Current angle of the servo, measured by the potentiometer.
m Number of pixels between the lights on the camera image. n Number of horizontal pixels of the camera image.
d Pixels that the center of the lights is skewed from the center of the camera image. ap The maximum angle that the potentiometer can be turned
ac Current angle of the potentiometer
Upot The voltage applied over the potentiometer
Introduction
1.1
Background
A personal assistant for carrying various goods is something one can feel the need for sometimes, the reasons can be various. These reasons are a lack of strength or endurance, a disability or the need for an increased feeling of freedom. The idea of this project is to solve this problem with the use of mechatronics to build a robot that can track and follow a person while carrying goods.
Object following robots have been subject of previous degree projects at KTH where different methods have been implemented to track and follow a target. STALK-E utilizes color recognition to track the target but results proved the method too unreliable as it was sensitive to disturbances [18]. Another project implemented ul-trasound by using a device that transmits ulul-trasounds that were caught by receivers mounted on the robot. The robot was restricted in numerous ways. For example, the transmitter had to be carried by hand and pointed directly at the receivers at a close proximity and the ultrasound signal could easily be disrupted by obstacles [19]. This bachelor’s degree project seeks to provide a different approach on the same idea and avoid problems that have plagued previous projects by developing a more definite tracking system.
1.2
Purpose
This project seeks to develop a versatile product that is envisioned to be used in a wide range of applications and to increase the efficiency of businesses or make life easier for the general consumer. The purpose of this thesis is to develop a four-wheeled robot that follows a person with the help of an IR positioning camera that identifies a user wearing infrared emitters. This raises some questions that are researched throughout the project.
• How can data received from an IR positioning camera be used to make the robot mimic the movement of the user so that it is tailing at a predefined distance?
CHAPTER 1. INTRODUCTION
• Will the robots maximum speed be sufficient and what will its turning radius be?
• How to optimize power transfer while maintaining low friction and size? • How to hold track of the target when it leaves the field of view of the camera
and keep following?
1.3
Scope
The possibilities of this project are limited due to time constraints. A set of goals were, however, set for the project.
• The robot should be able to carry a weight of 10kg and hold a predefined distance of 2m while tailing a target with a walking pace of 1.5 m/s.
• It should be able to predict the movement of the target when it leaves the field of view.
1.4
Method
The development of this product explores the possibilities of solving the problem with the use of different systems that are conjoined into a final product. These systems are:
• Driving by using a single DC-motor. The motor transmits torque to the driving axle with the use of gears. A driver is used to supply the power demanded by the motor.
• Using tie rods and a pitman arm connected to a servomotor to steer.
• Localization of user with infrared emitters and an infrared positioning camera. This data is used to make the robot imitate the movement of the user and maintaining a predefined distance. This is done by communicating with the driving DC-motor and the Servo-motor used for steering.
• Create a stable modular platform that enables the robot to be used with and switch between various accessories such as a basket for shopping bags or a plate for food.
Theory
2.1
Infrared tracking
Infrared light is radiated with wavelengths which exceed the upper limit of visible light and can range from lengths of 700 nm to 1 mm. The infrared region in the electromagentic spectrum is divided in to bands: Near, Short, Mid, Long and Far [20]. Infrared radiation is detected using different types of sensors and can therefore be used in a wide variety of applications. For example object tracking, anything with a temperature emits infrared radiation which causes there to be a lot of ambient radiation. The intensity of the radiation increases with the level of heat so objects with higher temperatures than our surrounding or lights that emit intense infrared light can be recognized and tracked by sensors [21].
2.2
Components
2.2.1 Arduino Uno
An Arduino Uno is an electronic board that contains a microcontroller. The con-troller can read inputs and output signals. Ardunio includes an open-source IDE that support programming languages C and C++. The Arduino uno contains the microcontroller ATmega328p that can hold 32kB of data. It has an input voltage in the range of 6-20V. The board has 14 I/O digital pins, 6 of which can provide PWM output and 6 Analog input pins [22].
2.2.2 Infrared Camera
Infrared cameras detect infrared light, convert it to an electronic signal and then process to produce an image or provide data points. There are many types of infrared cameras with a variety of ranges in the electromagnetic spectrum. IR-positioning cameras are a type that can detect light in the Near-band. These are sources with high intensity and short wavelengths [23].
CHAPTER 2. THEORY 2.2.3 IR Diode
Diodes are semiconducting elements with two electrodes that allow flow of current in a single direction [24]. IR diodes are a form of light-emitting diodes that illuminate infrared light with intensity greater than that of ambient infrared light.
2.2.4 DC Motor
A DC motor utilizes direct current to transform electrical energy to mechanical energy by inducing an electromagnetic force. It is mainly composed of a rotor, stator and commutator. The rotor is placed inside the stator which provide a magnetic field. A DC source is connected to the rotor with the help of a commutator. When electric current flows through the rotor an electromagnetic force is exerted on the rotor in conjunction with the magnetic field that is then translated to torque. A DC motor holds the simple property of linear relationship between electric current and torque. [24]
2.2.5 Servo Motor
The angular output of a Servo motor can be regulated with the use of sensors and control theory. The sensor reads the value of the angle and calculates the error from the reference angular output and the error is then implemented in a control feedback-loop that precisely adjusts the signal to the motor to get the desired angular output.
2.2.6 Motor Driver
A motor usually demands a power-input greater than a microcontroller can provide. A motor driver is used to help supply the motor with sufficient power by connecting it to an external power source. The driver is used to regulate the motor power by changing the duty cycle using Pulse Width Modulation (PWM) this method supplies a capacitor with short bursts of power to keep a certain constant voltage on the output [25].
2.2.7 Linear Voltage Regulator
A voltage regulator is a semiconductor component that drops the voltage level to a certain level [26]. The remaining energy is dissipated as heat energy, a heatsink is therefore used to conduct the heat away.
Pwaste= I × (Uin− Uout) (2.1)
Where Pwasteis the wasted power converted into heat Uin in the supplied voltage
Uout is the voltage the regulator is specified at I is the current through the regulator.
CHAPTER 2. THEORY
Many voltage regulators also utilizes current limiting to avoid damage.
2.2.8 Linear Potentiometer
A linear potentiometer is a variable resistor that varies its resistance linearly in regards to either the angle of a rotating shaft or to the distance of a slider [27].
Figure 2.1: Image of a typical potentiometer [1].
A typical potentiometer and the one used has three pins, a voltage is applied over the right and left pin and the angle of the shaft varies the voltage between the middle pin and the ground as:
U = UP ot× ac
ap (2.2)
ac is the current angle of the potentiometer
ap is the maximum angle that the potentiometer can be turned Upot is the voltage applied over the potentiometer
U is the voltage measured from the middle pin of the potentiometer
2.3
System Control
Control theory is used to decrease the error between a reference value and the system output by regulating the input signal. This is done by calculating the error between the reference value and the system output and constructing a control feedback loop that adjusts the input to the system after the size of the error. The input value is calculated using a PID-controller as follows:
u(t) = u0+ KPe(t) + KI Z t 0 e(τ)dτ + KD de(t) dt (2.3)
[28]. u0 is the default value of the signal. The proportional part increases the
CHAPTER 2. THEORY
therefore an integral part is added that grows as long as there is an error. The system approaches the reference value the faster as the gain factors Kp and KI increase but as they grow, the system becomes more unstable. The instability is rectified by adding a differential part but makes the system slower.
Demonstrator
3.1
Design
This section seeks to give an overview of the design in order to present the reader with a clear picture of the finished product before delving deeper into details in the following sections.
Figure 3.1: Image showing the side of the robot (Edited in Paint.net [2])
To make the robot somewhat resistant to the elements and dropped items, most electronic parts where put in a black box with connectors for easy testing of com-ponents.
CHAPTER 3. DEMONSTRATOR
3.2
Hardware
Figure 3.2: Scheme of the electrical coupling on the robot (Made in Fritzing [3])
Figure 3.3: Scheme of the electrical coupling of the wearable lights (Made in Fritzing [3])
CHAPTER 3. DEMONSTRATOR 3.2.1 Wearable lights
The target is distinguished by the robot by wearing a set of IR-diodes, as seen in figure 3.4. The lights are positioned on the user at waist-level with intentions that they are mounted on the belt.
Figure 3.4: Image of the lights (Edited in Paint.net [2])
This device consists of two sets of IR lights (1 in image), a battery pack for the lights (3 in image) and an easily accessible button to toggle the lights and thus halting the robot (2 in image). The reason there are 4 IR-diodes per side is to extend the maximum range between the user and the robot. The clusters of lights has been tested to only register as one object from the camera per cluster and therefore the IR-radiation from the 4 diodes are quadrupled compared to only using one diode per side. This approximation is derived from that the cluster is feed with 4 times the electrical power as one IR diode.
3.2.2 Infrared-camera
SEN0158 is an infrared positioning camera that is used in this project to measure where the user is in regards to the robot. The camera has a horizontal view of 33 degrees and a vertical view of 23 degrees. It can track up to four sources and returns their position in a coordinate system that ranges from 0 to 1023 in both horizontal and vertical direction. Sources with high IR-radiation such as IR-lights are detected whilst the small amounts of ambient radiation is neglected. This IR-camera is specified to be able to track objects up to 3 meters away, however, it has
CHAPTER 3. DEMONSTRATOR
a greater range if the source of the IR-radiation is strong enough. The camera can detect the lights used in this project at 10 meters away.
Figure 3.5: Image of the IR-camera mounted
Because the camera should optimally be at the same height as the lights, the camera was mounted to a stick protruding from the front of the robot’s frame.
3.2.3 Battery
Because electronic devices require electricity and since the intended range of the robot makes a power cord from a wall outlet unfeasible, a battery was implemented into the design of the device. The voltage of the battery decides the powergrid and also the components that can be used, an established standard voltage was deemed beneficial. Because of the low cost to stored energy ratio a 12V battery (and therefore a 12V powergrid) was chosen. The battery chosen has a capacity of 7.0 Ah which was deemed enough for the project.
3.2.4 Motor driver
The motor driver is used to help supply the motor with required power and regulate the input signal using PWM to emulate a constant voltage. MD03 is a medium power motor driver with a 5v logic supply and 5V - 24V supply to the motor. This project utilizes the analog mode which allows the motor to controlled with an analog signal that ranges from 0V to 5V.
CHAPTER 3. DEMONSTRATOR
3.2.5 Voltage regulator/Servo drivers
Since the servos used requires a voltage of 4.8V to 6V and the powergrid used is 12V, a voltage regulator was needed to deliver the correct voltage. This is a feasible solution for creating different voltage levels on a battery powered device [29]. The regulator used is a HA17806 (6V). This driver is a linear voltage regulator and has a maximum rated current of about 1.5A whilst the servo can exceed 4A, however this semiconductor utilizes internal monitoring to limit the current. The excess power from the voltage regulator was unloaded to a separate heatsink screwed tight to the regulator. The heatsink was deemed large enough to radiate the theoretical maximum power the regulator can produce: (12V −6V )×1.5A = 9W from equation 2.1.
3.2.6 Motors
Propulsion motor
The motor used was found label-less however it has been identified as a MY6812. This engine is a cheap generic DC motor which has a lot of different specifications. It comes in many varieties: 100W, 120W and 150W as well as all of them in a 12V and 24V version. Through testing of the current draw at no load and a spec sheet of every model it has been deducted that the engine used in the project is the 12V and 150W version. However since this matter still isn’t confirmed, the motor may be another version.
Since the engine only has a connection at one side, gears are necessary for transfer-ring the engines torque to the solid front axis. The following graphic (Figure 3.6) shows how the gearing is made:
CHAPTER 3. DEMONSTRATOR
Figure 3.6: Graphic of the gearing (Created in Paint.net [2])
The motor (1 in the figure) require a cog belt (the cogs of the belt is not shown in the graphic) to distance the wheelaxis (2 in the figure) outside its radius.
Steering servo
The steering servo used is a Hitec HS-5745MG. This servo is power by a constant 6 V from the servo driver. This servo is digital and is controlled with a data pulse width of 900 µs to 2100 µs with a cycle refresh time of 20ms. The servo was mounted in the rear of the frame and rotates the back wheels and thereby enables the robot to turn.
Camera servo
The camera servo used is a Hitec HS-705MG. This servo is controlled the same way as the steering servo. To improve upon the relatively low horizontal viewing angle of the IR-camera, a servo was used to rotate the camera assembly so that the camera would look straight at the user. A potentiometer (22kΩ) was used to get the servos position at a given moment. The real angle to the user would therefore be the sum of the servos angle and the cameras horizontal angle to the user.
CHAPTER 3. DEMONSTRATOR
Figure 3.7: The camera mounted on the servo (Edited in Paint.net [2]) The camera is mounted to the servo (1 in figure) and since only the target for the angle of the servo is known at a given moment a potentiometer (3 in figure) was mounted with a clamp (2 in figure) to get a reading on the current angle of the servo.
CHAPTER 3. DEMONSTRATOR
3.3
Software
Because several of the components used in this project utilize digital signals, a microprocessor was needed. The microprocessor used is an Arduino uno and was programmed to interpret the sensors and send signals to the different systems. Fig-ure 3.8 displays a flowchart of the program.
Figure 3.8: A scheme of the control regulation (made in draw.io [4])
3.3.1 Camera data processing
Because the camera have a fixed aperture and the lights on the user have a fixed predetermined distance between them, it is possible to know the distance to the lights. You can by analyzing the distance in pixels between the points on the IR image and with the horizontal field of view of the camera the Arduino can calculate how far away the user is. The following image will demonstrate this thesis:
CHAPTER 3. DEMONSTRATOR
Figure 3.9: Figure 1 of the lights (Created in Paint.net [2])
Where L is the distance between the camera and the lights. bis the distance between the lights.
α is half the angle that the camera sees the lights at. β is the constant horizontal view angle of the camera.
It is also possible to calculate at what angle the user is in respect to the direct path of the camera. The following image will demonstrate this thesis:
Figure 3.10: Figure 2 of the lights (Created in Paint.net [2])
Where γ is the angle between the current straight path of the camera and the center of the lights.
nis the number of horizontal pixels of the camera image.
m is the calculated number of pixels between the lights on the camera image. dis the number of horizontal pixels that the center of the lights is skewed from the center of the camera image.
CHAPTER 3. DEMONSTRATOR
Calculation 3.1 for the horizontal angle between the robot and the lights:
γ = β × d
n (3.1)
Because of that the camera is mounted on a servo that can rotate, the robots angle to the user is not always the same as the cameras angle to the user. The poten-tiometer that measures the cameras angle can rotate a 270 degrees while varying its resistance from 0 to 22kΩ. When applying a voltage over the potentiometer, the voltage of the middle pin can be measured as the shaft of the potentiometer is turned. γreal= γ + γservo (3.2) γservo= ap× Upot− U Upot (3.3) Results in: γreal= γ + ap× Upot− U Upot (3.4)
γreal is the angle between the current straight path of the robot and the center of the lights.
γservo is the current angle of the servo, measured by the potentiometer. ap is the maximum angle that the potentiometer can be turned
Upot is the voltage applied over the potentiometer
U is the voltage measured from the middle pin of the potentiometer Calculation for the distance between the camera and the lights:
α= β 2 × m n (3.5) L= 0.5 tan(α) (3.6) Results in: L= 0.5 tan(β 2 × m n) (3.7)
CHAPTER 3. DEMONSTRATOR
or decelerate in order to maintain a constant distance to the user.
The centre of the points can also be analyzed to get the angle between the cur-rent path of the robot and the curcur-rent path of the user. The robot can thereafter steer in the direction of the user. The robot utilizes a PID feedback controller for both between the distance to the light and engine duty cycle aswell as between angle to the lights and servo pulse.
CHAPTER 3. DEMONSTRATOR
3.4
Body
This chapter aims to display the mechanical build of the robot.
3.4.1 Steering
Figure 3.11: Graphic of the steering (Created in Paint.net [2])
The steering servo (1 in image) rotates a pitman arm around a point (2 in image), the pitman arm is connected to two translating beams (3 in image) that is connected to both back wheels and rotates them around a point (4 in image).
The reversed geometry was chosen rather than a linear design because it was tested and proved to give the wheels a larger turning angle at maximum servo angles, giv-ing the robot a smaller turngiv-ing radius than if another geometry was chosen. Since the scope of this project sought to make the robot as nimble as possible, giving the the robot the smallest possible turning radius was deemed optimal. It was decided that any issues with nonlinearities caused by the geometry was to be remedied in the code if they would surface. The code controlling the steering of the robot utilizes a closed loop feedback system. The steering was in testing very stable therefore was this not considered an issue and didn’t need further investigation.
CHAPTER 3. DEMONSTRATOR 3.4.2 Wheels
Front wheels
The front driver wheels is two larger all terrain wheels. These wheels are mounted with screws to a plastic clamp that is mounted to an axle using a clamping connec-tion as following image demonstrates:
Figure 3.12: Graphic of the axle mount (Created in Paint.net [2])
The wheel is mounted to the clamp with screws (2 in image), this clamp is (hence its name) clamped to the axle (3 in image) with two screws (1 in image).
CHAPTER 3. DEMONSTRATOR
Figure 3.13: Render of the clamp that hold the wheels (Created in Solidedge and Rendered in Keyshot [5])
This clamp is used on both sides of each front wheel to ensure that the wheels are rigidly mounted to the front axis.
Back wheels
Figure 3.14: Image of the steering and rear wheels
CHAPTER 3. DEMONSTRATOR 3.4.3 Frame
The frame of the robot was for simplicity’s sake constructed like a ladder frame with 2 45x45mm wooden beams och opposite side of the robot with crossbeams made of 2 45x45mm wooden beams. For mounting the components a flat sheet of acrylic was fitted to the ladder frame.
Figure 3.15: Render of the frame used (Created in Solidedge and Rendered in Keyshot [5])
3.4.4 Wheel bearings
Because the wheels are not fitted directly to the engine and that the robot is designed to carry a load, the robot is fitted with various wheel bearings to ensure minimal power losses due to friction or wear to the parts that axes rotate inside. Due to the bearings main purpose in the robot is to lower friction, no life expectancy calculations has been made because of the relative low loads on the bearings. The bearings used can be found in the appendix.
Results and Discussion
4.1
Testing methodology
The testing of the robot was made by walking down a hallway while dynamically changing the walking speed and moving from side to side, avoiding obstacles to simulate a realistic scenario. The data was collected to by an assistant carrying a laptop connected to the robot by a USB cord, the cord was kept slack during the extent of the tests to not affect the results.
I should be noted that when exporting multiple values for plotting, unwanted time delays appear and will at times cause bugs in the code used (as spikes in the graphs). The more variables being exported the more bugs appeared in the graphs, as can be seen in the measurement chapter in appendix. However, the robot appeared to function normally throughout the testing and therefore the tests were deemed valid.
CHAPTER 4. RESULTS AND DISCUSSION
4.2
Results
The robot has undergone various tests to see if the robot meets the criterias that were set in the scope of the project. It should be noted that since the project focuses on a practical application of the theory, most answers to the research questions will be addressed in discussion (chapter 4.3).
Figure 4.1: One of the measurements of the distance and dutycycle (Created in Matlab [6])
As can be seen in the beginning of Figure 4.1, the integrating part quickly reaches its maximum, causing the robot not to be too far behind its user. However the distance to the user rarely reaches the reference distance, suggesting a larger in-tegrating part of the PID controller or a larger limit of it might be necessary to maintain the requested distance.
Since the maximum range between the camera and the IR-lights can be in excess of 10m, this static error was deemed to be acceptable since as the graph displays the distance never reaches above 4m.
CHAPTER 4. RESULTS AND DISCUSSION
Figure 4.2: One of the measurements of the servo angle and user angle (Created in Matlab [6])
See Testing methodology (chapter 4.1), this test was made exporting multiple vari-ables.
It can be seen in figure 4.2 that the servo rotates proportional to the angle to the user.Whilst the robot is stable it can be seen there is a static error causing the robot not to focus directly on the user.
CHAPTER 4. RESULTS AND DISCUSSION
4.3
Discussion
The resulting robot of this project is sturdy albeit heavy, it has been proven to function very well in a straight line, even at high speeds. The robot could very well keep up with a person at even a higher than average walking pace which satisfies the first part of the second research question. The solid front axle of the robot gives it a reluctance to turning, which aids stability. However, it has issues in regards to navigation in small spaces. This has several factors, some of which can be remedied. The physical size of the robot gives it a turning radius larger than smaller robots. The utilization of turning wheels for steering rather than differential steering makes it hard for the robot to rotate due to the robot having to move forwards or backwards in order to turn, whereas differential steering/drive could turn on a dime. However, the robot was proved to be able to turn 180 degrees within the confines of a corri-dor that measured 2.5 meters across, giving the robot a confirmed turning radius of less than 1.25 meters which answers the second part of the second research question. The different regulating controllers for the robot were constructed as were tested to make the robot follow at an optimal distance while also being stable, as this was the goal for the project. The controller for the propulsion motor was a combination of a Proportional-Integrating controller and a minimum motor speed in either clockwise or anti-clockwise rotation. The latter part was used to overcome some of the friction in the power transfer of the robot driving wheels to give the robot a more responsive behavior at low speeds. The Proportional part of the controller was used to make the robot react fast to user movement and therefore make the robot saver if the user suddenly stops. The Integrating part of the controller was used to make the robot follow at the reference distance but was limited to avoid integral windup that would cause the motor signal to overshoot. The figure 4.2 in chapter 4.2 suggest that this limit was set too low seeing that the robot rarely reaches the specified reference distance. The reasoning behind not doing this is that if the user suddenly stops, the overshoot might be large enough for the robot to run into the user which was deemed not to be appreciated and should therefore be avoided. A derivative part of the controller was not implemented because rise time of the system was deemed as rather low. A larger minimum motor speed was considered to reduce the static error as well to make the robots movement more responsive at low speeds. The reason why the minimum motor speed was not increased over the current value (10% duty-cycle) was to prevent the robot from driving back and forth oscillating around the reference distance when the user was not moving. Furthermore would this value have to change when goods where loaded on the robot which would make the friction increase.
The steering servo used a proportional controller with limits at maximum wheel turning, this rather simple controller seemed to be very stable and where therefore deemed satisfactory. The robot did however experience a static error in testing (as
CHAPTER 4. RESULTS AND DISCUSSION
can be seen in figure 4.2), this may be due to the camera not pointing directly straight, also the steering may be wrongly calibrated so that the robot will turn even with the servo in its middle position. Possible improvements are to calibrate the steering or to make the servo signal controller have an integrating part.
The servo for the camera should not be in the need of a controller if all constants are known, however due to ripple, tolerances and nonlinearities of the potentiometer it was not that simple. The controller implemented was a proportional however since the input and output are of the same dimension the servo will start oscillate, and if the value is set too low the servo will lag behind the optimal value. Since only the latter is stable, it was chosen. A remedy for this would be to implement an integrating part to this controller, however since this servo was unstable (as will be discussed later) stability was prioritized. This is the solution that was used for the robot to answer the first research question.
The IR- camera has limited viewing angles, this could be remedied by a better, more expensive camera, alternatively use multiple cameras and merge their images. Another solution could be to use a wide angle lens in front of the camera, this could however cause issues if the lights where far away therefore the lights would be closer together than if no wide angle lens where used and this was the reasoning why this wasn’t implemented.
The solution that was implemented with the camera mounted on a rotating servo is a working strategy however, a number of issues made the implementation explored in this report difficult. The servos used where very sensitive to disturbances caused by the motor driver and steering servo thus producing sudden voltage drops. If and how these voltage drops causes unintended instabilities between the steering servo and the camera servo was not explored in this project, however, since both these devices are part of the same system it may be a feasible cause. This is the solution that was used in the demonstrator to answer the fourth research question.
There was also a optimization problem when dealing with power transfer between two axis. The usage of gearing would be optimal, however due to tolerance issues which caused lockups in the gears when tested, this method was opted out of in this project. The usage of a belt proved to be a simpler solution, however there were instead a tradeoff between belt pressure (to avoid slipping) and overall friction and wear, this result is however a common issue [30]. The belt used in the demonstrator was 3D printed and was in the end unusable because it had expanded to a point of constantly slipping. Therefore, a part of the third research question unanswered because the method used in the demonstrator was deemed flawed. This is however a area of research that is very large with a large number of appliances, and it was deemed a valid result that the information gathered from the demonstrator would assist further development.
Future work
Future work could be to investigate the tracking abilities of a camera or IR cam-era with limited viewing angles, with the use of stepper motors or servo motors. Due to time constraints and that it was not a focus of this project, theories and development of the servo that extended the IR cameras horizontal viewing angles where not explored in depth. A possible project could be to control the camera in 2 dimensions for full tracking coverage.
Another recommendation would be to make a project dedicated to explore an op-timal method of power transfer at limited physical dimensions, such as comparing cogs, chain or cog belt for mechanical power transfer.
Bibliography
[1] [Accessed: 2019-06-03]. [Online]. Available: https://www.sparkfun.com/ products/14624
[2] dotPDN, “Paint.net.” [Online]. Available: https://www.getpaint.net/ [3] Fritzing, “Fritzing.” [Online]. Available: http://fritzing.org/home/ [4] draw.io, “draw.io.” [Online]. Available: https://www.draw.io/
[5] Siemens, “Solid edge st10.” [Online]. Available: https://solidedge.siemens. com/en/
[6] Mathworks, “Matlab 2018a.” [Online]. Available: https://www.mathworks. com/products/matlab.html
[7] “Ha17806v,” [Accessed: 2019-06-03]. [Online]. Available: https://pdf1. alldatasheet.com/datasheet-pdf/view/63506/HITACHI/HA17800V.html [8] “Hitec hs-5755mg,” [Accessed: 2019-06-03]. [Online]. Available:
https://hitecrcd.com/products/servos/discontinued-servos-servo-accessories/ hs-5755mg-metal-gear-giant-servo/product
[9] “Hitec hs-705mg,” [Accessed: 2019-06-03]. [Online]. Available: https://www. worthpoint.com/worthopedia/hitec-hs-705mg-4-scale-servo-135885400
[10] “Hitec hs-705mg,” [Accessed: 2019-06-03]. [Online]. Available: https: //servodatabase.com/servo/hitec/hs-705mgl
[11] “Md03,” [Accessed: 2019-06-03]. [Online]. Available: http://www.pishrobot. com/files/products/datasheets/md03.pdf
[12] “Md03,” [Accessed: 2019-06-03]. [Online]. Available: https://robot-electronics. co.uk/products/md03-24v-20a-h-bridge-motor-drive.html
[13] “Sen0158,” [Accessed: 2019-06-03]. [Online]. Available: https://wiki.dfrobot. com/Positioning_ir_camera
[14] “Tsal6400,” [Accessed: 2019-06-03]. [Online]. Available: https://www.vishay. com/docs/81009/tsal6100.pdf
BIBLIOGRAPHY
[15] “537-5488,” [Accessed: 2019-06-03]. [Online]. Available: https://docs-emea. rs-online.com/webdocs/16c3/0900766b816c34e3.pdf
[16] “612-6164,” [Accessed: 2019-06-03]. [Online]. Available: https://se.rs-online. com/web/p/ball-bearings/6126164/
[17] “618-9957,” [Accessed: 2019-06-03]. [Online]. Available: https://se.rs-online. com/web/p/ball-bearings/6189957/
[18] T. Köylüoglu and E. Lindbergh, “Stalk-e: Object following robot,” KTH, Skolan för industriell teknik och management (ITM), Maskinkonstruktion (Inst.), Tech. Rep., 2017. [Online]. Available: http://www.diva-portal.org [19] R. Hasenson and C. Larsson Olsson, “How to track an object using
ultrasound,” KTH, Skolan för industriell teknik och management (ITM), Maskinkonstruktion (Inst.), Tech. Rep., 2017. [Online]. Available: http: //www.diva-portal.org
[20] S. C. Liew. (2001) Electromagnetic waves. [Accessed: 2019-05-02]. [Online]. Available: https://crisp.nus.edu.sg/~research/tutorial/em.htm
[21] X. Maldague and S. Marinetti, “Pulse phase infrared thermography,” Journal of applied physics, vol. 79, no. 5, pp. 2694–2698, 1996.
[22] (2019) Arduino uno. [Accessed: 2019-05-02]. [Online]. Available: https: //store.arduino.cc/arduino-uno-rev3
[23] M. Planck, The theory of heat radiation, 2013, [Accessed: 2019-05-03]. [Online]. Available: https://books.google.se
[24] H. Johansson, Elektroteknik, 2013th ed. KTH, Department of Machine Design, 2013.
[25] S. Ketelaars, “Dc motor driver for low rpm,” Linnéuniversitetet, Fakulteten för teknik (FTK), Institutionen för fysik och elektroteknik (IFE). Fontys., Tech. Rep., 2016. [Online]. Available: http://www.diva-portal.org
[26] D. KNIGHT. (2016) Introduction to linear voltage regulators. [Accessed: 2019-05-02]. [Online]. Available: https://www.digikey.com/en/maker/blogs/ introduction-to-linear-voltage-regulators
[27] R. Santos. (2016) Electronics basics – how a potentiometer works. [Accessed: 2019-05-25]. [Online]. Available: https://randomnerdtutorials.com/ electronics-basics-how-a-potentiometer-works
BIBLIOGRAPHY
[29] M. A. d. Francisco, “Research on the buck converter’s efficiency,” Blekinge Institute of Technology, School of Engineering., Tech. Rep., 2013. [Online]. Available: http://www.diva-portal.org
[30] R. Hjortman and K. Harrysson, “Driveline: Moving infantry target (mit),” Jönköping University, School of Engineering, JTH, Mechanical Engineering., Tech. Rep., 2016. [Online]. Available: http://www.diva-portal.org
[31] “My6812 dc 150w,” [Accessed: 2019-06-03]. [Online]. Available: https: //www.aliexpress.com/item/32887481196.html
Measurements
Note that some filtering has been made to filter out certain spikes/bugs caused by the time delays induced by extracting datapoints.
1st measurement
APPENDIX A. MEASUREMENTS
2nd measurement
(Created in Matlab)
APPENDIX A. MEASUREMENTS
3rd measurement
(Created in Matlab)
APPENDIX A. MEASUREMENTS
4th measurement
APPENDIX B. DATASHEET FOR COMPONENTS
Datasheet for components
Voltage Regulator
APPENDIX B. DATASHEET FOR COMPONENTS
Steering servo
APPENDIX B. DATASHEET FOR COMPONENTS
Camera servo
APPENDIX B. DATASHEET FOR COMPONENTS
DC motor
APPENDIX B. DATASHEET FOR COMPONENTS
Motor driver
APPENDIX B. DATASHEET FOR COMPONENTS
IR camera
APPENDIX B. DATASHEET FOR COMPONENTS
*
IR diode
APPENDIX B. DATASHEET FOR COMPONENTS
Battery
APPENDIX B. DATASHEET FOR COMPONENTS
10mm Wheel bearing
APPENDIX B. DATASHEET FOR COMPONENTS
8mm Wheel bearing
Code
// T a i l i n g S e r v i c e D r o i d // B a c h e l o r ’ s D e g r e e P r o j e c t in M e c h a t r o n i c s , KTH // D a v i d A n d e r s s o n & S h i l w a n P i r o t i // 2019 -06 -03 // S o f t w a r e for the T a i l i n g S e r v i c e D r o i d # i n c l u d e < W i r e . h > # i n c l u d e < S e r v o . h > int I R s e n s o r A d d r e s s = 0 xB0 ; // int I R s e n s o r A d d r e s s = 0 x58 ; int s l a v e A d d r e s s ; int l e d P i n = 13; b o o l e a n l e d S t a t e = f a l s e ; b y t e d a t a _ b u f [ 1 6 ] ; int i ; S e r v o m y s e r v o ; int Ix [ 4 ] ; int Iy [ 4 ] ; int s ; int pos = 1 6 0 0 ; f l o a t L , alpha , gamma , m , d ; int n = 1 0 2 3 ; f l o a t b = 0 . 2 ; int b e t a = 33; int asd =1; f l o a t k = 1 5 0 0 ; // M i n i m u m v a l u e for m o t o r w h e n d r i v i n g f o r w a r d int m i n m o t = 2 0 0 ; // M a x i m u m v a l u e for m o t o rAPPENDIX C. CODE f l o a t m a x m o t = 6 0 0 ; // M o m e n t a r y m o t o r s p e e d f l o a t mh = m a x m o t ; // P r e v i o u s m o t o r s p e e d f l o a t mhd = mh ; // I - p a r t for m o t o r s p e e d r e g u l a t o r f l o a t mhi =0; // R e f e r e n c e v a l u e for d i s t a n c e f l o a t d i s t s t = 2; // P r e v i o u s d i s t a n c e f l o a t Ld = d i s t s t ; // M o m e n t a r y s e r v o p o s i t i o n int sp = 1 5 0 0 ; // P r e v i o u s s e r v o p o s i t i o n int spd = 1 5 0 0 ; // S i g n on bf d e c i d e s if d r i v i n g f o r w a r d or b a c k int bf =1; // T i m e r int t i m c n t ; int d e t e ; int t e t e = 1 1 0 0 ; int s e r s i g = 9 0 ; int s e r s i g d = 9 0 ; f l o a t kk = 7 . 5 ; f l o a t g a m m a k =0; f l o a t g a m m a p =0; int s v o l = 0; v o i d W r i t e _ 2 b y t e s ( b y t e d1 , b y t e d2 ) { W i r e . b e g i n T r a n s m i s s i o n ( s l a v e A d d r e s s ); W i r e . w r i t e ( d1 ); W i r e . w r i t e ( d2 ); W i r e . e n d T r a n s m i s s i o n (); } v o i d s e t u p () { // T h i s r e s u l t s in 0 x21 as the a d d r e s s to p a s s to TWI s l a v e A d d r e s s = I R s e n s o r A d d r e s s > > 1; S e r i a l . b e g i n ( 1 9 2 0 0 ) ; S e r i a l . p r i n t l n (" C L E A R D A T A "); S e r i a l . p r i n t l n (" LABEL , D i s t a n c e [ m ] , S e r v o ␣ P u l s e [ M i c r o S ] ,
APPENDIX C. CODE
M o t o r ␣ Speed , S t e e r i n g ␣ a n g l e ");
// Set the LED pin as o u t p u t
p i n M o d e ( ledPin , O U T P U T ); p i n M o d e (2 , O U T P U T ); // F o r w a r d s or b a c k w a r d s to m o t o r d r i v e r p i n M o d e (4 , O U T P U T ); p i n M o d e (9 , O U T P U T ); // S t e e r i n g s e r v o p i n M o d e (11 , O U T P U T ); // 5 V to the p o t e n t i o m e t e r p i n M o d e (6 , O U T P U T ); // A b s o l u t e v a l u e of m o t o r s i g n a l d i g i t a l W r i t e (11 , H I G H ); m y s e r v o . a t t a c h ( 3 ) ; // The c a m e r a s e r v o W i r e . b e g i n (); // IR s e n s o r i n i t i a l i z e W r i t e _ 2 b y t e s (0 x30 ,0 x01 ); d e l a y ( 1 0 ) ; W r i t e _ 2 b y t e s (0 x30 ,0 x08 ); d e l a y ( 1 0 ) ; W r i t e _ 2 b y t e s (0 x06 ,0 x90 ); d e l a y ( 1 0 ) ; W r i t e _ 2 b y t e s (0 x08 ,0 xC0 ); d e l a y ( 1 0 ) ; W r i t e _ 2 b y t e s (0 x1A ,0 x40 ); d e l a y ( 1 0 ) ; W r i t e _ 2 b y t e s (0 x33 ,0 x33 ); d e l a y ( 1 0 ) ; d e l a y ( 1 0 0 ) ; } v o i d l o o p () { d i g i t a l W r i t e (2 , H I G H ); l e d S t a t e = ! l e d S t a t e ; if ( l e d S t a t e ){ d i g i t a l W r i t e ( ledPin , H I G H ); } e l s e{ d i g i t a l W r i t e ( ledPin , LOW ); } // IR s e n s o r r e a d W i r e . b e g i n T r a n s m i s s i o n ( s l a v e A d d r e s s ); W i r e . w r i t e (0 x36 ); W i r e . e n d T r a n s m i s s i o n (); // R e q u e s t the 2 b y t e h e a d i n g ( MSB c o m e s f i r s t ) W i r e . r e q u e s t F r o m ( s l a v e A d d r e s s , 1 6 ) ; for ( i =0; i < 1 6 ; i ++) { d a t a _ b u f [ i ] = 0 ; } // T r a n s a t e I2C
APPENDIX C. CODE w h i l e( W i r e . a v a i l a b l e () && i < 16) { d a t a _ b u f [ i ] = W i r e . r e a d (); i ++; } Ix [0] = d a t a _ b u f [ 1 ] ; Iy [0] = d a t a _ b u f [ 2 ] ; s = d a t a _ b u f [ 3 ] ; Ix [0] += ( s & 0 x30 ) < <4; Iy [0] += ( s & 0 xC0 ) < <2; Ix [1] = d a t a _ b u f [ 4 ] ; Iy [1] = d a t a _ b u f [ 5 ] ; s = d a t a _ b u f [ 6 ] ; Ix [1] += ( s & 0 x30 ) < <4; Iy [1] += ( s & 0 xC0 ) < <2; Ix [2] = d a t a _ b u f [ 7 ] ; Iy [2] = d a t a _ b u f [ 8 ] ; s = d a t a _ b u f [ 9 ] ; Ix [2] += ( s & 0 x30 ) < <4; Iy [2] += ( s & 0 xC0 ) < <2; Ix [3] = d a t a _ b u f [ 1 0 ] ; Iy [3] = d a t a _ b u f [ 1 1 ] ; s = d a t a _ b u f [ 1 2 ] ; Ix [3] += ( s & 0 x30 ) < <4; Iy [3] += ( s & 0 xC0 ) < <2; m y s e r v o . w r i t e ( s e r s i g ); // Set s e r v o s i g n a l m = Ix [1] - Ix [ 0 ] ; // The d i s t a n c e b e t w e e n the l i g h t s d = ( Ix [ 0 ] + Ix [ 1 ] ) / 2 ; // The s k e w of the l i g h t s // C a l c u l a t e a n g l e to u s e r g a m m a = b e t a * d / n - b e t a /2; a l p h a = ( b e t a /2) * ( m / n ); // C a l c u l a t e d i s t a n c e L = abs ( 0 . 5 * b / tan ( a l p h a * 3 . 1 4 / 1 8 0 ) ) ; s v o l = a n a l o g R e a d ( A2 ); // R e a d p o t e n t i o m e t e r // C a l c u l a t e s e r v o a n g l e g a m m a p = - 1 8 0 . 0 * ( svol - 5 1 2 . 0 ) / ( 1 0 2 3 . 0 ) ; g a m m a k = - g a m m a + 0 . 3 * g a m m a p ; s e r s i g =3* g a m m a k + 9 0 ; // C a l c u l a t e c a m e r a s e r v o a n g l e
APPENDIX C. CODE // ksp =1500 - kk * g a m m a k ; // Set i n t e g r a t i n g p a r t of the m o t o r mhi = mhi +( L + Ld -2* d i s t s t )* d e t e / 4 0 0 0 0 ; m i n m o t = 2 0 0 ; if ( L < d i s t s t ){ m i n m o t = - m i n m o t ; } // ( mh - mhd )/ d e t e // KP = 300 , KI = 0 . 5 // Set m o t o r s i g n a l mh = m i n m o t + 3 0 0 * ( L - d i s t s t ) + 0 . 5 * mhi - m a x m o t + m a x m o t ;
// Set s e r v o signal , 1 5 0 0 is the m i d d l e for the s e r v o
sp = bf * 3 0 * ( gamma - g a m m a p + 1 0 ) + 1 5 0 0 ; Ld = L ; // R e s e t p r e v i o u s l o o p s d i s t a n c e to u s e r . // default , no s i g n a l is 1 0 2 3 if ( Ix [ 0 ] = = 1 0 2 3 && Ix [ 1 ] = = 1 0 2 3 ) { // Do no l i g h t s t u f f // S e r i a l . p r i n t (" I n g e n l a m p a "); mh = m a x m o t ; mhi = m a x m o t ; sp = 1 5 0 0 ; s e r s i g = s e r s i g d ; g a m m a =0; } if ( Ix [ 0 ] = = 1 0 2 3 && Ix [ 1 ] ! = 1 0 2 3 ) { // Do one l i g h t s t u f f // S e r i a l . p r i n t (" En l a m p a "); mh = m a x m o t ; mhi = m a x m o t ; sp = spd ; s e r s i g = s e r s i g d ; } if ( Ix [ 1 ] = = 1 0 2 3 && Ix [ 0 ] ! = 1 0 2 3 ) { // Do one l i g h t s t u f f // S e r i a l . p r i n t (" En l a m p a ");
APPENDIX C. CODE mh = m a x m o t ; mhi = m a x m o t ; sp = spd ; s e r s i g = s e r s i g d ; } d i g i t a l W r i t e (9 , H I G H ); d e l a y M i c r o s e c o n d s ( sp ); // S e r v o p u l s e d i g i t a l W r i t e (9 , LOW ); // D r i v e f o r w a r d och b a c k w a r d s if ( mh < m a x m o t ) { d i g i t a l W r i t e (4 , LOW ); bf = -1;} e l s e { d i g i t a l W r i t e (4 , H I G H ); bf = 1 ; } // L i m i t m a x i m a l m o t o r s p e e d if ( mh >2* m a x m o t ) { mh =2* m a x m o t ;} e l s e if ( mh <0) { mh = 0 ; } if ( mhi > 1 . 5 * m a x m o t ) { mhi = 1 . 5 * m a x m o t ;} e l s e if ( mhi < 0 . 5 * m a x m o t ) { mhi = 0 . 5 * m a x m o t ;} // L i m i t s t e e r i n g s e r v o a n g l e if ( sp > 2 0 0 0 ) { sp = 2 0 0 0 ; } e l s e if ( sp < 1 0 0 0 ) { sp = 1 0 0 0 ; } // L i m i t c a m e r a s e r v o a n g l e if ( sersig > 1 8 0 ) { s e r s i g = 1 8 0 ; } e l s e if ( sersig < 6 0 ) { s e r s i g = 6 0 ; }
APPENDIX C. CODE // M o t o r pwm // Set pwm for m o t o r d r i v e r a n a l o g W r i t e ( 6 , ( 2 5 5 . 0 / 1 0 0 0 . 0 ) * abs ( mh - m a x m o t )); S e r i a l . p r i n t ( d e t e ); S e r i a l . p r i n t l n (" ") S e r i a l . p r i n t (" DATA , TIME , "); S e r i a l . p r i n t ( L ); S e r i a l . p r i n t (" , "); S e r i a l . p r i n t ( ksp ); S e r i a l . p r i n t (" , "); S e r i a l . p r i n t ( mh ); S e r i a l . p r i n t (" , "); */ S e r i a l . p r i n t ( s e r s i g ); S e r i a l . p r i n t (" , "); S e r i a l . p r i n t ( s v o l ); S e r i a l . p r i n t (" , "); S e r i a l . p r i n t ( g a m m a p ); S e r i a l . p r i n t (" , "); S e r i a l . p r i n t ( g a m m a ); d i g i t a l W r i t e (9 , H I G H ); d e l a y M i c r o s e c o n d s ( sp ); // S e r v o p u l s e d i g i t a l W r i t e (9 , LOW ); S e r i a l . p r i n t ( mh ); S e r i a l . p r i n t (" , "); S e r i a l . p r i n t l n ( d e t e );
// T i m e for one l o o p in the c o d e
d e t e =( m i c r o s () - t i m c n t ); spd = sp ; // R e s e t the l a s t l o o p s s t e e r i n g s e r v o p u l s e s e r s i g d = s e r s i g ; // R e s e t the l a s t l o o p s c a m e r a s e r v o p u l s e mhd = mh ; // R e s e t the l a s t l o o p s m o t o r s i g n a l t i m c n t = m i c r o s (); // If a c o n s t a n t l o o p d u r a t i o n is n e e d e d // d e l a y M i c r o s e c o n d s (20000 - d e t e ); } }
