Drivercontroller.emadl 773 Bytes
Newer Older
1 2 3 4
package dp.subcomponents;

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

	implementation Math {
9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24
	    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
	    commandsOut(2,1,1) = Constant * (angle-distToLaneCenter/roadWidth);  // steering

	    // Acceleration
	    Q vMax = 72;
	    Q vC = 1;
	    Q vD = 0;
	    Q desiredSpeed = vMax * (exp(-(vC/vMax) * affordanceIn(0,0,6) - vD));
	    commandsOut(1,1,1) = desiredSpeed/vMax;  // acceleration

25 26 27
        commandsOut(3,1,1)=0.0;  // brake
    }
}