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

component Drivercontroller {
    ports
Svetlana's avatar
Svetlana committed
5
        in Q^{4} anglesIn,
6 7
	    in Affordance affordanceIn,
	    in Q(0 m/s:0.1 m/s:100 m/s) speedIn,
Svetlana's avatar
Svetlana committed
8
	    out Q(0:1)^{3} commandsOut;
9 10

	implementation Math {
Svetlana's avatar
Svetlana committed
11
        Q roadWidth = 8;
12 13

	    // Acceleration
Svetlana's avatar
Svetlana committed
14
	    Q slowDown = 100;
15

Svetlana's avatar
Svetlana committed
16 17 18 19
	    if (affordanceIn.distMM < 15)
            Q vMax = 20;
            Q vC = 2.772;
            Q vD = 0.693;
Svetlana's avatar
Svetlana committed
20
            slowDown = vMax * (exp(-(vC/vMax) * affordanceIn.distMM - vD));
21

Svetlana's avatar
Svetlana committed
22 23
            if (slowDown < 0)
                slowDown = 0;
Svetlana's avatar
Svetlana committed
24
            end
25 26
        end

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

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

Svetlana's avatar
Svetlana committed
51 52 53
	    Q desiredSpeed = 20;
	    if (desiredSpeed > slowDown)
            desiredSpeed = slowDown;
Svetlana's avatar
Svetlana committed
54 55
	    end

56
        // acceleration and brake
Svetlana's avatar
Svetlana committed
57 58
        if (desiredSpeed >= speedIn)
            Q accelCmd = 0.2*(desiredSpeed - speedIn + 1);
Svetlana's avatar
Svetlana committed
59 60 61
            if (accelCmd > 1)
                accelCmd = 1;
            else
Svetlana's avatar
Svetlana committed
62
                commandsOut(1) = accelCmd;
Svetlana's avatar
Svetlana committed
63
            end
Svetlana's avatar
Svetlana committed
64
            commandsOut(3) = 0;
Svetlana's avatar
Svetlana committed
65
        else
Svetlana's avatar
Svetlana committed
66
            Q brakeCmd = 0.1 * (speedIn - desiredSpeed);
Svetlana's avatar
Svetlana committed
67
            if (brakeCmd > 1)
Svetlana's avatar
Svetlana committed
68
                commandsOut(3) = 1;
Svetlana's avatar
Svetlana committed
69
            else
Svetlana's avatar
Svetlana committed
70
                commandsOut(3) = brakeCmd;
Svetlana's avatar
Svetlana committed
71 72
            end

Svetlana's avatar
Svetlana committed
73
            commandsOut(1) = 0;
74
        end
75 76
    }
}