Commit f05e7021 authored by Svetlana Pavlitskaya's avatar Svetlana Pavlitskaya

Localization controller model

parent b046691a
package dp.subcomponents;
enum Lane {
CENTER_LANE | RIGHT_LANE | LEFT_LANE | OUTSIDE_LANE
}
package dp.subcomponents;
component LocalizationController {
ports
in Affordance affordanceIn,
out B onLane,
out B onMarking,
out Z laneNumber,
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)
laneNumber = 1;
lane = CENTER_LANE;
elseif (affordanceIn.toMarkingLL > -7.5 && affordanceIn.toMarkingRR < 7.5)
laneNumber = 3;
lane = CENTER_LANE;
elseif (affordanceIn.toMarkingLL > -7.5 || affordanceIn.toMarkingRR < 7.5)
laneNumber = 2;
if (affordanceIn.toMarkingRR >=7.5)
lane = RIGHT_LANE;
end
if (affordanceIn.toMarkingLL <= -7.5)
lane = LEFT_LANE;
end
else
laneNumber = 0;
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)
laneNumber = 2;
lane = CENTER_LANE;
// When on Outer lanes consider the road only 1 lane.
else
laneNumber = 1;
if (affordanceIn.toMarkingL < -6)
lane = LEFT_LANE;
elseif (affordanceIn.toMarkingR > 6)
lane = RIGHT_LANE;
end
end
end
}
}
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment