HAMMS - Diva1201180/FULLTEXT01.pdf · autonomous vehicles in four-way intersections SEBASTIAN...

IN DEGREE PROJECT TECHNOLOGY,FIRST CYCLE, 15 CREDITS

, STOCKHOLM SWEDEN 2017

HAMMSManaging a mix of human driven and autonomous vehicles in four-way intersections

SEBASTIAN LJUNGBERG

FREDRIK SCHALLING

KTH ROYAL INSTITUTE OF TECHNOLOGYSCHOOL OF INDUSTRIAL ENGINEERING AND MANAGEMENT

HAMMS

Managing a mix of human driven and autonomous vehicles in four-way intersections

SEBASTIAN LJUNGBERGFREDRIK SCHALLING

Bachelor’s Thesis at ITMSupervisor: Naveen MohanExaminer: Nihad Subasic

MMK 2017:35 MDAB 653

Abstract

The purpose of this report is to improve the flow of traf-fic in intersections through the use of a dynamic algorithm.People spend on average more than six weeks commuting towork in Stockholm every year. A large part of the time thatis spent in traffic is due to the time delay in intersections.

In this report, sensors that measure speed and distanceto the vehicle are used instead of detectors that only knowif a car is there or not. There are existing solutions that canoptimise the flow for autonomous cars but since the trafficmay consist of a mix of autonomous and human drivenvehicles during the upcoming 40 years those solutions arenot usable for some time.

In this work, a system that can handle both autonomousand human driven vehicles is created. The limitation of thesystem is that it can only receive two cars coming from dif-ferent directions simultaneously. The system does not workfor car queues. The system measures the speed of- and thedistance to the vehicles continuously.

According to the simulations that were made the algo-rithm that has been designed through this project is moretime efficient than the system that is in place today, assum-ing that the assumptions that were made for the currentsystem are correct.

The conclusion in this report is that it is possible tomake a system that is more time efficient than the one thatis in use today.

Keywords: Traffic Management Intersection System, Four-way intersection, Ul-trasonic Sensor, Autonomous Vehicle, Human driven

ReferatHAMMS: Korsningshantering för

blandningen av mänskligt och autonomtframförda fordon

Syftet med den här rapporten är att förbättra flödet ikorsningar genom en dynamisk algoritm. Människor sitterdrygt 6 veckor i bilköer varje år. En stor del av av denspenderande tiden i traffiken är på grund av att fordonbehöver stanna i korsningar.

I den här rapporten har sensorer som mäter hastighetoch distans använts istället för dagens detektorer som en-dast känner av om ett fordon kör över detektorn eller inte.Det finns andra rapporter med lösningar för att öka flödeti korsningar för självkörande bilar men om man kollar pådet kommande 40 åren kommer det troligtsvis att vara enblandning av självkörande och mänskligt körda bilar.

I det här arbetet skapas ett system som kan interageramed både mänskligt körda och autonoma bilar. Begräns-ningarna i det här systemet är att systemet endast kan taemot två bilar som kommer från olika ingångar i korsningensamtidigt. Systemet fungerar inte för bilköer. Systemet mä-ter den nuvarande hastigheten och distansen på fordonen.Systemet fungerar för alla olika kombinationer av mänskligtoch självkörande bilar.

Resultatet av den här rapporten är att en algoritm harutvecklats och är mer tidseffektivt än systemet som användsi Sverige idag, med våra antaganden om systemet som harutveklats i den här rapporten och systemet som användsidag. Resultatet är baserat på korsningar där bara två bilarmöts utan köer.

Slutsatsen av den här rapporten är att det är möjligtatt göra ett system som är mer tidseffektivt än systemetvi använder oss av idag, men vi kan inte säkertsätlla attsystemet i den här rapporten är mer robus och driftsäkertän det som används i Sverige idag.

Acknowledgements

We would like to thank the KTH ITM MDA and the examiner Nihad Subasic for aninspiring and worthwhile course, the lab assistants and the lab engineers for helpingout in the laboratory hall.

We especially want to thank our supervisor Naveen Mohan for great feedbackduring the course, to help us focus on the valuable things and inspiring us to workharder and do a better job in this thesis.

Sebastian Ljungberg and Fredrik SchallingStockholm, June 2017

v

Nomenclature

Abbreviation

AIM Autonomous Intersection Management

ETA Estimated Time of Arrival

GND Electrical Ground

GUI Graphical User Interface

HAMMS Human and Autonomous Mixed Management System

ITM School of Industrial Engineering and Management

LED Light Emitting Diode

MDA Division of Mechatronics

TMS Traditional Management System

USS Ultrasonic Sensor

Symbols

g Standard acceleration due to gravity, ≈ 9.81m/s2

KD differential coefficient in a PD controller

Kp proportional coefficient in a PD controller

Vocabulary

HAMMS The intersection which is developed in this project.

LHOV RA An intersection management theory that is applied in Sweden as wellas in other countries. [15]

ping The process of sending and receiving a signal in for example an ultrasonicsensor.

vii

Contents

1 Introduction 11.1 Background . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11.2 Purpose and Scope . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11.3 Method . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2

2 Theory 32.1 Related Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32.2 Distance measuring techniques . . . . . . . . . . . . . . . . . . . . . 4

2.2.1 Ultrasonic sensor . . . . . . . . . . . . . . . . . . . . . . . . . 42.2.2 Induction Loop sensor . . . . . . . . . . . . . . . . . . . . . . 42.2.3 Radar . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5

2.3 Vehicle manoeuvring . . . . . . . . . . . . . . . . . . . . . . . . . . . 62.3.1 Feedback Controller . . . . . . . . . . . . . . . . . . . . . . . 62.3.2 Acceleration and deceleration . . . . . . . . . . . . . . . . . . 7

2.4 System used today . . . . . . . . . . . . . . . . . . . . . . . . . . . . 72.4.1 Concept . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 72.4.2 Components . . . . . . . . . . . . . . . . . . . . . . . . . . . 82.4.3 Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8

3 Demonstration 113.1 Problem formulation . . . . . . . . . . . . . . . . . . . . . . . . . . . 113.2 Concept . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 113.3 Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 123.4 Software Modelling . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14

3.4.1 Chosen Parameters . . . . . . . . . . . . . . . . . . . . . . . . 143.4.2 Implementation in Java . . . . . . . . . . . . . . . . . . . . . 143.4.3 GUI . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 153.4.4 Test Cases . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16

3.5 Setup/Demonstrator . . . . . . . . . . . . . . . . . . . . . . . . . . . 173.5.1 Microcontroller . . . . . . . . . . . . . . . . . . . . . . . . . . 173.5.2 Intersection . . . . . . . . . . . . . . . . . . . . . . . . . . . . 183.5.3 Vehicles . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 203.5.4 Implementation in C . . . . . . . . . . . . . . . . . . . . . . . 22

viii

3.6 Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 233.6.1 Practical tests . . . . . . . . . . . . . . . . . . . . . . . . . . 233.6.2 Quantitative tests . . . . . . . . . . . . . . . . . . . . . . . . 23

4 Discussion/Conclusion 254.1 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26

5 Future Work 27

Bibliography 29

Appendices 30

A Graphs over test cases 31

B Java code used in simulation 41

C C code used in the demonstrator 61

List of Figures

2.1 Diagram of the intersection system. [23] . . . . . . . . . . . . . . . . . . 32.2 Ultrasonic sensor. [17] . . . . . . . . . . . . . . . . . . . . . . . . . . . . 42.3 Induction Loop sensor. [1] . . . . . . . . . . . . . . . . . . . . . . . . . 52.4 Doppler effect by a right-moving source. [9] . . . . . . . . . . . . . . . . 52.5 A flowchart of feedback control in general. [11] . . . . . . . . . . . . . . 62.6 A general schematic view of a closed loop feedback controller. [13] . . . 62.7 Concept of the current systems for intersections. [12] . . . . . . . . . . 82.8 A normal working method. [10] [15] . . . . . . . . . . . . . . . . . . . . 9

3.1 The concept of the intersection with the system HAMMS. [12] . . . . . 123.2 The algorithm for HAMMS. [14] . . . . . . . . . . . . . . . . . . . . . . 133.3 UML class diagram over Java implementation. [6] . . . . . . . . . . . . 153.4 A screen shot of GUI after test case 1 is executed. [7] . . . . . . . . . . 153.5 Intersection for the different test cases. [12] . . . . . . . . . . . . . . . . 163.6 Schematic over electronics. [5] . . . . . . . . . . . . . . . . . . . . . . . 193.7 Demonstrator, "intersection". [3] . . . . . . . . . . . . . . . . . . . . . . 203.8 Demonstrator, "autonomous car". [3] . . . . . . . . . . . . . . . . . . . . 213.9 Demonstrator, "human driven car". [3] . . . . . . . . . . . . . . . . . . . 22

A.1 Graph of testcase 1 for HAMMS. [8] . . . . . . . . . . . . . . . . . . . . 32A.2 Graph of testcase 1 for TMS. [8] . . . . . . . . . . . . . . . . . . . . . . 33A.3 Graph of testcase 2 for HAMMS. [8] . . . . . . . . . . . . . . . . . . . . 34A.4 Graph of testcase 2 for TMS. [8] . . . . . . . . . . . . . . . . . . . . . . 35A.5 Graph of testcase 3 for HAMMS. [8] . . . . . . . . . . . . . . . . . . . . 36A.6 Graph of testcase 3 for TMS. [8] . . . . . . . . . . . . . . . . . . . . . . 37A.7 Graph of testcase 4 for HAMMS. [8] . . . . . . . . . . . . . . . . . . . . 38A.8 Graph of testcase 4 for TMS. [8] . . . . . . . . . . . . . . . . . . . . . . 39

x

List of Tables

2.1 The proportional and differential effects on the system. . . . . . . . . . 62.2 Human parameters. [15] . . . . . . . . . . . . . . . . . . . . . . . . . . . 72.3 Parameters set for algorithm used today. [15] . . . . . . . . . . . . . . . 9

3.1 Parameters set for HAMMS. . . . . . . . . . . . . . . . . . . . . . . . . 123.2 Setup parameters for testcase 1. . . . . . . . . . . . . . . . . . . . . . . 163.3 Setup parameters for testcase 2. . . . . . . . . . . . . . . . . . . . . . . 173.4 Setup parameters for testcase 3. . . . . . . . . . . . . . . . . . . . . . . 173.5 Setup parameters for testcase 4. . . . . . . . . . . . . . . . . . . . . . . 173.6 Summary of ten test runs of Case 1. . . . . . . . . . . . . . . . . . . . . 233.7 Result of test case 1: Regular passage, low margin. . . . . . . . . . . . 233.8 Result of test case 2: Regular passage, larger margin. . . . . . . . . . . 243.9 Result of test case 3: Bad road condition. . . . . . . . . . . . . . . . . 243.10 Result of test case 4: Different prioritisation. . . . . . . . . . . . . . . . 24

xi

Chapter 1

Introduction

1.1 BackgroundA recent study on commutation to work [20] shows that people in Stockholm spendon average 6.4 weeks commuting to work per year. A great part of the time is spentin either tailbacks or intersections. Traffic lights in Sweden are today controlled bya binary trigger under the road. This means that the traffic lights do not considerthe following data of approaching vehicles:

• Its speed.

• Its distance to the intersection.

• Whether it is autonomous or driven by a human.

• Number of queueing vehicles.

1.2 Purpose and ScopeThe purpose of this paper was to make the intersections more time efficient byusing ultrasonic sensors instead of in-pavement detectors. The report will present adynamic algorithm for priority management which is taking distance and speed ofvehicles into account. The report will also discuss the benefits of using this “smart”intersection.

The research is focused on the traffic light system and its communication withboth driverless and human driven vehicles. The report is also limited to studyingone case; two cars from different directions crossing the intersection. Therefore, thesystem that is created in this study will not take queues into account. This studyis set to explore how to design an algorithm that takes more data, than the existingtraffic lights do, into account. The variables that is taken into consideration foreach vehicle that approaches the traffic lights are:

• Its speed.

1

CHAPTER 1. INTRODUCTION

• Its distance to the intersection.

• Whether it is autonomous or driven by a human.

Parameters that is not taken into consideration are:

• Receive more than two cars coming from different directions.

• Not applicable on queues of cars.

The question that will be answered in this study is:How much would a smart algorithm reduce the passage time in an two car intersec-tion compared to the system used today?

1.3 MethodTwo simulation algorithms were created one for the system that is in place todayand one for the system that was designed through this study. A demonstratorwas then built to illustrate how the new system and algorithm works as well as toexamine the benefits and defects of it.

2

Chapter 2

Theory

2.1 Related WorkA group of researchers at a project called Autonomous Intersection (AIM) has cre-ated several systems for autonomous intersection management. In the report [23],AIM was used for semi-autonomous vehicles to create an intersection with trafficlights. The paradigm that was created proceeds as follows:

1. An approaching vehicle announces its impending arrival to the intersectionmanager.

2. The vehicle indicates its size, predicted arrival time, velocity, acceleration,arrival and departure lanes.

Figure 2.1 shows the concept of an autonomous vehicle connecting with thisinfrastructure.

Figure 2.1. Diagram of the intersection system. [23]

The advantage with this intersection is, according to the results, that the sys-tem can greatly decrease traffic delays with semi-autonomous and fully-autonomous

3

CHAPTER 2. THEORY

vehicles. On the other hand, in order for this to work all of the cars have to beconnected to the infrastructure. Since there is hard for a human driver to adapt tocomplex system at this extent it would difficult to implement AIM in traffic today.

According to a recent study [16], fully autonomous vehicles for everyday useare going to be on the market around 2020 to 2030 and autonomous vehicles willbe affordable first around 2040 to 2050. Somewhere between 2040 and 2060 themajority of cars will be autonomous. The study’s prediction reveals the great useof a system for an intersection that works for both human driven and autonomousvehicles before 2060, when cars will be mainly autonomous. When all the cars areautonomous the project AIM will be a better use than the system that is createdin this study.

2.2 Distance measuring techniquesHaving good techniques for measuring the distance is a central part in creatingintersections. Here are some of the most efficient techniques.

2.2.1 Ultrasonic sensorAn ultrasonic sensor (USS), shown in Figure 2.2, is a device that can measuredistance between itself and an object trough the use of sound waves.

Figure 2.2. Ultrasonic sensor. [17]

The USS is often used to measures distance by sending a sound pulse and mea-sure the time it takes until the pulse have bounced back. With the assumption ofconstant speed of sound the distance could be calculated by Formula 2.1,

D = vsound · t

2 . (2.1)

2.2.2 Induction Loop sensorThe induction loop sensor is an electrical device that transmits energy into a wireloop. When an electrical conductor is crossing (vehicle is driving over the sen-sor), the wired loop generates energy from the electrical conductors and creates an

4

2.2. DISTANCE MEASURING TECHNIQUES

electromagnetic field under the vehicle. The sensor measures the level of the elec-tromagnetic field, as shown in Figure 2.3, below and giving back a digital signal. Itsignals that either a vehicle is driving through the area of the induction loop sensoror that is is not a moving vehicle over the induction loop area.

Figure 2.3. Induction Loop sensor. [1]

2.2.3 RadarA radar is sending out a signal and measuring the time it takes to return. In a radarthis, either a pulse or a continuous signal, is electromagnetic waves in microwave orradio domain.

When a surface reflecting waves is moving these waves will have a altered wavelength as shown in Figure 2.4. The effect of altering wave length by movement iscalled the Doppler effect. [19] By using this effect a radar could be used in a simpleway be used to track the direction and speed of a moving object in close to real time.This since phase shift is technically simple to compute and analyse and thereforeadds more significance to computed values. [18]

Figure 2.4. Doppler effect by a right-moving source. [9]

5

CHAPTER 2. THEORY

2.3 Vehicle manoeuvring

2.3.1 Feedback ControllerA dynamic system is often designed without the exact knowledge of the systemsbehaviour. As a cause of this, it is not possible to optimise a static transformationbetween the reference signal and the input signal on beforehand. Therefore it isbeneficial to create a feedback loop that defines the desired output signal as afunction of the error in real time. This method is often described as in Figure 2.5

Figure 2.5. A flowchart of feedback control in general. [11]

To apply this concept in mechatronics a PD-controller is often used. This con-troller has a proportional part and a differential part. The proportional controllerwill increase the control signal proportionally to the difference between the desiredoutput and the actual output. The derivative action will have the effect of in-creasing the stability of the system, reducing overshoot and improving the transientresponse. These effects are summarised in Table 2.1:

Rise Time Overshoot Settling Time Steady-State errorKp Decrease Increase Small change DecreaseKD Small Change Decrease Decrease No Change

Table 2.1. The proportional and differential effects on the system.

A general schematic view over the closed loop is shown in Figure 2.6:

Figure 2.6. A general schematic view of a closed loop feedback controller. [13]

The way a controller is designed is to adjust the constants Kp and KD. To designthe controller usual requirements are maximum levels of overshoot, rise time andsettling time. The mathematical algorithm is formulated as [21],

u(t) = Kp · e(t) + KD · de(t)dt

. (2.2)

6

2.4. SYSTEM USED TODAY

2.3.2 Acceleration and decelerationA formula for reaction time and acceleration/deceleration distance is,

Dacc = v0tr + v02

2a. (2.3)

In [15] it is assumed that the reaction time and the deceleration for strongbraking is:

Tab Value UnitReaction time 0.75 sBreaking factor 4.905 m/s2

Table 2.2. Human parameters. [15]

2.4 System used todayThis section contains a simplified explanation of the system that is being used todaythat is trough this report called Traditional management system (TMS). In TMSan intersection prioritises larger vehicles, like trucks and buses, and roads are notin level of consideration. In Sweden, where this system is implemented, there aredifferent intersections with varied amounts of detectors before the intersection. Thefocus in this study has however been on the system that is the most common; twodetectors before the intersection.

2.4.1 ConceptIn the algorithm that is used today, the system has different states depending onwhat is happening. A normal intersection has two detectors [15], one 80 metersbefore the intersection and a few detectors from 20 to 0 meters ahead. If one cardrives over the detector that lies 80 meters ahead of the intersection, before thesecond car, the first car will be prioritised first by the system. This concept isshown in Figure 2.7:

7

CHAPTER 2. THEORY

Figure 2.7. Concept of the current systems for intersections. [12]

2.4.2 ComponentsThe detectors in the traditional system are induction loop detectors, that are hiddenunder the ground.

2.4.3 AlgorithmIn the traditional system initiation value of the intersection is red. When a carwith a higher priority is approaching the intersection, the lights change from redto red-yellow to green. This sequence is shown in Figure 2.8 where the states arebased on a static routine.

8

2.4. SYSTEM USED TODAY

Figure 2.8. A normal working method. [10] [15]

