Aufgrund einer Wartung wird GitLab am 17.08. zwischen 8:30 und 9:00 Uhr kurzzeitig nicht zur Verfügung stehen. / Due to maintenance, GitLab will be temporarily unavailable on 17.08. between 8:30 and 9:00 am.

Drivercontroller.emadl 2.68 KB
Newer Older
1 2 3 4
package dp.subcomponents;

component Drivercontroller {
    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
	    out Q(-1:1)^{3} commandsOut, // [accelCmd, steerCmd, brakeCmd]
	    out Q steerCmd;
10 11

	implementation Math {
Svetlana's avatar
Svetlana committed
12
        Q roadWidth = 8;
13 14 15 16 17 18
        Q centerLine;
        Q coeSteer = 1.0;
        Q preML;
        Q preMR;
        Q desiredSpeed;
	    Q slowDown = 100; // Acceleration
19

Svetlana's avatar
Svetlana committed
20 21 22
	    if (affordanceIn.distMM < 15)
            Q vMax = 20;
            Q vC = 2.772;
23
            Q vD = -0.693;
24
            // optimal velocity car-following model
25
            slowDown = vMax * (1-exp(-(vC/vMax) * affordanceIn.distMM - vD));
26

Svetlana's avatar
Svetlana committed
27 28
            if (slowDown < 0)
                slowDown = 0;
Svetlana's avatar
Svetlana committed
29
            end
30 31
        end

Svetlana's avatar
Svetlana committed
32
        if (-affordanceIn.toMarkingML + affordanceIn.toMarkingMR < 5.5)
Svetlana's avatar
Svetlana committed
33 34 35 36
            coeSteer = 1.5;
            centerLine = (affordanceIn.toMarkingML + affordanceIn.toMarkingMR) / 2;
            preML = affordanceIn.toMarkingML;
            preMR = affordanceIn.toMarkingMR;
Svetlana's avatar
Svetlana committed
37
            if (affordanceIn.toMarkingM < 1)
Svetlana's avatar
Svetlana committed
38
                  coeSteer = 0.4;
Svetlana's avatar
Svetlana committed
39 40
            end
        else
Svetlana's avatar
Svetlana committed
41 42
            if (-preML > preMR)
               centerLine = (affordanceIn.toMarkingL + affordanceIn.toMarkingM) / 2;
Svetlana's avatar
Svetlana committed
43
            else
Svetlana's avatar
Svetlana committed
44
               centerLine=(affordanceIn.toMarkingR + affordanceIn.toMarkingM) / 2;
Svetlana's avatar
Svetlana committed
45
            end
Svetlana's avatar
Svetlana committed
46
            coeSteer = 0.3;
47 48
        end

Svetlana's avatar
Svetlana committed
49
        Q steerCmd = 0;
Svetlana's avatar
Svetlana committed
50
        // steering control, "shared->steerCmd" [-1,1] is the value sent back to TORCS
51 52
	    steerCmd = (affordanceIn.angle - centerLine/roadWidth) / 0.541052 / coeSteer;

Svetlana's avatar
Svetlana committed
53 54 55 56
	    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
57

58 59 60 61
        desiredSpeed = 20;
	    if (affordanceIn.fast != 1)
	        desiredSpeed = 20 - abs(steeringRecordIn(0) + steeringRecordIn(1) + steeringRecordIn(2) + steeringRecordIn(3) + steeringRecordIn(4)) * 4.5;
	    end
62 63 64 65 66 67

	    if (desiredSpeed < 10)
	        desiredSpeed = 10;
	    end


Svetlana's avatar
Svetlana committed
68 69
	    if (desiredSpeed > slowDown)
            desiredSpeed = slowDown;
Svetlana's avatar
Svetlana committed
70 71
	    end

72
        // acceleration and brake
Svetlana's avatar
Svetlana committed
73 74
        if (desiredSpeed >= speedIn)
            Q accelCmd = 0.2*(desiredSpeed - speedIn + 1);
Svetlana's avatar
Svetlana committed
75 76 77
            if (accelCmd > 1)
                accelCmd = 1;
            end
78 79

            commandsOut(1) = accelCmd;
80
            commandsOut(3) = 0; // brakeCmd
Svetlana's avatar
Svetlana committed
81
        else
Svetlana's avatar
Svetlana committed
82
            Q brakeCmd = 0.1 * (speedIn - desiredSpeed);
Svetlana's avatar
Svetlana committed
83
            if (brakeCmd > 1)
84
                brakeCmd = 1;
Svetlana's avatar
Svetlana committed
85 86
            end

87
            commandsOut(3) = brakeCmd;
88
            commandsOut(1) = 0; // accelCmd
89
        end
90 91
    }
}