Skip to content

GitLab

Commit 848d98f3 authored by Svetlana Pavlitskaya's avatar Svetlana Pavlitskaya
Browse files

added further check if predicted values are within a specified range

parent 4fdadcfb
Hide whitespace changes
Inline Side-by-side
EMADL2TORCS/src/main/dp/Mastercomponent.emadl
View file @ 848d98f3
......@@ -4,7 +4,6 @@ import dp.subcomponents.*;
component Mastercomponent {
ports
in Z(0:255)^{3, 210, 280} imageIn,
//in Q^{16} groundTruthAffordance,
in Q(0 m/s:0.1 m/s:100 m/s) speedIn,
out Q^{16} predictedAffordanceOut,
out Q(-1:1)^{3} commandsOut;
......@@ -14,7 +13,6 @@ component Mastercomponent {
instance Denormalizer denorm;
instance KFMastercomponent kfm;
instance SteeringBuffer steeringBuffer;
instance VectorToAffordance vecToAffordance;
instance Localization locController;
instance AffordanceToVector affToVector;
......@@ -24,9 +22,6 @@ component Mastercomponent {
connect kfm.affordanceSmoothed -> locController.affordanceIn, driverController.affordanceIn, affToVector.affordanceIn;
connect locController.numLanes -> driverController.lanesCountIn;
connect affToVector.affordanceOut -> predictedAffordanceOut;
//connect groundTruthAffordance -> vecToAffordance.affordanceIndicators;
//connect vecToAffordance.affordance -> driverController.affordanceIn, locController.affordanceIn;
connect steeringBuffer.outputBuffer -> driverController.steeringRecordIn;
connect speedIn -> driverController.speedIn;
......
EMADL2TORCS/src/main/dp/subcomponents/Denormalizer.emadl
View file @ 848d98f3
......@@ -7,10 +7,19 @@ component Denormalizer {
out Affordance affordance;
implementation Math {
Q oldMin = 0.1;
Q oldMin = 0.1;
Q oldMax = 0.9;
Q oldRange = oldMax - oldMin;
for i=1:size(normalizedPredictions, 1)
if normalizedPredictions(i) < oldMin
normalizedPredictions(i) = oldMin;
elseif normalizedPredictions(i) > oldMax
normalizedPredictions(i) = oldMax;
end
end
Q newMinAngle = -0.5;
Q newRangeAngle = 0.5 - newMinAngle;
affordance.angle = (((normalizedPredictions(1) - oldMin) * newRangeAngle) / oldRange) + newMinAngle;
......
EMADL2TORCS/src/main/dp/subcomponents/DriverController.emadl
View file @ 848d98f3
......@@ -7,13 +7,13 @@ component DriverController {
in Q(0 m/s:0.1 m/s:100 m/s) speedIn,
in Z timerLeftIn,
in Z timerRightIn,
in LaneChange laneChangeIn,
in Z laneChangeIn,
in Z lanesCountIn,
out Q(-1:1)^{3} commandsOut, // [accelCmd, steerCmd, brakeCmd]
out Q steerCmd,
out Q(-1:1)^{3} commandsOut, // [accelCmd, steerCmd, brakeCmd]
out Q steerCmd,
out Z timerLeftOut,
out Z timerRightOut,
out LaneChange laneChangeOut;
out Z laneChangeOut;
implementation Math {
Q roadWidth = 8;
......@@ -21,7 +21,7 @@ component DriverController {
Q coeSteer = 1.0;
Q preML;
Q preMR;
Q slowDown = 100; // Acceleration
Q slowDown = 100; // Acceleration
Q preDistL = 60;
Q preDistR = 60;
B leftClear = false;
......@@ -103,9 +103,9 @@ component DriverController {
end
if (lanesCountIn == 2)
if (laneChangeIn == NO_CHANGE && affordanceIn.distMM < 15) // if current lane is occupied
if (laneChangeIn == 0 && affordanceIn.distMM < 15) // if current lane is occupied
if (affordanceIn.toMarkingLL > -8 && leftClear == true && steerTrend >= 0) // move to left lane
laneChangeIn = TO_LEFT;
laneChangeIn = -2;
coeSteer = 6;
rightClear = false;
rightTimer = 0;
......@@ -113,7 +113,7 @@ component DriverController {
leftTimer = 0;
timerSet = 30;
elseif (affordanceIn.toMarkingRR < 8 && rightClear == true && steerTrend <= 0) // move to right lane
laneChangeIn = TO_RIGHT;
laneChangeIn = 2;
coeSteer = 6;
rightClear = false;
rightTimer = 0;
......@@ -126,18 +126,18 @@ component DriverController {
slowDown = 0;
end
end
elseif (laneChangeIn == NO_CHANGE && affordanceIn.distMM >= 15) // prefer to stay in the right lane
elseif (laneChangeIn == 0 && affordanceIn.distMM >= 15) // prefer to stay in the right lane
if (affordanceIn.toMarkingLL < -8 && rightClear == true && steerTrend <= 0 && steerTrend > -0.2) // in right lane, so move to central lane
laneChangeIn = TO_RIGHT;
laneChangeIn = 2;
coeSteer = 6;
rightClear = false;
rightTimer = 20;
end
end
elseif (lanesCountIn == 3)
if (laneChangeIn == NO_CHANGE && affordanceIn.distMM < 15) // current lane is occupied
if (laneChangeIn == 0 && affordanceIn.distMM < 15) // current lane is occupied
if (affordanceIn.toMarkingLL > -8 && leftClear == true && steerTrend >= 0 && steerTrend < 0.2) // move to left lane
laneChangeIn = TO_LEFT;
laneChangeIn = -2;
coeSteer = 6;
rightClear = false;
rightTimer = 0;
......@@ -145,7 +145,7 @@ component DriverController {
leftTimer = 30;
timerSet = 60;
elseif (affordanceIn.toMarkingRR < 8 && rightClear == true && steerTrend <= 0 && steerTrend > -0.2) // move to right lane
laneChangeIn = TO_RIGHT;
laneChangeIn = 2;
coeSteer = 6;
rightClear = false;
rightTimer = 30;
......@@ -158,14 +158,14 @@ component DriverController {
slowDown = 0;
end
end
elseif (laneChangeIn == NO_CHANGE && affordanceIn.distMM >= 15) // prefer to stay in the central line
elseif (laneChangeIn == 0 && affordanceIn.distMM >= 15) // prefer to stay in the central line
if (affordanceIn.toMarkingRR > 8 && leftClear == true && steerTrend >= 0 && steerTrend < 0.2) // in right lane, so move to central lane
laneChangeIn = TO_LEFT;
laneChangeIn = -2;
coeSteer = 6;
leftClear = false;
leftTimer = 30;
elseif (affordanceIn.toMarkingLL < -8 && rightClear == true && steerTrend <= 0 && steerTrend > -0.2) // in left lane, so move to central lane
laneChangeIn = TO_RIGHT;
laneChangeIn = 2;
coeSteer = 6;
rightClear = false;
rightTimer = 30;
......@@ -174,7 +174,7 @@ component DriverController {
end
// go through all possible lane change maneuvers
if (laneChangeIn == NO_CHANGE) //car following
if (laneChangeIn == 0) //car following
if (-affordanceIn.toMarkingML + affordanceIn.toMarkingMR < 5.5)
coeSteer = 1.5;
centerLine = (affordanceIn.toMarkingML+affordanceIn.toMarkingMR)/2;
......@@ -191,7 +191,7 @@ component DriverController {
end
coeSteer = 0.3;
end
elseif (laneChangeIn == TO_LEFT)
elseif (laneChangeIn == -2)
if (-affordanceIn.toMarkingML+affordanceIn.toMarkingMR < 5.5)
centerLine = (affordanceIn.toMarkingLL+affordanceIn.toMarkingML)/2;
if (affordanceIn.toMarkingL > -5 && affordanceIn.toMarkingM < 1.5)
......@@ -200,9 +200,9 @@ component DriverController {
else
centerLine = (affordanceIn.toMarkingL+affordanceIn.toMarkingM)/2;
coeSteer = 20;
laneChangeIn = IN_LEFT;
laneChangeIn = -1;
end
elseif (laneChangeIn == IN_LEFT)
elseif (laneChangeIn == -1)
if (affordanceIn.toMarkingL > -5 && affordanceIn.toMarkingM < 1.5)
centerLine = (affordanceIn.toMarkingL+affordanceIn.toMarkingM)/2;
if (-affordanceIn.toMarkingML+affordanceIn.toMarkingMR < 5.5)
......@@ -210,9 +210,9 @@ component DriverController {
end
else
centerLine = (affordanceIn.toMarkingML+affordanceIn.toMarkingMR)/2;
laneChangeIn = NO_CHANGE;
laneChangeIn = 0;
end
elseif (laneChangeIn == TO_RIGHT)
elseif (laneChangeIn == 2)
if (-affordanceIn.toMarkingML+affordanceIn.toMarkingMR < 5.5)
centerLine = (affordanceIn.toMarkingRR+affordanceIn.toMarkingMR)/2;
if (affordanceIn.toMarkingR < 5 && affordanceIn.toMarkingM < 1.5)
......@@ -221,23 +221,23 @@ component DriverController {
else
centerLine = (affordanceIn.toMarkingR+affordanceIn.toMarkingM)/2;
coeSteer = 20;
laneChangeIn = IN_RIGHT;
laneChangeIn = 1;
end
elseif (laneChangeIn == IN_RIGHT)
elseif (laneChangeIn == 1)
if (affordanceIn.toMarkingR < 5 && affordanceIn.toMarkingM < 1.5)
centerLine = (affordanceIn.toMarkingR+affordanceIn.toMarkingM)/2;
if (-affordanceIn.toMarkingML+affordanceIn.toMarkingMR < 5.5)
centerLine = (centerLine+(affordanceIn.toMarkingML+affordanceIn.toMarkingMR)/2)/2;
else
centerLine = (affordanceIn.toMarkingML+affordanceIn.toMarkingMR)/2;
laneChangeIn = NO_CHANGE;
laneChangeIn = 0;
end
end
end
steerCmd = (affordanceIn.angle - centerLine/roadWidth) / 0.541052 / coeSteer;
if (laneChangeIn == NO_CHANGE && coeSteer > 1 && steerCmd > 0.1) // reshape the steering control curve
if (laneChangeIn == 0 && coeSteer > 1 && steerCmd > 0.1) // reshape the steering control curve
steerCmd = steerCmd * (2.5*steerCmd+0.75);
end
commandsOut(2) = steerCmd;
......
EMADL2TORCS/src/main/dp/subcomponents/SteeringBuffer.emadl
View file @ 848d98f3
......@@ -4,15 +4,15 @@ component SteeringBuffer {
ports in Q inSteerCmd,
in Z timerLeftIn,
in Z timerRightIn,
in LaneChange laneChangeIn,
in Z laneChangeIn,
out Z timerLeftOut,
out Z timerRightOut,
out LaneChange laneChangeOut,
out Z laneChangeOut,
out Q^{5} outputBuffer;
implementation Math {
static Q^{5} buffer = [0; 0; 0; 0; 0];
static LaneChange laneChangeBuffer = NO_CHANGE;
static Z laneChangeBuffer = 0;
static Z timerLeftBuffer = 0;
static Z timerRightBuffer = 0;
......
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