From the point when the first car gets the priority a timer starts. The lightschange from Red to Red-yellow directly, that is the first state and after a given timethe state is changed to green, which is the second state. After a certain amount oftime the light changes to yellow, that is state three. Which works as a safety marginso that all the cars that could not drive through the green light should have timeto choose whether to brake or drive through the intersection. After the third statethe system goes into Overlap red time, the fourth state where yet another safetymargin exists. During overlap red time both cars are given red light for a moment.Table 2.3 shows the different time dependency for different parameters and speedlimits.

State 50km/h 70km/hRed and yellow light 0.7s 0.87sMinimum green time 2.1s 2.61sMax green time 6.125s 6.125sYellow time 4s 5sOverlap red light 1.5s 1.5s

Table 2.3. Parameters set for algorithm used today. [15]

In the table above, the time for the states is both depending on the roads speedlimit and whether a vehicle gets the priority when passing the first detector, 80meters away, or when passing the second detector, 20 meters before the intersection.If the car gets priority when passing the first detector the state max green time ischosen but if a car is getting the priority when passing the second detector thestate minimum green time is chosen. The reason why there is a time differencebetween the states is for optimising the algorithm so a car shall have time to crossthe intersection.

9

Chapter 3

Demonstration

3.1 Problem formulationA demonstrator were built to illustrate the smart algorithm and hence had to beable to:

• Detect cars, both autonomous and those driven by a human.

• Measure distance and speed.

• Give signals to avoid collision.

• Controlling the autonomous vehicle.

• Recognise when a car drives through the intersection.

The cars were detected using two ultrasonic sensors that send a list of times tothe computer in the intersection, the Arduino.

3.2 ConceptIn HAMMS (Human and Autonomous Mixed Management System), the new systemthat is proposed in this study, incoming vehicles are monitored by ultrasonic sensors.Every lane has one traffic light and one ultrasonic sensor.

When a vehicle approaches the intersection a sensor is used to measure thespeed and the distance to the vehicle. The sensor sends the information to theintersection’s computer. The computer receives the data and changes the state ofthe traffic lights according to the situation. If the vehicle is driven autonomouslythe intersection sends a reference speed to the vehicle. This concept is shown inFigure 3.1.

11

CHAPTER 3. DEMONSTRATION

Figure 3.1. The concept of the intersection with the system HAMMS. [12]

3.3 AlgorithmIn the HAMMS, the initial value is green and the states change depending on thedifference between the two approaching vehicle’s Estimated time of Arrival (ETA).The advantage with this system is that the ETA value is updated all the time. Thesystem prioritises the car with the lowest ETA. If the difference in ETA, betweenthe cars, is at least 3 seconds both cars will only see green light. If the cars aredriving in 50 km/h this would mean that the distance between the cars is almost42 m which is considered as a sufficient safety margin. Table 3.1 shows the marginsfor different states where the state changes for the vehicle with longest ETA.

State Parameter Timediff

Distancediff50km/h

Distancediff70km/h

Green Sufficient difference in ETA >3 s >42 m >58 mYellow Low difference in ETA ≤3 s ≤42 m ≤58 mRed Critical difference in ETA <1 s <14 m <19 m

Table 3.1. Parameters set for HAMMS.

The vehicle with the shortest ETA only has state green. State yellow in HAMMS

12

3.3. ALGORITHM

does not represent the yellow state in the traditional System. In the traditionalsystem yellow represents a safety margin but in HAMMS the yellow light meansthat the car should engine break for human driven vehicles and the computer sendsa reference speed to autonomous vehicles. The idea of changing the function forthe yellow light was to give the drivers more signals on when to both brake anddrive faster to receive a more even flow. Figure 3.2 shows an overview of how thealgorithm works.

Figure 3.2. The algorithm for HAMMS. [14]

Figure 3.2 can also be summarised as:

1. Update ETA.

2. Check if a vehicle is detected.

3. If a vehicle is detected, look if there are multiple vehicles approaching.

4. Compare ETA and send suitable states to the vehicles.

13


3.4 Software ModellingTo further test HAMMS’ durability a quantitative comparison was performed be-tween the algorithm used to lead traffic flow today and the one used in HAMMS.Java was chosen as language since it has great libarys for graphical interface devel-opment which the group had prior experience of.

3.4.1 Chosen ParametersSince human behaviour is complex, the choice was made to fix a few parameters.

• A red light makes a driver decelerate with 0.3 · g. This is a little below whatis today considered as maximum breaking [22] p.116.

• When a yellow light is shown a driver releases the throttle and this gives aacceleration of −g.

• At a green light a human driver accelerates up to the same speed it hadinitially with the same pace it breaks with at a red light.

3.4.2 Implementation in JavaThe program was structured so that the model of each algorithm takes objects ofcars and speed limit as parameter. Identical objects of these are sent to respectiveintersection model to start a test. The car objects then collect data as it is simulatedthrough the respective intersection passage. This allows the user to, evaluate thesetup by extracting data such as change of speed over time, total passage time andinput signal over time. The result is presented to observers in a simple graphicaluser interface, GUI.

The tests were made in cycles in which each of the car objects are sent the realtime and either a light signal or if autonomous and in HAMMS, a reference speed.The car then calculates the time passed since last update and updates speed andmoves forward accordingly. For example, if the signal given a red signal the speed isreduced and the distance from the intersection is lowered accordingly, as you wouldexpect a normal driver and car to react.

The written structure as show in Figure 3.3 presents central variables and func-tions used in the tests. For more information about the simulation code, see Ap-pendix B.

14

3.4. SOFTWARE MODELLING

Figure 3.3. UML class diagram over Java implementation. [6]

3.4.3 GUIA GUI was built in Java to easier demonstrate the comparisons made betweenthe traditional system and HAMMS, and to make the executions of tests trivial.The user can give five input arguments; speed and distance for each vehicle andthe speed limit for the testing environment. As Figure 3.4 shows six outputs ispresented after a performed test. The user will be able to read each passage timeand how the different system performed as a time difference.

Figure 3.4. A screen shot of GUI after test case 1 is executed. [7]

15


3.4.4 Test CasesAll tests were conducted as a set of different parameters for each car as shown inFigure 3.5.

Figure 3.5. Intersection for the different test cases. [12]

Case 1: Regular passage, low margin

The first case is what is supposed to be two regular cars in ordinary speed approach-ing an intersection with a high risk of collision. Speed limit 50 km/h.

Vehicle Distance[m]

Speed[km/h]

Car 1 85 50Car 2 95 50

Table 3.2. Setup parameters for testcase 1.

Case 2: Regular passage, larger margin

This case is a version of case 1 but with a larger margin between the vehiclesrespective ETA of about 3 s, just above what is said to be a sufficient margin.Speed limit 50km/h.

16

3.5. SETUP/DEMONSTRATOR

Vehicle Distance[m]

Speed[km/h]

Car 1 80 50Car 2 125 50


Case 3: Bad road condition

In case three one car, drives really slow. Example of this could be bad road con-ditions. Still the slow driving Car 1 will reach the detector at 80 meter first andbe given priority. Since the traditional system does not read what speed the car isdriving at and instead assumes it is close to the speed limit the car will not haveenough time to pass the intersection before the light turns red again. Speed limit50km/h.

Vehicle Distance[m]

Speed[km/h]

Car 1 85 25Car 2 100 50


Case 4: Different prioritisation

The last simulated test case showcases a situation where different vehicles will begiven priority in the intersection. Since the slower driving Car 2 passes the detectorat 80 meter before the faster Car 1 it is given priority in the traditional intersectionmanagement system even though the faster car is driving at the speed limit andwould reach the intersection first. Speed limit 70km/h.

Vehicle Distance[m]

Speed[km/h]

Car 1 85 70Car 2 80 50


3.5 Setup/Demonstrator

3.5.1 MicrocontrollerThe Arduino™ serie were chosen as the testing environment since the departmenthas previous experience of working with it. The Arduino UNO is a commonlyused circuit board for software and hardware development. The board has the

17


ATMEGA328P microprocessor implemented and has 14 digital I/O ports and sixanalog inputs. The code is usually written in the open-source Arduino IDE whichoperates is an extended C library. [2]

3.5.2 IntersectionA demonstrator of the intersection was built to visualise the intersection in reality.Two traffic lights were created. First, two wooden boxes were sawed out. Second,the holders for the led lights and ultrasonic sensors were created in Solid Edge ST7.Third, all of the electrical parts were assembled. The electrical parts were controlledby an Arduino UNO that was connected to each component. A visualisation of thesecan be seen in Figure 3.6:

18


Figure 3.6. Schematic over electronics. [5]

Lastly, all of the electrical parts and the traffic lights were glued added on to asmall intersection shown in Figure 3.7.

19


Figure 3.7. Demonstrator, "intersection". [3]

3.5.3 VehiclesTwo cars were created to visualise the intersection, one “autonomous car” and one“Human driven car”. The “autonomous car” was controlled by an Arduino Uno,the rear wheel was driven by two motors, one for each rear wheel. A white frontplate was built so the ultrasonic sensor could easily detect the car. Figure 3.8 showsthe demonstrator of the “autonomous car”.

20


Figure 3.8. Demonstrator, "autonomous car". [3]

The “human driven car” was created with four motors, one for each wheel. The“human driven car” has no microcontroller that controls the robot and is thereforeonly able to drive forward. The demonstrator for the “human driven car” is shownin Figure 3.9.

21


Figure 3.9. Demonstrator, "human driven car". [3]

3.5.4 Implementation in CThe program was divided into three main categories; mainContent, updateFunctionsand calcFunctions. With this structure is was possible to tweak the functionalityand still keep track of the rising number of functions.

To call the data updated in close to “real-time”, several pings per USS perseconds were demanded. The well-known library Ping does not support fast pingswith multiple USS and could therefore not be used. Instead the library NewPing [4]from Arduino Playground, the user community of Arduino, was used. This libraryfulfils the set requirements and additionally has an easy to use interface.

