Commit 454a8bd7 authored by Svetlana's avatar Svetlana

Driver controller works based on the dynamic lanes count detection

parent 485bd921
package dp.subcomponents;
component DriverController {
ports
in Q^{5} steeringRecordIn,
in Affordance affordanceIn,
in Q(0 m/s:0.1 m/s:100 m/s) speedIn,
out Q(-1:1)^{3} commandsOut, // [accelCmd, steerCmd, brakeCmd]
out Q steerCmd;
implementation Math {
Q roadWidth = 8;
Q centerLine;
Q coeSteer = 1.0;
Q preML;
Q preMR;
Q desiredSpeed;
Q slowDown = 100; // Acceleration
// Car following speed model: if a car is less than 20m ahead
if (affordanceIn.distMM < 20)
Q vMax = 20;
Q vC = 2.772;
Q vD = -0.693;
// optimal velocity car-following model
slowDown = vMax * (1-exp(-(vC/vMax) * affordanceIn.distMM - vD));
if (slowDown < 0)
slowDown = 0;
end
end
if (-affordanceIn.toMarkingML + affordanceIn.toMarkingMR < 5.5)
coeSteer = 1.5;
centerLine = (affordanceIn.toMarkingML + affordanceIn.toMarkingMR) / 2;
preML = affordanceIn.toMarkingML;
preMR = affordanceIn.toMarkingMR;
if (affordanceIn.toMarkingM < 1)
coeSteer = 0.4;
end
else
if (-preML > preMR)
centerLine = (affordanceIn.toMarkingL + affordanceIn.toMarkingM) / 2;
else
centerLine=(affordanceIn.toMarkingR + affordanceIn.toMarkingM) / 2;
end
coeSteer = 0.3;
end
// steering control, "shared->steerCmd" [-1,1] is the value sent back to TORCS
steerCmd = (affordanceIn.angle - centerLine/roadWidth) / 0.541052 / coeSteer;
if (coeSteer > 1 && steerCmd > 0.1) // reshape the steering control curve
steerCmd = steerCmd * (2.5 * steerCmd + 0.75);
end
commandsOut(2) = steerCmd;
desiredSpeed = 20; // 20m/s = 72 kmh
if (affordanceIn.fast != 1)
desiredSpeed = 20 - abs(steeringRecordIn(1) + steeringRecordIn(2) + steeringRecordIn(3) + steeringRecordIn(4) + steeringRecordIn(5)) * 4.5;
end
if (desiredSpeed < 10)
desiredSpeed = 10;
end
if (desiredSpeed > slowDown)
desiredSpeed = slowDown;
end
// acceleration and brake
if (desiredSpeed >= speedIn)
Q accelCmd = 0.2*(desiredSpeed - speedIn + 1);
if (accelCmd > 1)
accelCmd = 1;
end
commandsOut(1) = accelCmd;
commandsOut(3) = 0; // brakeCmd
else
Q brakeCmd = 0.1 * (speedIn - desiredSpeed);
if (brakeCmd > 1)
brakeCmd = 1;
end
commandsOut(3) = brakeCmd;
commandsOut(1) = 0; // accelCmd
end
}
}
......@@ -5,7 +5,7 @@ component LocalizationController {
in Affordance affordanceIn,
out B onLane,
out B onMarking,
out Z laneNumber,
out Z lanesCount,
out Lane lane;
implementation Math {
......@@ -17,15 +17,15 @@ component LocalizationController {
onLane = true;
onMarking = false;
if (affordanceIn.toMarkingLL <= -7.5 && affordanceIn.toMarkingRR >= 7.5)
laneNumber = 1;
lanesCount = 1;
lane = CENTER_LANE;
elseif (affordanceIn.toMarkingLL > -7.5 && affordanceIn.toMarkingRR < 7.5)
laneNumber = 3;
lanesCount = 3;
lane = CENTER_LANE;
elseif (affordanceIn.toMarkingLL > -7.5 || affordanceIn.toMarkingRR < 7.5)
laneNumber = 2;
lanesCount = 2;
if (affordanceIn.toMarkingRR >=7.5)
lane = RIGHT_LANE;
end
......@@ -34,7 +34,7 @@ component LocalizationController {
end
else
laneNumber = 0;
lanesCount = 0;
onLane = false;
lane = OUTSIDE_LANE;
end
......@@ -43,12 +43,12 @@ component LocalizationController {
onMarking = true;
// Car on central marking
if (affordanceIn.toMarkingR < 6 && affordanceIn.toMarkingL > -6)
laneNumber = 2;
lanesCount = 2;
lane = CENTER_LANE;
// When on Outer lanes consider the road only 1 lane.
else
laneNumber = 1;
lanesCount = 1;
if (affordanceIn.toMarkingL < -6)
lane = LEFT_LANE;
elseif (affordanceIn.toMarkingR > 6)
......
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