DriverController.emadl 11.9 KB
Newer Older
1 2
package dp.subcomponents;

Svetlana's avatar
Svetlana committed
3
component DriverController {
4
    ports
5
        in Q^{5} steeringRecordIn,
6 7
	    in Affordance affordanceIn,
	    in Q(0 m/s:0.1 m/s:100 m/s) speedIn,
8 9
        in Z timerLeftIn,
        in Z timerRightIn,
10
        in Z laneChangeIn,
11
        in Z lanesCountIn,
12 13
	out Q(-1:1)^{3} commandsOut, // [accelCmd, steerCmd, brakeCmd]
	out Q steerCmd,
14 15
        out Z timerLeftOut,
        out Z timerRightOut,
16
        out Z laneChangeOut;
17 18

	implementation Math {
Svetlana's avatar
Svetlana committed
19
        Q roadWidth = 8;
20 21 22 23
        Q centerLine;
        Q coeSteer = 1.0;
        Q preML;
        Q preMR;
24
	Q slowDown = 100; // Acceleration
25 26
        Q preDistL = 60;
        Q preDistR = 60;
Svetlana's avatar
Svetlana committed
27 28 29 30 31 32 33 34 35 36 37 38 39 40
        B leftClear = false;
        B rightClear = false;
        Z timerSet = 60;
        Z leftTimer = timerLeftIn;
        Z rightTimer = timerRightIn;
        Q steerTrend = steeringRecordIn(1) + steeringRecordIn(2) + steeringRecordIn(3) + steeringRecordIn(4) + steeringRecordIn(5); // am I turning or not;
        
        //parameters of car following modlel
        Q vMax = 20;
        Q vC = 2.772;
        Q vD = -0.693;
        Q carFollowingSlowDown = vMax * (1-exp(-(vC/vMax) * affordanceIn.distMM - vD)); // optimal velocity for car-following model
        
        // CASE 1: one lane
41
        if (lanesCountIn == 1)
Svetlana's avatar
Svetlana committed
42 43
            if (affordanceIn.distMM < 20) // car in front of me, slow down
                
44
                // optimal velocity car-following model
Svetlana's avatar
Svetlana committed
45
                slowDown = carFollowingSlowDown;
46
                if (slowDown < 0)
47 48 49
                    slowDown = 0;
                end
            end
50

51 52
            if (-affordanceIn.toMarkingML + affordanceIn.toMarkingMR < 5.5)
                coeSteer = 1.5;
53
                centerLine = (affordanceIn.toMarkingML + affordanceIn.toMarkingMR) / 2;
54 55 56
                preML = affordanceIn.toMarkingML;
                preMR = affordanceIn.toMarkingMR;
                if (affordanceIn.toMarkingM < 1)
57
                      coeSteer = 0.4;
58 59 60
                end
            else
                if (-preML > preMR)
61
                   centerLine = (affordanceIn.toMarkingL + affordanceIn.toMarkingM) / 2;
62
                else
63
                   centerLine=(affordanceIn.toMarkingR + affordanceIn.toMarkingM) / 2;
64 65
                end
                coeSteer = 0.3;
Svetlana's avatar
Svetlana committed
66
            end
67 68 69 70 71 72 73 74 75

            // steering control, "shared->steerCmd" [-1,1] is the value sent back to TORCS
	        steerCmd = (affordanceIn.angle - centerLine/roadWidth) / 0.541052 / coeSteer;

	        if (coeSteer > 1 && steerCmd > 0.1)   // reshape the steering control curve
                steerCmd = steerCmd * (2.5 * steerCmd + 0.75);
            end
            commandsOut(2) = steerCmd;

Svetlana's avatar
Svetlana committed
76 77 78 79 80
        // CASE 2: two or three lanes
        elseif (lanesCountIn == 2 || lanesCountIn == 3)
            if (preDistL < 20 && affordanceIn.distLL < 20) // left lane is occupied or not
                leftClear = false;
                leftTimer = 0;
81
            else
Svetlana's avatar
Svetlana committed
82
                leftTimer += 1;
83
            end
84

Svetlana's avatar
Svetlana committed
85 86 87
            if (preDistR < 20 && affordanceIn.distRR < 20) // right lane is occupied or not
                rightClear = false;
                rightTimer = 0;
88
            else
Svetlana's avatar
Svetlana committed
89
                rightTimer += 1;
90
            end
91 92 93 94

            preDistL = affordanceIn.distLL;
            preDistR = affordanceIn.distRR;

Svetlana's avatar
Svetlana committed
95 96 97
            if (leftTimer > timerSet) // left lane is clear
                leftTimer = timerSet;
                leftClear = true;
98 99
            end

Svetlana's avatar
Svetlana committed
100 101 102
            if (rightTimer > timerSet) // right lane is clear
                rightTimer = timerSet;
                rightClear = true;
103
            end
Svetlana's avatar
Svetlana committed
104 105
            
            if (lanesCountIn == 2)
106
                if (laneChangeIn == 0 && affordanceIn.distMM < 15) // if current lane is occupied
Svetlana's avatar
Svetlana committed
107
                    if (affordanceIn.toMarkingLL > -8 && leftClear == true && steerTrend >= 0) // move to left lane
108
                        laneChangeIn = -2;
Svetlana's avatar
Svetlana committed
109 110 111 112 113 114 115
                        coeSteer = 6;
                        rightClear = false;
                        rightTimer = 0;
                        leftClear = false;
                        leftTimer = 0;
                        timerSet = 30;
                    elseif (affordanceIn.toMarkingRR < 8 && rightClear == true && steerTrend <= 0) // move to right lane
116
                        laneChangeIn = 2;
Svetlana's avatar
Svetlana committed
117 118 119 120 121 122 123 124 125 126 127
                        coeSteer = 6;
                        rightClear = false;
                        rightTimer = 0;
                        leftClear = false;
                        leftTimer = 0;
                        timerSet = 30;
                    else // follow car in front
                        slowDown = carFollowingSlowDown; 
                        if slowDown < 0
                            slowDown = 0;
                        end
128
                    end
129
                elseif (laneChangeIn == 0 && affordanceIn.distMM >= 15) // prefer to stay in the right lane
Svetlana's avatar
Svetlana committed
130
                    if (affordanceIn.toMarkingLL < -8 && rightClear == true && steerTrend <= 0 && steerTrend > -0.2) // in right lane, so move to central lane
131
                        laneChangeIn = 2;
Svetlana's avatar
Svetlana committed
132 133 134
                        coeSteer = 6;
                        rightClear = false;
                        rightTimer = 20;
135 136
                    end
                end
Svetlana's avatar
Svetlana committed
137
            elseif (lanesCountIn == 3)
138
                 if (laneChangeIn == 0 && affordanceIn.distMM < 15) // current lane is occupied
Svetlana's avatar
Svetlana committed
139
                     if (affordanceIn.toMarkingLL > -8 && leftClear == true && steerTrend >= 0 && steerTrend < 0.2) // move to left lane
140
                        laneChangeIn = -2;
Svetlana's avatar
Svetlana committed
141 142 143 144 145 146 147
                        coeSteer = 6;
                        rightClear = false;
                        rightTimer = 0;
                        leftClear = false;
                        leftTimer = 30;
                        timerSet = 60;
                    elseif (affordanceIn.toMarkingRR < 8 && rightClear == true && steerTrend <= 0 && steerTrend > -0.2) // move to right lane
148
                        laneChangeIn = 2;
Svetlana's avatar
Svetlana committed
149 150 151 152 153 154 155 156 157 158 159
                        coeSteer = 6;
                        rightClear = false;
                        rightTimer = 30;
                        leftClear = false;
                        leftTimer = 0;
                        timerSet = 60;
                    else // car following
                        slowDown = carFollowingSlowDown; 
                        if slowDown < 0
                            slowDown = 0;
                        end
160
                    end
161
                elseif (laneChangeIn == 0 && affordanceIn.distMM >= 15) // prefer to stay in the central line
Svetlana's avatar
Svetlana committed
162
                    if (affordanceIn.toMarkingRR > 8 && leftClear == true && steerTrend >= 0 && steerTrend < 0.2) // in right lane, so move to central lane
163
                        laneChangeIn = -2;
Svetlana's avatar
Svetlana committed
164 165 166 167
                        coeSteer = 6;
                        leftClear = false;
                        leftTimer = 30;
                    elseif (affordanceIn.toMarkingLL < -8 && rightClear == true && steerTrend <= 0 && steerTrend > -0.2) // in left lane, so move to central lane
168
                        laneChangeIn = 2;
Svetlana's avatar
Svetlana committed
169 170 171
                        coeSteer = 6;
                        rightClear = false;
                        rightTimer = 30;
172 173
                    end
                end
Svetlana's avatar
Svetlana committed
174
            end
175
                
Svetlana's avatar
Svetlana committed
176
            // go through all possible lane change maneuvers
177
            if (laneChangeIn == 0) //car following
178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193
                if (-affordanceIn.toMarkingML + affordanceIn.toMarkingMR < 5.5)
                    coeSteer = 1.5;
                    centerLine = (affordanceIn.toMarkingML+affordanceIn.toMarkingMR)/2;
                    preML = affordanceIn.toMarkingML;
                    preMR = affordanceIn.toMarkingMR;
                    if (affordanceIn.toMarkingM < 1)
                        coeSteer = 0.4;
                    end
                else
                    if (-preML > preMR)
                        centerLine = (affordanceIn.toMarkingL+affordanceIn.toMarkingM)/2;
                    else
                        centerLine = (affordanceIn.toMarkingR+affordanceIn.toMarkingM)/2;
                    end
                    coeSteer = 0.3;
                end
194
            elseif (laneChangeIn == -2)
195 196 197 198 199 200 201 202
                if (-affordanceIn.toMarkingML+affordanceIn.toMarkingMR < 5.5)
                    centerLine = (affordanceIn.toMarkingLL+affordanceIn.toMarkingML)/2;
                    if (affordanceIn.toMarkingL > -5 && affordanceIn.toMarkingM < 1.5)
                        centerLine = (centerLine+(affordanceIn.toMarkingL+affordanceIn.toMarkingM)/2)/2;
                    end
                else
                    centerLine = (affordanceIn.toMarkingL+affordanceIn.toMarkingM)/2;
                    coeSteer = 20;
203
                    laneChangeIn = -1;
204
                end
205
            elseif (laneChangeIn == -1)
206 207 208 209 210 211 212
                if (affordanceIn.toMarkingL > -5 && affordanceIn.toMarkingM < 1.5)
                    centerLine = (affordanceIn.toMarkingL+affordanceIn.toMarkingM)/2;
                    if (-affordanceIn.toMarkingML+affordanceIn.toMarkingMR < 5.5)
                        centerLine = (centerLine+(affordanceIn.toMarkingML+affordanceIn.toMarkingMR)/2)/2;
                    end
                else
                    centerLine = (affordanceIn.toMarkingML+affordanceIn.toMarkingMR)/2;
213
                    laneChangeIn = 0;
214
                end
215
            elseif (laneChangeIn == 2)
216 217 218 219 220 221 222 223
                if (-affordanceIn.toMarkingML+affordanceIn.toMarkingMR < 5.5)
                    centerLine = (affordanceIn.toMarkingRR+affordanceIn.toMarkingMR)/2;
                    if (affordanceIn.toMarkingR < 5 && affordanceIn.toMarkingM < 1.5)
                        centerLine = (centerLine + (affordanceIn.toMarkingR+affordanceIn.toMarkingM)/2)/2;
                    end
                else
                    centerLine = (affordanceIn.toMarkingR+affordanceIn.toMarkingM)/2;
                    coeSteer = 20;
224
                    laneChangeIn = 1;
225
                end
226
            elseif (laneChangeIn == 1)
227 228 229 230 231 232
                if (affordanceIn.toMarkingR < 5 && affordanceIn.toMarkingM < 1.5)
                    centerLine = (affordanceIn.toMarkingR+affordanceIn.toMarkingM)/2;
                    if (-affordanceIn.toMarkingML+affordanceIn.toMarkingMR < 5.5)
                        centerLine = (centerLine+(affordanceIn.toMarkingML+affordanceIn.toMarkingMR)/2)/2;
                    else
                        centerLine = (affordanceIn.toMarkingML+affordanceIn.toMarkingMR)/2;
233
                        laneChangeIn = 0;
234 235 236 237 238 239
                    end
                end
            end

            steerCmd = (affordanceIn.angle - centerLine/roadWidth) / 0.541052 / coeSteer;

240
            if (laneChangeIn == 0 && coeSteer > 1 && steerCmd > 0.1) // reshape the steering control curve
241 242 243 244
                steerCmd = steerCmd * (2.5*steerCmd+0.75);
            end
            commandsOut(2) = steerCmd;

Svetlana's avatar
Svetlana committed
245 246 247 248 249 250 251 252 253 254
        // DEFAULT CASE - no information on lanes number
        else
            commandsOut(2) = 0;
        end
        
        // compute desired speed
        Q desiredSpeed = 20; // 20m/s = 72 kmh
        if (affordanceIn.fast != 1)
            desiredSpeed = 20 - abs(steerTrend) * 4.5;
        end
255

Svetlana's avatar
Svetlana committed
256 257 258
        if (desiredSpeed < 10)
            desiredSpeed = 10;
        end
259

Svetlana's avatar
Svetlana committed
260 261 262
        if (desiredSpeed > slowDown)
            desiredSpeed = slowDown;
        end
263

Svetlana's avatar
Svetlana committed
264 265 266 267 268
        // compute acceleration and brake
        if (desiredSpeed >= speedIn)
            Q accelCmd = 0.2*(desiredSpeed - speedIn + 1);
            if (accelCmd > 1)
                accelCmd = 1;
269
            end
Svetlana's avatar
Svetlana committed
270 271 272

            commandsOut(1) = accelCmd;
            commandsOut(3) = 0; // brakeCmd
273
        else
Svetlana's avatar
Svetlana committed
274 275 276 277 278 279 280
            Q brakeCmd = 0.1 * (speedIn - desiredSpeed);
            if (brakeCmd > 1)
                brakeCmd = 1;
            end

            commandsOut(3) = brakeCmd;
            commandsOut(1) = 0; // accelCmd
281
        end
282

Svetlana's avatar
Svetlana committed
283 284 285
        timerLeftOut = leftTimer;
        timerRightOut = rightTimer;
        laneChangeOut = laneChangeIn;
286
    }
287
}