The speed was measured from a set of timed pings is done by the USS, fromwhich speed is derived trivially. For more information about the code for the demon-strator, see Appendix C.

22

3.6. RESULTS

3.6 Results

3.6.1 Practical testsWith our demonstrator a few experiments were conducted to test the robustness ofHAMMS in this implementation. To compare theory and real implementation tentest runs of Case 1, defined as Section 3.4.4 were executed and filmed. Fluctuationsof the actual margin in each run were measured and documented. A summary isseen in Table 3.6 for the full documentation see Appendix A.

Margin [m]Mean 39Maximum 50Minimum 25Theoretical mean 42

Table 3.6. Summary of ten test runs of Case 1.

3.6.2 Quantitative testsFour test cases have been simulated and is presented below. Graphs of how speedand distance changes over time in respective test case is found in Appendix 1. Thesituations tested are described and defined in Section 3.4.4. This result is presentedbelow as passage times for respective car and the difference,

∆t = passage time in HAMMS − passage time in TMS. (3.1)

Result of Case 1: Regular passage, low margin

Car 2 has to pass through a red light and will as a cause of this break down andalmost has to take a complete stop. When Car 2 is given green light Car 1 is almost140 meter away.

Vehicle HAMMS TMS ∆tCar 1 6.1s 6.1s 0sCar 2 8.1s 15.5s -7.4sTotal 14.2s 21.6s -7.4s

Table 3.7. Result of test case 1: Regular passage, low margin.

Result of Case 2: Regular passage, larger margin

With a 45 meter difference Car 2 still gets caught in red light and as in Case 1 needsto break and wait.

23


Vehicle HAMMS TMS ∆tCar 1 5.9s 5.9s 0sCar 2 9.1s 15.7s -6.6sTotal 15.0s 21.6s -6.6s

Table 3.8. Result of test case 2: Regular passage, larger margin.

Result of Case 3: Bad road condition

The car given priority drives to slow to have time to pass through and as a conse-quence both car get stuck at red light. As apposed in HAMMS there would actuallybe a large margin between them when they arrive to the intersection, so both areshown green.

Vehicle HAMMS TMS ∆tCar 1 12.3s 23.7s -11.4sCar 2 7.3s 16.0s -8.7sTotal 19.6s 39.7s -20.1s

Table 3.9. Result of test case 3: Bad road condition.

Result of Case 4: Different prioritisation

Even though Car two is driving at the speed limit the car with both slower andwith lower ETA is prioritised in the traditional way of managing traffic

Vehicle HAMMS TMS ∆tCar 1 4.4s 14.6s -10.2sCar 2 6.7s 5.8s +0.9sTotal 11.1s 23.4s -9.3s

Table 3.10. Result of test case 4: Different prioritisation.

24

Chapter 4

Discussion/Conclusion

The results imply that it is possible to improve passage times relative the sys-tem that is used today in Sweden. Clear disadvantages of not knowing speed anddistance of approaching vehicles is displayed in chosen tests. In clear words a pri-oritising algorithm will benefit from analysing more data. More data also shows forexample in Case 4 how priority management would be more equitable

A dynamic management also proves smoother passages and less need of hardbreaks. As commonly known sudden changes in speed could evolve into large queues.So even though this paper only directly analysed two vehicles situations the resultcould conduce to reduce queue formation. The reduced need of large margins isan additional queue reducing factors since with more sensors the system could keepbetter track of all approaching vehicles. However the practical tests with the demon-strator showed large deviations in actual margin between passing vehicles. This isalarming results since all reactions are well defined and preprogramed in these tests.In a real world with real human drivers could react even more extreme. If HAMMSwere to be implemented in a real system it would be critical to analyse how well thefixed parameters are. All that is known now is that our models of human behaviourare probably a fair estimation, but only an average value.

Suppose safety margins where to be kept at the levels used today, still positiveaspects in more data collections is seen. To basically force vehicles to eco driveby giving incentive to slow down sufficiently long before the intersection, actuallycreates larger margins in distance. Advantages with this is not only does it reduceswear on both road and vehicle, it could also accidents. The larger margins indistance between two approaching vehicles appear when one is slowing down muchearlier in the lane. This could prevent some of the effect of a misjudgement of roadconditions. For example, if a vehicle skids it would be preferable if this happensupstream in a lane to decrease the risk of a potentially lethal unintended crossingof red light.

To further set the saved seconds in perspective, some comparisons could be done.For one person this change could make up to one workday extra per year as seen inEquation 4.1,

25

CHAPTER 4. DISCUSSION/CONCLUSION

10 intersections · 5.5 seconds · 2tripsday · 261workdaysyear ≈ 8hoursyear . (4.1)

Beside the potential benefits a system as HAMMS could have today, the possi-bility to communicate with the arising autonomous vehicle trend needs to be takeninto account. The system used in traditional intersections is developed to suit hu-man drivers and them alone. A step in the direction of for instance the AIM projectat Texas, the infrastructure need to work in small increments that have a large phys-ical potential of further development. It is not feasible to change traffic over nightbut the work of developing infrastructure could benefit by follow the same path asits users.

4.1 ConclusionIf more data is collected more precise directions could be given to road users.Though, if margins are reduced to much the robustness of the system could belost. But result show that HAMMS is managing the traffic more efficiently than themodel of the system used today with the set scope and that this is a consequenceof more dynamic management based on more data.

26

Chapter 5

Future Work

During this project it was important to get a good understanding of all of the parts;ultrasonic sensors, induction loop sensors and doppler radars, even though we onlyused ultrasonic sensors. The reason that ultrasonic sensors were chosen was becausethey are both easy to manage and cheap. Unfortunately they cannot reach a levelof preciseness that is acceptable in reality. There is a huge difference between theultrasonic sensors and induction loops when it comes to reliability, induction loopsare incredibly accurate in their judgement, even in extreme weather conditions.They are also cheap to manage since they do not need any maintenance. Sinceinduction loops are much better than ultrasonic sensors when it comes to reliabilitythey could be a possible alternative for the sensors, if this system is going to beimplemented in reality. If the inductions loops are placed at more than two pointsbefore the intersection, more data would be collected. An equal result that wasachieved with the sensors would be reached, however this would result in highercosts when building intersections.

Because of the increase in cost different radar setups might be a more sufficientalternative. The radar is very precise, it measures both speed and distance andis also easier to replace and modify. An advantage with the radar, compared toultrasonic sensors, is that it is able to know the size of the object- and what kindof object that it is aiming on. These applications are also necessary in order forHAMMS to work.

There is a security system that is used in Sweden today called LHOVRA [15].LHOVRA is used to decrease the weaknesses in the system for intersections byoptimising the security and the flow of the system. This is done by:

• Prioritising trucks and buses.

• Prioritising main roads.

• Reducing the amount of accidents.

These interventions are made in detail and it is possible to implement them inHAMMS.

27

Bibliography

[1] Tarun Agarwal. Available at: https://www.elprocus.com/dynamic-road-traffic-signal-control/, 2017. [Accessed 2017-06-02].

[2] Arduino. Arduino uno. https://www.arduino.cc/en/Main/ArduinoBoardUno, 2017. [Accessed 2017-05-21].

[3] Captured 2017 at KTH. [Captured 2017-04-15].

[4] Time Eckel. New ping. http://playground.arduino.cc/Code/NewPing,2016. [Accessed 2017-05-15].

[5] Created in Fritzing 2017. [Created 2017-04-15].

[6] Created in Gliffy. [Created 2017-05-01].

[7] Created in Java JDK 8 development kit. [Created 2017-05-07].

[8] Created in Microsoft Excel 2017. [Created 2017-04-14].

[9] Created in Microsoft Paint. [Created 2017-05-15].

[10] Created in Microsoft Paint 2017. [Created 2017-05-19].

[11] Created in Microsoft PowerPoint 2017. [Created 2017-05-15].

[12] Created in Microsoft PowerPoint 2017. [Created 2017-04-15].

[13] Created in Microsoft Visio 2017. [Created 2017-05-15].

[14] Created in Microsoft Visio 2017. [Created 2017-04-15].

[15] Bengt Skarersjö Jan Moberg. Vägar och gators utformning. Technical Report114, Svenska kommunförbundet, Vägverket, Stockholm, 2004.

[16] Todd Litman. Autonomous vehicle implementation predicitions. TechnicalReport 15, Victoria Transport Policy Institute, USA, 02 2017.

[17] Dejan Nedelkovski. Available at: http://howtomechatronics.com/tutorials/arduino/ultrasonic-sensor-hc-sr04/, 2017. [Accessed 2017-06-02].

29

https://www.elprocus.com/dynamic-road-traffic-signal-control/

https://www.elprocus.com/dynamic-road-traffic-signal-control/

https://www.arduino.cc/en/Main/ArduinoBoardUno

https://www.arduino.cc/en/Main/ArduinoBoardUno

http://playground.arduino.cc/Code/NewPing

http://howtomechatronics.com/tutorials/arduino/ultrasonic-sensor-hc-sr04/

http://howtomechatronics.com/tutorials/arduino/ultrasonic-sensor-hc-sr04/

BIBLIOGRAPHY

[18] National Oceanic and Atmospheric Administration. How radar works. http://www.srh.noaa.gov/jetstream/doppler/how.html, 2017. [Accessed 2017-06-02].

[19] Florian Ion Petrescu. A New Dopple Effect. Norderstedt, 2012.

[20] Göran Tegner. Restider till arbete i svenska regioner perioden 1996 till 2011.Technical report, WSP Analysis & Strategy, Stockholm, 05 2012.

[21] Lennart Ljung Torkel Glad. Reglerteknik : grundläggande teori, volume 4.Studentlitteratur AB, 2006.

[22] Trafikverket. Krav för vägars och gators utformning. Technical report, 2015.

[23] Peter Stone Tsz-Chiu Au, Shun Zhang. Autonomous intersection managementfor semi-autonomous vehicles. Technical Report 3, The University of Texasat Austin U.S.A., Ulsan National Institute of Science and Technology SouthKorea, USA, 2015.

30

http://www.srh.noaa.gov/jetstream/doppler/how.html

http://www.srh.noaa.gov/jetstream/doppler/how.html

Appendix A

Graphs over test cases

The following figures show the speed-time- and distance-time -chart for each testcase. When the distance is zero the car is arriving the intersection. The blue linerepresent Car 1 and the orange line represents Car 2. The Speed axis is in m/s andthe time axis is in seconds. Distance is measured in meter. Figures A.1 and A.2shows the distance and velocity varied for different times for testcase 1.

31

APPENDIX A. GRAPHS OVER TEST CASES

Figure A.1. Graph of testcase 1 for HAMMS. [8]

32

Figure A.2. Graph of testcase 1 for TMS. [8]

Figures A.3 and A.4 shows the distance and velocity varied for different timesfor testcase 2.

33



34



35



36



37



38


39

Appendix B

Java code used in simulation

///////////////////////////////////////////////////////////// HAMMS: Human and Autonomous Mixed Management system //// Bachelor thesis in Mechatronics //// Authors: Sebastian Ljungberg & Fredrik Schalling //// TRITA MMK 2017:35 MDAB 653 //// File: HAMMS.java /////////////////////////////////////////////////////////////

package technicalCalculators;

public class HAMMS {public Vehicle[] vehicles;public double speedLimit;double ETA_minMarginY;double ETA_minMarginG;

public HAMMS(Vehicle[] v, double s){/** - (models of Vehicles[Vehicle], speed[km/h])* - void* - construct a model of a HAMMS intersection*/

vehicles = v;// Convert km/h to m/sspeedLimit = s/3.6;ETA_minMarginY = 1;ETA_minMarginG = 3;

}

public double[] startTest(){/** - (N/A)* - passage times for respective vehicle[s]

41

APPENDIX B. JAVA CODE USED IN SIMULATION

* - Performing simulation in HAMMS in real time.*/

double[] times = new double[2];double startTime = System.currentTimeMillis();boolean hasBothPassed[] = new boolean[]{false, false};

while(true){double time = (System.currentTimeMillis() - startTime) / 1000; //

[s]System.out.println("////////////////////\n//TIME: " + time);setCommands();for(int i = 0; i < vehicles.length; i++){

Vehicle car = vehicles[i];int a = i + 1;System.out.print("Vehicle" + a + ": ");car.update(time);if(car.hasPassed()){

times[i] = car.getPassTime();hasBothPassed[i] = true;

}}if(hasBothPassed[0] && hasBothPassed[1])

break;

try {Thread.sleep(100);

} catch(InterruptedException ex){Thread.currentThread().interrupt();

}

}

return times;}

private void setCommands(){/** - ()* - void* - Updates the setting for respective vehicle*/

double ETAs[] = new double[2];for(int i = 0; i < vehicles.length; i++){

double d = vehicles[i].distance;double s = vehicles[i].speed;

ETAs[i] = d/s;}

42

double ETA_difference = Math.abs(ETAs[0]-ETAs[1]);Vehicle vehicleIsPrio;Vehicle vehicleNotPrio;double ETA_ofNotPrioVehicle;

// determinate which vehicle that will be prioritisedif(ETAs[0] < ETAs[1]){

vehicleIsPrio = vehicles[0];vehicleNotPrio = vehicles[1];ETA_ofNotPrioVehicle = ETAs[1];

}else if(ETAs[0] > ETAs[1]){

vehicleIsPrio = vehicles[1];vehicleNotPrio = vehicles[0];ETA_ofNotPrioVehicle = ETAs[0];

} else{// catch if the ETAs are samevehicleIsPrio = vehicles[1];vehicleNotPrio = vehicles[0];ETA_ofNotPrioVehicle = ETAs[0];

}

// set commands// Prio vehicleif(!vehicleIsPrio.isAutonomous)

vehicleIsPrio.giveCommandRYG("G");else

vehicleIsPrio.giveCommandsSpeed(speedLimit);

// Controlled vehicleif(vehicleIsPrio.hasPassed()){

if(!vehicleNotPrio.isAutonomous){vehicleNotPrio.giveCommandRYG("G");

} elsevehicleNotPrio.giveCommandsSpeed(speedLimit);

}else{if(!vehicleNotPrio.isAutonomous){

if(ETA_difference < ETA_minMarginY){vehicleNotPrio.giveCommandRYG("R");

} else if(ETA_difference < ETA_minMarginG){vehicleNotPrio.giveCommandRYG("Y");

} elsevehicleNotPrio.giveCommandRYG("G");

} else if(vehicleNotPrio.isAutonomous){double ETA_wishedChange = ETA_minMarginG - ETA_difference;if(ETA_wishedChange < 0){

vehicleNotPrio.giveCommandsSpeed(speedLimit);} else{

43


double newRefSpeed = vehicleNotPrio.distance/(ETA_minMarginG+ ETA_ofNotPrioVehicle);

vehicleNotPrio.giveCommandsSpeed(newRefSpeed);}

}}

}}

44

///////////////////////////////////////////////////////////// HAMMS: Human and Autonomous Mixed Management system //// Bachelor thesis in Mechatronics //// Authors: Sebastian Ljungberg & Fredrik Schalling //// TRITA MMK 2017:35 MDAB 653 //// File: TMS.java /////////////////////////////////////////////////////////////


public class TMS {public Vehicle[] vehicles;public double speedLimit;

double ryTime;double doubleredTime;double mingreenTime;double maxgreenTime;double yTime;

Vehicle vehicleIsPrio = null;Vehicle vehicleNotPrio = null;

public TMS(Vehicle[] v, double s){/** - (models of Vehicles[Vehicle], speed[km/h])* - void* - construct a model of a TMS intersection*/

vehicles = v;// Convert from km/h to m/sspeedLimit = s/3.6;

setLightTimes();}

private void setLightTimes(){/** - (N/A)* - void* - Set fixed values for respective time period in TMS*/

if(speedLimit == 50/3.6){ryTime = 1;doubleredTime = 1.5;mingreenTime = 2.1;maxgreenTime = 6.125;yTime = 4;

}

45


else if(speedLimit == 70/3.6){ryTime = 1;doubleredTime = 1.5;mingreenTime = 2.61;maxgreenTime = 6.125;yTime = 5;

}}

public double[] startTest(){/** - (N/A)* - passage times for respective vehicle[s]* - Performing simulation in TMS in real time.*/

double[] times = new double[2];double startTimeTest = System.currentTimeMillis();double[] eightyMeterPassTime = new double[]{-1, -1}; //

[firstPassingCar, secondPassingCar]boolean hasBothPassed[] = new boolean[]{false, false};

while(true){double time = (System.currentTimeMillis() - startTimeTest) / 1000;

// [s]System.out.println("////////////////////\n//TIME: " + time);setCommands(eightyMeterPassTime);for(int i = 0; i < vehicles.length; i++){

Vehicle car = vehicles[i];int a = i + 1;System.out.print("Vehicle" + a + ": ");car.update(time);if(car.distance < 80 && vehicleIsPrio == null){

vehicleIsPrio = car;eightyMeterPassTime[0] = System.currentTimeMillis();int otherVehicle = (i + 1) % 2;vehicleNotPrio = vehicles[otherVehicle];

} else if(!(vehicleIsPrio == null)){if(vehicleNotPrio.distance < 80 && eightyMeterPassTime[1] ==

-1){eightyMeterPassTime[1] = System.currentTimeMillis();}

}if(car.hasPassed()){

times[i] = car.getPassTime();hasBothPassed[i] = true;

}}if(hasBothPassed[0] && hasBothPassed[1])

break;

46

try {Thread.sleep(100);

} catch(InterruptedException ex){Thread.currentThread().interrupt();

}}return times;

}

private void setCommands(double[] eightyMeterPassTime){/** - (when respective vehicle passed the 80m mark[s])* - void* - Updates the light setting for respective lane*/

if(!(eightyMeterPassTime[0] == -1)){String state;if(vehicleIsPrio.initSpeed > vehicleIsPrio.initDistance/(ryTime +

maxgreenTime + yTime)){state = "A";

} else if(vehicleIsPrio.initSpeed <vehicleIsPrio.initDistance/(ryTime + maxgreenTime + yTime)){

state="B";} else{

System.out.print("STATE NOT FOUND");System.exit(0);state = "notastate"; // To compile

}double currentTime0 = (System.currentTimeMillis() -

eightyMeterPassTime[0]) / 1000; // [ms] -> [s]double currentTime1 = 0;if(!(eightyMeterPassTime[1] == -1))

currentTime1 = (System.currentTimeMillis() -eightyMeterPassTime[1]) / 1000; // [ms] -> [s]

switch(state){

// Both approaching vehicles pass through in respective green timezone

case "A":vehicleIsPrio.giveCommandRYG("G");if(currentTime0 < ryTime){ //state reaction

// reaction time}else if(currentTime0 <= (ryTime + maxgreenTime + yTime +

doubleredTime + ryTime) && currentTime1 > ryTime){ // statered

vehicleNotPrio.giveCommandRYG("R");}else if(currentTime0 > (ryTime + maxgreenTime + yTime +

doubleredTime + ryTime)){ //state green

47


vehicleNotPrio.giveCommandRYG("G");} else

vehicleNotPrio.giveCommandRYG("G");

// The slow vehicle doesn’t have sufficient speed to pass in firstgreen time zone

case "B":vehicleIsPrio.giveCommandRYG("G");if(currentTime0 < ryTime){ //state reaction

// reaction time}else if(currentTime0 > (ryTime + maxgreenTime + yTime +

doubleredTime + ryTime + mingreenTime + yTime+doubleredTime+ryTime)){

vehicleIsPrio.giveCommandRYG("G");}else if(currentTime0 > (ryTime + maxgreenTime)){

vehicleIsPrio.giveCommandRYG("R");}

if(currentTime0 < (ryTime + maxgreenTime + yTime +doubleredTime + ryTime) && currentTime1 > ryTime){ // statered

vehicleNotPrio.giveCommandRYG("R");}else if(currentTime0 > (ryTime + maxgreenTime + yTime +

doubleredTime + ryTime)){ //state greenvehicleNotPrio.giveCommandRYG("G");

}}

}}

}

