hstc-sim/Caravan.m at master · MohitsSingh/hstc-sim · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
classdef Caravan < hgsetget % subclass hgsetget
    %CARAVAN Summary of this class goes here
    %   Detailed explanation goes here

    properties
        id = 0;
        maxSpeed                = 90.0;
        minVehicleSpacing       = 3.0 / 5280.0; % in miles
        destination             = 0.0; %to where is it going?
        position                = 0.0; %where is it now?
        velocity                = 0.0;
        maxSize                 = 21;
        isAbleToTakeNewCars     = true;
        gapLocation             = 0;
        allVehicles             = Vehicle.empty;
    end

    methods

        function obj = Update(obj)
%             myObj = obj
%             vehLen = length(obj.allVehicles)
            if ~isempty(obj.allVehicles)
                leadVeh = obj.allVehicles(1);
                obj.position = leadVeh.posY;
                obj.velocity = leadVeh.velocity;
            end
        end

        function sizeOfGap = GapSize(obj)
            sizeOfGap = obj.allVehicles(obj.gapLocation).posY - ...
                obj.allVehicles(obj.gapLocation+1).posY ;
        end

        function [gapFront,gapBack] = GetGap(obj)
            gapFront = obj.allVehicles(obj.gapLocation).posY - ...
                obj.allVehicles(obj.gapLocation).length - ...
                obj.minVehicleSpacing;

            gapBack = obj.allVehicles(obj.gapLocation+1).posY + ...
                obj.minVehicleSpacing;
        end

        function midPoint = GetMidPoint(obj)
            cvFront = obj.allVehicles(1).posY;

            cvBack = obj.allVehicles(length(obj.allVehicles)).posY + ...
                obj.allVehicles(length(obj.allVehicles)).length;
            midPoint = (cvFront + cvBack)/2.0;
        end
        function endPoint = GetEndPoint(obj)
            endPoint = obj.allVehicles(length(obj.allVehicles)).posY + ...
                obj.allVehicles(length(obj.allVehicles)).length;
        end

        function insertionPoint = GetInsertionPoint(obj)
            if obj.gapLocation == 0
                insertionPoint = obj.allVehicles(1).posY;
            else
                gapFront = obj.allVehicles(obj.gapLocation).posY - ...
                    obj.allVehicles(obj.gapLocation).length - ...
                    obj.minVehicleSpacing;

                gapBack = obj.allVehicles(obj.gapLocation+1).posY + ...
                    obj.minVehicleSpacing;
                insertionPoint = (gapFront + gapBack)/2.0;
            end
        end

        %location = 1 HEAD
        %location = -1 TAIL
        %location = 2..N this vehcile is the new one at that location
        function obj = InsertRequest(obj,location)
            obj.gapLocation = location;
            obj.allVehicles(location+1).gapMode = true;
            % send a message to all cars behind insertion point to
            % slow down 2 mph.   All cars in front of point  to speed up 2mph
            for i = 1 : obj.gapLocation
                obj.allVehicles(i).targetVelocity   = obj.allVehicles(i).targetVelocity + 2.0;
                obj.allVehicles(i).targetRate       = 0.2;
            end
        end

        function obj = ExtractRequest(obj,vid)
            % |V|V|V|V|V|E|V|V|V|V|
            extractLocation = -1;
            for i=1:length(obj.allVehicles)
                if obj.allVehicles(i).id == vid
                    extractLocation = i-1;
                    obj.gapLocation = extractLocation;
                    break;
                end
            end
            if extractLocation ~= -1
                %TODO what to do if vehicle is head or tail
                obj.allVehicles(extractLocation+1).gapMode = true;
                obj.allVehicles(extractLocation+2).gapMode = true;
                % send a message to all cars behind insertion point to
                % slow down 2 mph.   All cars in front of point  to speed up 2mph
                for i = 1 : extractLocation
%                     obj.allVehicles(i).targetVelocity   = obj.allVehicles(i).targetVelocity + 2.0;
%                     obj.allVehicles(i).targetRate       = 0.2;
                end
            end
        end


        function obj = ResumeSpeed(obj)
            % send a message to all cars behind insertion point to
            % slow down 2 mph.   All cars in front of point  to speed up 2mph
            for i = 1 : obj.gapLocation
                obj.allVehicles(i).targetVelocity = obj.maxSpeed;
            end
        end


        function gapMode = isGapClosingMode(obj)
            gapMode = obj.allVehicles(obj.gapLocation).closingRanks ;
        end

        function  targetVelocity = ShareGapTargetVelocity(obj)
            targetVelocity = obj.allVehicles(obj.gapLocation).targetVelocity;
            for i = obj.gapLocation:length(obj.allVehicles)
                obj.allVehicles(i).targetVelocity = targetVelocity;
            end
        end


        function obj = CloseRanks(obj)
            % send a message to all cars to return to follow mode
            % all cars will follow the velocity commands of the
            %'gap' mode car
            %remove car from caravan vehicle list TODO
            obj.gapLocation =  find([obj.allVehicles.gapMode],1,'first');
            obj.allVehicles(obj.gapLocation).gapMode        = false;
            obj.allVehicles(obj.gapLocation).closingRanks   = true;
        end


        %assumes that new veichle is added to end of list.   For now
        %this is used to initalize a caravan before the simulation starts
        %running
        function obj = AddToVehicleList(obj,v)
            %find nearest car in front of me, in my lane
            %posX = myposX, posY > my Posy
            numCars = length(obj.allVehicles);
            numCars = numCars + 1 ;
            obj.allVehicles = [obj.allVehicles v];


            %update position and destination
            obj.position = obj.allVehicles(1).posY; %lead car
            if v.destination > obj.destination      %car going the farthest
                obj.destination  = v.destination ;
            end

        end
    end
end