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

component Drivercontroller {
    ports
5
	    in Q(0:1)^{1,1,13} affordanceIn,
6
	    in Q speedIn,
7 8 9
	    out Q(0:1)^{3,1,1} commandsOut;

	implementation Math {
10 11 12 13 14 15 16
	    Q Constant = 1;
	    Q angle = affordanceIn(0,0,0);
	    Q roadWidth = affordanceIn(0,0,1) + affordanceIn(0,0,4);
	    Q laneWidth = affordanceIn(0,0,2) + affordanceIn(0,0,3);
	    Q distToLaneCenter = abs(laneWidth/2 - affordanceIn(0,0,2));

	    // Steering
17
	    commandsOut(2,1,1) = 0;//Constant * (angle-distToLaneCenter/roadWidth);  // steering
18 19

	    // Acceleration
20
	    Q vMax = 25;
21 22
	    Q vC = 1;
	    Q vD = 0;
23
	    Q desiredSpeed = 15;//vMax * (exp(-(vC/vMax) * affordanceIn(0,0,6) - vD));
24

25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42
	    Q accelCmd = 0.1 * (desiredSpeed - speedIn + 1);
	    if accelCmd > 1
            accelCmd = 1.0;
        end

        Q brakeCmd = 0.1 * (speedIn - desiredSpeed);
        if brakeCmd > 1
            brakeCmd = 1;
        end

        // acceleration and brake
	    if speedIn < desiredSpeed
	        commandsOut(1,1,1) = accelCmd;
	        commandsOut(3,1,1) = 0;
	    else
            commandsOut(1,1,1) = 0;
            commandsOut(3,1,1) = brakeCmd;
        end
43 44
    }
}