48

///////////////////////////////////////////////////////////// HAMMS: Human and Autonomous Mixed Management system //// Bachelor thesis in Mechatronics //// Authors: Sebastian Ljungberg & Fredrik Schalling //// TRITA MMK 2017:35 MDAB 653 //// File: Vehicle.java /////////////////////////////////////////////////////////////


public class Vehicle {double initSpeed;double initDistance;

double distance;boolean isAutonomous;double speed;double time = 0;String state = "G";double passTime = -1;

//regular vehicle paramsdouble breakingFactor;double oldSpeed;

//PD paramsdouble kp;double kd;boolean firstRun;double lastOutput;double refSpeed;

// collecting datadouble[][] dst; // distance, speed vs time matrixint updateCounter = 0;

public Vehicle(boolean a, double d, double s){/** - (whether the vehicle should represent a autonomous

vehicle[Boolean],* initial distance from the intersection[m],* initial speed[km/h])* - void* - construct a model of a vehicle*/

isAutonomous = a;initDistance = d;

// Convert from km/h to m/s

49


initSpeed = s/3.6;

// initiates all variables at starting state.reset();

}

public void reset(){/** - (N/A)* - void* - construct a model of a TMS intersection*/

speed = initSpeed;distance = initDistance;oldSpeed = speed;passTime = -1;time = 0;

dst = new double[4][10000];updateCounter = 0;

}

public void update(double newTime){/** - (real time[ms])* - void* - updates position and speed according to set parameters*/

double dt = newTime - time;if(!isAutonomous){

// Updates speed if this object represent a human driverswitch(state){case "R":

if(distance > 0){breakingFactor = -Math.pow(speed, 2) / distance;speed = breakingFactor * dt + speed;if(speed < 0)

speed = 0;} else

speed = initSpeed;break;

case "Y":speed = breakingFactor * dt + speed;break;

case "G":if(speed < initSpeed){

//speed += 0.5 * (initSpeed - speed); // Godtycklig

50

accelerations funktiondouble accelerationKoef = 0.31 * 9.81; //g=9.81 0.5*9.81

according to trafikverket 55 p 5s -> koeff = 0.31speed += accelerationKoef * dt; //speed function

} elsespeed = 1 * speed;

}}else{

// Updates speed if this object represent a autonomous vehicledouble diff = doRegulator();speed += diff;

}distance -= dt * (oldSpeed + speed) / 2;oldSpeed = speed;time = newTime;

System.out.print(Math.round(distance) + "m, ");System.out.print(Math.round(speed) + "m/s, ");if(!isAutonomous)

System.out.println(state);else

System.out.println(Math.round(refSpeed) + "m/s");

dst[0][updateCounter] = distance;dst[1][updateCounter] = speed;dst[2][updateCounter] = time;if(!isAutonomous){

if(state == "R")dst[3][updateCounter] = 0;

else if(state == "Y")dst[3][updateCounter] = 1;

else if(state == "G")dst[3][updateCounter] = 2;

elsedst[3][updateCounter] = -1;

}else

dst[3][updateCounter] = refSpeed;updateCounter++;

}

public boolean hasPassed(){/** - (N/A)* - weither the vehicle has passed the intersection* - controls parameters if the vehicle has passed the intersection

51


*/if(distance <= 0){

if(passTime == -1){passTime = time;

}return true;

}else

return false;}

public double getPassTime(){return passTime;

}

public double[][] getDST(){return dst;

}

// functions for human driven vehiclepublic void giveCommandRYG(String com){

/** - (driving state[Green/Yellow/Red])* - void* - updates the state ie how the driver should act*/

state = com;if(com == "R"){

// speed = 0 @ distance = 0

} else if(com == "Y"){// lower speed by a (randomized) fixed factorbreakingFactor = -1; // [m/s^2]

}else if(com == "G"){// do nothing

}else{// raise error, unknown command

}}

// functions for autonomous vehiclepublic void giveCommandsSpeed(double r){

this.refSpeed = r;}

private double doRegulator(){double output;double Poutput;double Doutput;

52

this.kp = 0.31;

double error = refSpeed - speed;/*if(firstRun){

lastOutput = speed;firstRun=false;

}*/

Poutput = kp * error;

Doutput = 0; //- kd * (speed - lastOutput);

output = Poutput + Doutput;

lastOutput = output;return output;

}}

}

53


///////////////////////////////////////////////////////////// HAMMS: Human and Autonomous Mixed Management system //// Bachelor thesis in Mechatronics //// Authors: Sebastian Ljungberg & Fredrik Schalling //// TRITA MMK 2017:35 MDAB 653 //// File: MainFrame.java /////////////////////////////////////////////////////////////

package graphic;

import java.awt.BorderLayout;import java.awt.Dimension;import java.awt.GridLayout;import java.awt.event.ActionEvent;import java.awt.event.ActionListener;

import javax.swing.JButton;import javax.swing.JFrame;import javax.swing.JPanel;

import technicalCalculators.*;

public class MainFrame extends JFrame{VehiclePanel car1Pnl, car2Pnl;MiddlePanel mid;JPanel upperPnl, lowerPnl;JButton startBtn;public MainFrame(){

this.setTitle("Traffic Modulator v1.0");

startBtn = new JButton("START TEST");startBtn.setBounds(0, 0, 20, 30);

initiateListeners();

car1Pnl = new VehiclePanel("Car 1");car2Pnl = new VehiclePanel("Car 2");

mid = new MiddlePanel();

//this.setLayout(new GridLayout(2, 1));this.setDefaultCloseOperation(EXIT_ON_CLOSE);

upperPnl = new JPanel();upperPnl.setLayout(new GridLayout(1, 2));upperPnl.add(startBtn);;

lowerPnl = new JPanel();lowerPnl.setLayout(new GridLayout(1, 3));

54

lowerPnl.add(car1Pnl);lowerPnl.add(mid);lowerPnl.add(car2Pnl);

this.getContentPane().add(upperPnl, BorderLayout.NORTH);this.getContentPane().add(lowerPnl, BorderLayout.SOUTH);

this.setSize(600, 550);this.pack();this.setVisible(true);

}

private void initiateListeners(){startBtn.addActionListener(new ActionListener(){

public void actionPerformed(ActionEvent e) {performTest();

}});

}

public void performTest(){Object[] data1;Object[] data2;data1 = car1Pnl.getParameters();data2 = car2Pnl.getParameters();double speedLimit = mid.getSpeedLimit(); // km/h//System.out.println("Car1 speed: " +

Double.toString((double)data1[0]) + ", distance: " +Double.toString(data1[1]));

//System.out.println("Car2 speed: " +Double.toString((double)data2[0]) + ", distance: " +Double.toString(data2[1]));

//System.out.println("speed limit is set to: " +Double.toString(speedLimit) + "km/h");

Vehicle car1 = new Vehicle((Boolean)data1[2], (double)data1[1],(double)data1[0]); // isAutonomous, distance[m], speed[km/h]

Vehicle car2 = new Vehicle((Boolean)data2[2], (double)data2[1],(double)data2[0]);

HAMMS intrscnMngmnt =new HAMMS(new Vehicle[]{car1, car2}, speedLimit);

double[] passTimesIM = intrscnMngmnt.startTest();

System.out.println("Car 1: " + passTimesIM[0] + "s.");System.out.println("Car 2: " + passTimesIM[1] + "s.");

double[][] car1_dst_IM = car1.getDST();double[][] car2_dst_IM = car2.getDST();

55


car1.reset();car2.reset();

TMS intrscnNrml =new TMS(new Vehicle[]{car1, car2}, speedLimit);

double[] passTimesIN = intrscnNrml.startTest();

double[][] car1_dst_IN = car1.getDST();double[][] car2_dst_IN = car2.getDST();

double diff1 = passTimesIM[0] - passTimesIN[0];double diff2 = passTimesIM[1] - passTimesIN[1];car1Pnl.setResult(new double[]{passTimesIN[0], passTimesIM[0],

diff1});car2Pnl.setResult(new double[]{passTimesIN[1], passTimesIM[1],

diff2});

System.out.println("\nIntersection Management");System.out.println("Car 1: " + passTimesIM[0] + "s.");System.out.println("Car 2: " + passTimesIM[1] + "s.");

System.out.println("Intersection Normal");System.out.println("Car 1: " + passTimesIN[0] + "s.");System.out.println("Car 2: " + passTimesIN[1] + "s.");

}

public static void main (String[] args) {MainFrame mainframe = new MainFrame();

}}

56

