Localization.emadl 1.37 KB
Newer Older
1 2
package dp.subcomponents;

Svetlana's avatar
Svetlana committed
3
component Localization {
4 5 6 7
    ports
	in Affordance affordanceIn,
	out B onLane,
	out B onMarking,
Svetlana's avatar
Svetlana committed
8
	out Z numLanes,
9 10 11 12 13 14 15 16 17 18 19
	out Lane lane;

	implementation Math {
		onLane = false;
		onMarking = false;

		// Car is on lane
		if (-affordanceIn.toMarkingML + affordanceIn.toMarkingMR < 5.5)
			onLane = true;
			onMarking = false;
			if (affordanceIn.toMarkingLL <= -7.5 && affordanceIn.toMarkingRR >= 7.5)
Svetlana's avatar
Svetlana committed
20
				numLanes = 1;
21 22 23
				lane = CENTER_LANE;

			elseif (affordanceIn.toMarkingLL > -7.5 && affordanceIn.toMarkingRR < 7.5)
Svetlana's avatar
Svetlana committed
24
				numLanes = 3;
25 26 27
				lane = CENTER_LANE;

			elseif (affordanceIn.toMarkingLL > -7.5 || affordanceIn.toMarkingRR < 7.5)
Svetlana's avatar
Svetlana committed
28
				numLanes = 2;
29 30 31 32 33 34 35 36
				if (affordanceIn.toMarkingRR >=7.5)
					lane = RIGHT_LANE;
				end
				if (affordanceIn.toMarkingLL <= -7.5)
					lane = LEFT_LANE;
				end

			else
Svetlana's avatar
Svetlana committed
37
				numLanes = 0;
38 39 40 41 42 43 44 45
				onLane = false;
				lane = OUTSIDE_LANE;
			end
		else // Car is on marking
			onLane = false;
			onMarking = true;
			// Car on central marking
			if (affordanceIn.toMarkingR < 6 && affordanceIn.toMarkingL > -6)
Svetlana's avatar
Svetlana committed
46
				numLanes = 2;
47 48 49 50
				lane = CENTER_LANE;
			
			// When on Outer lanes consider the road only 1 lane.
			else
Svetlana's avatar
Svetlana committed
51
				numLanes = 1;
52 53 54 55 56 57 58 59 60
				if (affordanceIn.toMarkingL < -6)
                			lane = LEFT_LANE;
            			elseif (affordanceIn.toMarkingR > 6)
                			lane = RIGHT_LANE;
            			end
			end			
		end
	}
}