///////////////////////////////////////////////////////////// HAMMS: Human and Autonomous Mixed Management system //// Bachelor thesis in Mechatronics //// Authors: Sebastian Ljungberg & Fredrik Schalling //// TRITA MMK 2017:35 MDAB 653 //// File: MiddlePanel.java /////////////////////////////////////////////////////////////

package graphic;

import javax.swing.JComboBox;import javax.swing.JLabel;import javax.swing.JPanel;import javax.swing.SwingConstants;

import java.awt.Color;import java.awt.GridLayout;

public class MiddlePanel extends JPanel {

private JLabel LHOVRA, HAMMS, SLimit, VehicleID, speed,distance, diff1;private final JComboBox<String> speedChoice;

public MiddlePanel(){String[] choices = {"50 km/h", "70 km/h"};

Color myColor = new Color(215, 215, 215);this.setBackground(myColor);this.setLayout(new GridLayout(12,1));

VehicleID = new JLabel("Vehicle ID", SwingConstants.CENTER);SLimit = new JLabel("Choose the speed limit", SwingConstants.CENTER);LHOVRA= new JLabel("In TMS: ", SwingConstants.CENTER);HAMMS= new JLabel("In HAMMS: ", SwingConstants.CENTER);speed= new JLabel("km/h", SwingConstants.CENTER);distance= new JLabel("m", SwingConstants.CENTER);diff1= new JLabel("\u0394t between systems", SwingConstants.CENTER);

speedChoice = new JComboBox<String>(choices);speedChoice.setMaximumSize(speedChoice.getPreferredSize());

this.add(VehicleID);this.add(SLimit);this.add(speedChoice);this.add(new JLabel(""));this.add(speed);this.add(new JLabel(""));this.add(distance);this.add(LHOVRA);

57


this.add(new JLabel(""));this.add(HAMMS);this.add(new JLabel(""));this.add(diff1);this.setVisible(true);//this.setLayout(new BoxLayout(this, BoxLayout.Y_AXIS));

}public double getSpeedLimit(){

String choice = speedChoice.getSelectedItem().toString();if(choice.equals("50 km/h"))

return 50;else

return 70;}

}

58

///////////////////////////////////////////////////////////// HAMMS: Human and Autonomous Mixed Management system //// Bachelor thesis in Mechatronics //// Authors: Sebastian Ljungberg & Fredrik Schalling //// TRITA MMK 2017:35 MDAB 653 //// File: VehiclePanel.java /////////////////////////////////////////////////////////////

package graphic;

import java.awt.Color;import java.awt.Font;import java.awt.GridLayout;import java.awt.TextField;

import javax.swing.JButton;import javax.swing.JComboBox;import javax.swing.JLabel;import javax.swing.JPanel;import javax.swing.SwingConstants;

public class VehiclePanel extends JPanel{String id;private JLabel idLabel, speedLabel, distanceLabel,

time, resultLHOVRA, resultHAMMS, resultDiff;private TextField speedField, distanceField;private JComboBox<String> isAutonomous;

public VehiclePanel(String id){this.id = id;this.idLabel = new JLabel(id, JLabel.CENTER);this.idLabel.setFont(new Font("default", Font.BOLD, 24));this.speedLabel = new JLabel("Input Speed", JLabel.CENTER);this.distanceLabel = new JLabel("Input Distance", JLabel.CENTER);this.time = new JLabel("Passage times", SwingConstants.CENTER);this.resultLHOVRA = new JLabel("--", SwingConstants.CENTER);this.resultHAMMS = new JLabel("--", SwingConstants.CENTER);this.resultDiff = new JLabel("--", SwingConstants.CENTER);

speedField = new TextField();distanceField = new TextField();isAutonomous = new JComboBox<String>(new String[]{"Human driven",

"Autonomous"});

setLayout(new GridLayout(12,1));this.add(idLabel);this.add(new JPanel());this.add(isAutonomous);

59


this.add(speedLabel);this.add(speedField);this.add(distanceLabel);this.add(distanceField);this.add(time);this.add(resultLHOVRA);this.add(new JPanel());this.add(resultHAMMS);this.add(resultDiff);}

public Object[] getParameters(){String speed = speedField.getText();String distance = distanceField.getText();String autonomous = this.isAutonomous.getSelectedItem().toString();if(autonomous.equals("Human driven"))

return new Object[]{Double.parseDouble(speed),Double.parseDouble(distance), false};

elsereturn new Object[]{Double.parseDouble(speed),

Double.parseDouble(distance), false}; // Felaktigt egentligen:gor alla bilar till "human driven"

}

public void setResult(double[] results){// (double)double resLHOVRA = (double)Math.round(results[0] * 10) / 10;double resHAMMS = (double)Math.round(results[1] * 10) / 10;double resDiff = (double)Math.round(results[2] * 10) / 10;this.resultLHOVRA.setText(Double.toString(resLHOVRA) + "s");this.resultHAMMS.setText(Double.toString(resHAMMS) + "s");if(results[2] < 0){

resultDiff.setForeground(new Color(50, 150, 50));} else

resultDiff.setForeground(Color.red);this.resultDiff.setText(Double.toString(resDiff) + "s");

}}

60

Appendix C

C code used in the demonstrator

///////////////////////////////////////////////////////////// HAMMS: Human and Autonomous Mixed Management system //// Bachelor thesis in Mechatronics //// Authors: Sebastian Ljungberg & Fredrik Schalling //// TRITA MMK 2017:35 MDAB 653 //// File: Vehicle.ino /////////////////////////////////////////////////////////////

#include <Wire.h>// set pin numbers:const int LED = 13;const int Enable_A = 3; // A low-to-high transition on the STEP input

sequences the translator and advances the motor one increment.const int IN1 = 4; // Direction of rotationconst int IN2 = 5; // Mode of operation: Active/Sleepconst int Enable_B = 6; // Enable/Disable the Driver operationconst int IN3 = 7; // Reset when active turns off all of the FET

outputsconst int IN4 = 8; // Microstep Selectconst int Threshold = 20;double V0=10;double X= V0;double Y=V0;double Vdiff = 0;double SampleValue=V0*2;double Yref=0.5;double Xref=0.5;int counter=0;

#define IN1_L digitalWrite(IN1, LOW);#define IN1_H digitalWrite(IN1, HIGH); //left-back - reverse#define IN2_L digitalWrite(IN2, LOW);#define IN2_H digitalWrite(IN2, HIGH); //left-back - run

61

APPENDIX C. C CODE USED IN THE DEMONSTRATOR

#define IN3_L digitalWrite(IN3, LOW);#define IN3_H digitalWrite(IN3, HIGH); //right-back - reverse#define IN4_L digitalWrite(IN4, LOW);#define IN4_H digitalWrite(IN4, HIGH); //right-back - run

#define driveForward {IN1_L; IN2_H; IN3_L; IN4_H;} //Kor frammat#define driveReverse {IN1_H; IN2_L; IN3_H; IN4_L;} //Backa#define driveStop {IN1_L; IN2_L; IN3_L; IN4_L;} // stoppa

// Variables will change:long previousMillis = 0; // will store last time STEP was updatedvolatile int index = 1;long interval = 1000; // interval at which to make a STEP

void setup() {// set the digital pin as output:pinMode(LED, OUTPUT);pinMode(Enable_A, OUTPUT);pinMode(Enable_B, OUTPUT);pinMode(IN1, OUTPUT);pinMode(IN2, OUTPUT);pinMode(IN3, OUTPUT);pinMode(IN4, OUTPUT);

//Set the statedigitalWrite(Enable_A, HIGH);digitalWrite(Enable_B, HIGH);digitalWrite(IN1, LOW);digitalWrite(IN2, LOW);digitalWrite(IN3, LOW);digitalWrite(IN4, LOW);

Wire.begin(9);Wire.onReceive(receiveEvent);

Serial.begin(9600);

}

void receiveEvent(int bytes){

Vdiff = ((double)Wire.read())/100-1; // Procentuell forandring ihastighet (skickar mellan -1 och 1, 0 ar ingen skillnad

Serial.print("Vdiff= ");Serial.println(Vdiff); //Vdiff 100if(Vdiff>=0){

62

Yref = Yref - abs(Yref * (Vdiff));Xref = 1 - Yref;

}else if(Vdiff<0){Xref = Xref - abs(Xref * Vdiff);Yref = 1 - Xref;

}

}

void updateXY(){X = Xref;Y = Yref;

}

void loop() {counter++;if(counter==5){

updateXY();counter=0;

}Serial.print("Xref: ");Serial.print(X);Serial.print(", Yref: ");Serial.println(Y);driveForward;delay(X * SampleValue);driveStop;delay(Y * SampleValue);

}

63


///////////////////////////////////////////////////////////// HAMMS: Human and Autonomous Mixed Management system //// Bachelor thesis in Mechatronics //// Authors: Sebastian Ljungberg & Fredrik Schalling //// TRITA MMK 2017:35 MDAB 653 //// File: calcFunctions.ino /////////////////////////////////////////////////////////////

double calcDistance(double distances[], int lengthof_distances){double calculatedDistance;double maxDist = distances[0];double minDist = distances[0];double totDist = 0;for(int i = 0; i < lengthof_distances - 1; i++){

if(distances[i] < minDist){minDist = distances[i];

}if(distances[i] > maxDist){

maxDist = distances[i];}totDist += distances[i];

}totDist -= (minDist + maxDist);calculatedDistance = totDist / (lengthof_distances - 2); // -2 since min

and max val is subtractedreturn calculatedDistance;

}

double calcSpeed(double distances[], int lengthof_distances, doubletimeDiff){

double calculatedSpeed;double lastMesurements = (distances[lengthof_distances-1] +

distances[lengthof_distances-2])/2;double firstMesurements = (distances[0] + distances[1])/2;double totDist = firstMesurements - lastMesurements;/*for(int i = 0; i <= lengthof_distances - 2; i++) {

totDist += distances[i + 1] - distances[i];}*/calculatedSpeed = totDist * 1000 / timeDiff; // *1000 ms/sif(calculatedSpeed < 0)

return 0;else

return calculatedSpeed;}

double calcETA(double d, double s){if(s == 0){

64

return 99999;}else {

return d/s;}

}

65


///////////////////////////////////////////////////////////// HAMMS: Human and Autonomous Mixed Management system //// Bachelor thesis in Mechatronics //// Authors: Sebastian Ljungberg & Fredrik Schalling //// TRITA MMK 2017:35 MDAB 653 //// File: two_US_states_v7.ino ////////////////////////////////////////////////////////////

// Version 7// New in v7: HAMMS implemented correctly and splited to tabs

#include <stdio.h> // printf() et.#include <stdlib.h> // abs() et.#include <NewPing.h> // Used for ultrasonic sensors,

http://playground.arduino.cc/Code/NewPing#include <Wire.h> // Used for wired connection

#define ultraSoundSignal_echo_1 8 // Ultrasound signal pin#define ultraSoundSignal_trig_1 9 // Ultrasound signal pin#define ultraSoundSignal_echo_2 6 // Ultrasound signal pin#define ultraSoundSignal_trig_2 7 // Ultrasound signal pin#define MAX_DISTANCE_TO_US 500#define MIN_SPEED 6 // cm/s

#define SPEEDLIMIT 30 // 30 [cm/s] "=" 50 [km/h];#define ETA_minMarginY 1 // [s]#define ETA_minMarginG 3 // [s]

#define LED_GREEN_1 2#define LED_YELLOW_1 10#define LED_RED_1 4#define LED_GREEN_2 3#define LED_YELLOW_2 11#define LED_RED_2 5

#define LIGHT_LED_GREEN_1 digitalWrite(LED_GREEN_1, HIGH);#define TURNOFF_LED_GREEN_1 digitalWrite(LED_GREEN_1, LOW);#define LIGHT_LED_YELLOW_1 digitalWrite(LED_YELLOW_1, HIGH);#define TURNOFF_LED_YELLOW_1 digitalWrite(LED_YELLOW_1, LOW);#define LIGHT_LED_RED_1 digitalWrite(LED_RED_1, HIGH);#define TURNOFF_LED_RED_1 digitalWrite(LED_RED_1, LOW);

#define LIGHT_LED_GREEN_2 digitalWrite(LED_GREEN_2, HIGH);#define TURNOFF_LED_GREEN_2 digitalWrite(LED_GREEN_2, LOW);#define LIGHT_LED_YELLOW_2 digitalWrite(LED_YELLOW_2, HIGH);#define TURNOFF_LED_YELLOW_2 digitalWrite(LED_YELLOW_2, LOW);#define LIGHT_LED_RED_2 digitalWrite(LED_RED_2, HIGH);#define TURNOFF_LED_RED_2 digitalWrite(LED_RED_2, LOW);

66

double speed_1;double speed_2;double distance_1;double distance_2;double ETA_1;double ETA_2;

boolean isVehicleApproaching = false;boolean isVehicle_1 = false;boolean isVehicle_2 = false;String state_1;String state_2;

NewPing sonar_1(ultraSoundSignal_trig_1, ultraSoundSignal_echo_1,MAX_DISTANCE_TO_US);

NewPing sonar_2(ultraSoundSignal_trig_2, ultraSoundSignal_echo_2,MAX_DISTANCE_TO_US);

void setup() {Serial.begin(115200);pinMode(LED_GREEN_1, OUTPUT);pinMode(LED_YELLOW_1, OUTPUT);pinMode(LED_RED_1, OUTPUT);pinMode(LED_GREEN_2, OUTPUT);pinMode(LED_YELLOW_2, OUTPUT);pinMode(LED_RED_2, OUTPUT);

//init wired connectionWire.begin();

}

void loop(){updateData(5, 150); // (#measurements, time over which the measurements

is done)isVehicleApproaching = searchVehicle();if (isVehicleApproaching){

ETA_1 = calcETA(distance_1,speed_1);ETA_2 = calcETA(distance_2,speed_2);

setCommands();}// Controll printsString outputText = "";outputText = "@" + String(round(millis()/1000)) + "s: ";if (speed_1 > 3 || speed_2 > 3) {

outputText += "US_1: " + String(speed_1) + " \t US_2: " +

67


String(speed_2)+ " [cm/s]\t";//outputText += "1: " + state_1 + ", 2: " + state_2;outputText += "ETA_1: " + String(ETA_1) + ", ETA_2: " + String(ETA_2);//outputText += "distance_1 " + String(distance_1) + ", distance_2: "

+ String(distance_2);}

Serial.println(outputText);}

68

///////////////////////////////////////////////////////////// HAMMS: Human and Autonomous Mixed Management system //// Bachelor thesis in Mechatronics //// Authors: Sebastian Ljungberg & Fredrik Schalling //// TRITA MMK 2017:35 MDAB 653 //// File: updateFunctions.ino /////////////////////////////////////////////////////////////

void updateData(int numof_mesurements, int sampleTime){unsigned long time_start; // updated before mesurementsunsigned long time_end; // updated at the end of measurments

double distances_1[numof_mesurements];double distances_2[numof_mesurements];

double tot_distance_1 = 0;double tot_distance_2 = 0;

time_start = millis();for(int i = 0; i < numof_mesurements; i++) {

distances_1[i] = sonar_1.ping_cm();distances_2[i] = sonar_2.ping_cm();

delay(floor(sampleTime/numof_mesurements));}time_end = millis();

double timeDiff = (double)(time_end - time_start);speed_1 = calcSpeed(distances_1, numof_mesurements, timeDiff);speed_2 = calcSpeed(distances_2, numof_mesurements, timeDiff);

distance_1 = calcDistance(distances_1, numof_mesurements);distance_2 = calcDistance(distances_2, numof_mesurements);

}

boolean searchVehicle(){if (speed_1 > MIN_SPEED){

isVehicle_1 = true;}else

isVehicle_1 = false;

if (speed_2 > MIN_SPEED){isVehicle_2 = true;

}else

isVehicle_2 = false;

69


if (isVehicle_1 || isVehicle_2){return true;

}else{

return false;}

}

void changeStates(int entrance_1_newstate, int entrance_2_newstate){// IN: 0 - RED, 1 - YELLOW, 2 - GREEN

TURNOFF_LED_GREEN_1;TURNOFF_LED_YELLOW_1;TURNOFF_LED_RED_1;switch(entrance_1_newstate){

case 0:// REDLIGHT_LED_RED_1;state_1 = "RED";break;

case 1:// YELLOWLIGHT_LED_YELLOW_1;state_1 = "YELLOW";break;

case 2:// GREENLIGHT_LED_GREEN_1;state_1 = "GREEN";break;

}

TURNOFF_LED_GREEN_2;TURNOFF_LED_YELLOW_2;TURNOFF_LED_RED_2;switch(entrance_2_newstate){

case 0:// REDLIGHT_LED_RED_2;state_2 = "RED";break;

case 1:// YELLOWLIGHT_LED_YELLOW_2;state_2 = "YELLOW";break;

70

case 2:// GREENLIGHT_LED_GREEN_2;state_2 = "GREEN";break;

}}

void sendDriveSignal(int adress, double speedNow, double speedWish){double changeFactor = speedWish/speedNow;int sendData;if(changeFactor < 1){

sendData = changeFactor * 100;}else {

if(changeFactor > 2){sendData = 200;

} else{sendData = changeFactor * 100;

}}//Serial.println("Change factor: " + String(changeFactor) + ",

speedWish: " + String(speedWish) + ", speedNow" + String(speedNow));

if(distance_2 < 20){sendData = 0;

}

Wire.beginTransmission(adress);Wire.write(sendData);Wire.endTransmission();

}

void setCommands(){int s1;int s2;double ETA_difference = abs(ETA_2 - ETA_1);

if((ETA_1 < ETA_2) || (ETA_1 == ETA_2)){// reglera autonoma, ge ettan prios1 = 2;double ETA_wishedChange = ETA_minMarginG - ETA_difference;double newRefSpeed;if(ETA_wishedChange < 0){

newRefSpeed = SPEEDLIMIT;} else {

newRefSpeed = distance_2/(ETA_minMarginG + ETA_1);}

71


sendDriveSignal(9, speed_2, newRefSpeed);

if(ETA_difference < ETA_minMarginY){s2 = 0;

} else if(ETA_difference < ETA_minMarginG){s2 = 1;

} else {s2 = 2;

}changeStates(s1, s2);

} else if(ETA_1 > ETA_2){s2 = 2;double newRefSpeed = SPEEDLIMIT;sendDriveSignal(9, speed_2, newRefSpeed);

if(ETA_difference < ETA_minMarginY){s1 = 0;

} else if(ETA_difference < ETA_minMarginG){s1 = 1;

} else {s1 = 2;

}changeStates(s1, s2);

} else{Serial.println("ERROR");

}}

72

TRITA MMK 2017:35 MDAB 653

www.kth.se

HAMMS - Diva1201180/FULLTEXT01.pdf · autonomous vehicles in four-way intersections SEBASTIAN...

Documents

Transcript of HAMMS - Diva1201180/FULLTEXT01.pdf · autonomous vehicles in four-way intersections SEBASTIAN...