Skip to content

GitLab

Commit 8a9afe13 authored by Svetlana's avatar Svetlana
Browse files

Advanced driver controller

parent 4b2a268a
Hide whitespace changes
Inline Side-by-side
Showing with 89 additions and 51 deletions
TorcsEMAMGenerator/.idea/compiler.xml
View file @ 8a9afe13
......@@ -7,10 +7,12 @@
<sourceTestOutputDir name="target/generated-test-sources/test-annotations" />
<outputRelativeToContentRoot value="true" />
<module name="experiments" />
<module name="torcs-dl" />
</profile>
</annotationProcessing>
<bytecodeTargetLevel>
<module name="experiments" target="1.8" />
<module name="torcs-dl" target="1.8" />
</bytecodeTargetLevel>
</component>
</project>
\ No newline at end of file
TorcsEMAMGenerator/.idea/misc.xml
View file @ 8a9afe13
......@@ -10,7 +10,7 @@
</list>
</option>
</component>
<component name="ProjectRootManager" version="2" languageLevel="JDK_1_8" assert-keyword="true" jdk-15="true" project-jdk-name="1.8" project-jdk-type="JavaSDK">
<component name="ProjectRootManager" version="2" languageLevel="JDK_1_8" project-jdk-name="1.8" project-jdk-type="JavaSDK">
<output url="file://$PROJECT_DIR$/classes" />
</component>
</project>
\ No newline at end of file
TorcsEMAMGenerator/.idea/modules.xml
View file @ 8a9afe13
......@@ -3,6 +3,7 @@
<component name="ProjectModuleManager">
<modules>
<module fileurl="file://$PROJECT_DIR$/experiments.iml" filepath="$PROJECT_DIR$/experiments.iml" />
<module fileurl="file://$PROJECT_DIR$/torcs-dl.iml" filepath="$PROJECT_DIR$/torcs-dl.iml" />
</modules>
</component>
</project>
\ No newline at end of file
TorcsEMAMGenerator/src/main/models/dp/Mastercomponent.emadl
View file @ 8a9afe13
......@@ -3,21 +3,22 @@ import dp.subcomponents.*;
component Mastercomponent {
ports
in Z(0:255)^{3, 210, 280} imageIn,
in Q speedIn,
out Q(0:1)^{3,1,1} commandsOut;
in Z(0:255)^{3, 210, 280} imageIn,
in Q(0 m/s:0.1 m/s:100 m/s) speedIn,
out Q(0:1)^{3,1,1} commandsOut;
instance Imagepreprocessing ip;
instance Dpnet dpnet;
instance Drivercontroller dc;
instance Unnormalizer un;
instance KFMastercomponent kfm;
//instance KFMastercomponent kfm;
connect imageIn -> ip.imageIn;
connect ip.imageOut -> dpnet.image;
connect dpnet.predictions -> un.normalizedPredictions;
//connect un.affordance -> kfm.predictions;
//connect kfm.predictionsSmoothed -> dc.affordanceIn;
connect un.affordance -> dc.affordanceIn;
connect speedIn -> dc.speedIn;
connect dc.commandsOut -> commandsOut;
}
\ No newline at end of file
TorcsEMAMGenerator/src/main/models/dp/subcomponents/Drivercontroller.emadl
View file @ 8a9afe13
......@@ -12,35 +12,69 @@ component Drivercontroller {
//Q roadWidth = affordanceIn.toMarkingLL + affordanceIn.toMarkingRR;
//Q laneWidth = affordanceIn(0,0,2) + affordanceIn(0,0,3);
//Q distToLaneCenter = abs(laneWidth/2 - affordanceIn(0,0,2));
// Steering
commandsOut(2,1,1) = 0;//Constant * (angle-distToLaneCenter/roadWidth); // steering
Q road_width = 8;
// Acceleration
Q vMax = 25;
Q vC = 1;
Q vD = 0;
Q desiredSpeed = 15;//vMax * (exp(-(vC/vMax) * affordanceIn(0,0,6) - vD));
Q slow_down = 100;
commandsOut(2,1,1) = 0;
if (affordanceIn.distMM < 15)
Q vMax = 20;
Q vC = 2.772;
Q vD = 0.693;
slow_down = vMax * (exp(-(vC/vMax) * affordanceIn.distMM - vD));
Q(0:1) acceleration = 0.1 * (desiredSpeed - speedIn + 1);
if acceleration > 1
acceleration = 1.0;
if (slow_down < 0)
slow_down = 0;
end
end
Q(0:1) brake = 0.1 * (speedIn - desiredSpeed);
if brake > 1
brake = 1;
Q center_line;
Q coe_steer;
Q pre_ML;
Q pre_MR;
if (-affordanceIn.toMarkingML + affordanceIn.toMarkingMR < 5.5)
coe_steer = 1.5;
center_line = (affordanceIn.toMarkingML + affordanceIn.toMarkingMR) / 2;
pre_ML = affordanceIn.toMarkingML;
pre_MR = affordanceIn.toMarkingMR;
if (affordanceIn.toMarkingM < 1)
coe_steer = 0.4;
end
else
if (-pre_ML > pre_MR)
center_line = (affordanceIn.toMarkingL + affordanceIn.toMarkingM) / 2;
else
center_line=(affordanceIn.toMarkingR + affordanceIn.toMarkingM) / 2;
end
coe_steer = 0.3;
end
// steering control, "shared->steerCmd" [-1,1] is the value sent back to TORCS
commandsOut(2,1,1) = (affordanceIn.angle - center_line/road_width) / 0.541052/coe_steer;
Q desired_speed = 20;
if (desired_speed > slow_down)
desired_speed = slow_down;
end
// acceleration and brake
if speedIn < desiredSpeed
commandsOut(1,1,1) = acceleration;
commandsOut(3,1,1) = 0;
else
if (desired_speed >= speedIn)
Q accelCmd = 0.2*(desired_speed - speedIn + 1);
if (accelCmd > 1)
accelCmd = 1;
else
commandsOut(1,1,1) = accelCmd;
end
commandsOut(3,1,1) = 0;
else
Q brakeCmd = 0.1 * (speedIn - desired_speed);
if (brakeCmd > 1)
commandsOut(3,1,1) = 1;
else
commandsOut(3,1,1) = brakeCmd;
end
commandsOut(1,1,1) = 0;
commandsOut(3,1,1) = brake;
end
}
}
\ No newline at end of file
TorcsEMAMGenerator/src/main/models/dp/subcomponents/KFMastercomponent.emadl
View file @ 8a9afe13
......@@ -4,7 +4,7 @@ component KFMastercomponent {
ports in Affordance predictions,
out Affordance predictionsSmoothed;
instance Kalmanfilter kfLL;
//instance Kalmanfilter kfLL;
//instance Kalmanfilter([predictions.distLL, 0]) kfLL;
//instance Kalmanfilter([predictions.distMM, 0]) kfMM;
//instance Kalmanfilter([predictions.distRR, 0]) kfRR;
......
TorcsEMAMGenerator/src/main/models/dp/subcomponents/Kalmanfilter.emadl
View file @ 8a9afe13
......@@ -5,23 +5,23 @@ component Kalmanfilter {
out Q^{1,2} state;
implementation Math {
Q^{1, 1} dt = 0;
Q^{2, 2} A = [1, dt; 0, 1]; // state transition matrix
Q^{1, 2} measM = [1, 0] ; // C: measurement matrix
Q^{2, 2} procNoiseCov = [dt*dt*dt/3, dt*dt/2; dt*dt/2, dt]; // Q: covariance of process noise
Q^{1, 1} measNoiseCov = 5; // R: covariance of measurement noise
Q^{2, 2} errCov = [1000, 0; 0, 1000]; // P: estimate error covariance
Q^{2, 2} I = [1, 0; 0, 1];
//Q^{1, 1} dt = 0;
//Q^{2, 2} A = [1, dt; 0, 1]; // state transition matrix
//Q^{1, 2} measM = [1, 0] ; // C: measurement matrix
//Q^{2, 2} procNoiseCov = [dt*dt*dt/3, dt*dt/2; dt*dt/2, dt]; // Q: covariance of process noise
//Q^{1, 1} measNoiseCov = 5; // R: covariance of measurement noise
//Q^{2, 2} errCov = [1000, 0; 0, 1000]; // P: estimate error covariance
//Q^{2, 2} I = [1, 0; 0, 1];
// Prediction step
X = A * X;
errCov = A * errCov * trans(A) + procNoiseCov;
//X = A * X;
//errCov = A * errCov * trans(A) + procNoiseCov;
// Correction step
Q^{2,1} kalmanGain = errCov * trans(measM) * inverse(measM * errCov * trans(measM) + measNoiseCov);
X = X + kalmanGain * (measurement - measM * X);
errCov = (I - kalmanGain * errCov) * errorCovariance;
state = X(1, 1);
//Q^{2,1} kalmanGain = errCov * trans(measM) * inverse(measM * errCov * trans(measM) + measNoiseCov);
//X = X + kalmanGain * (measurement - measM * X);
//errCov = (I - kalmanGain * errCov) * errorCovariance;
//state = X(1, 1);
}
}
TorcsEMAMGenerator/src/main/models/dp/subcomponents/Unnormalizer.emadl
View file @ 8a9afe13
......@@ -5,18 +5,18 @@ component Unnormalizer {
out Affordance affordance;
implementation Math {
affordance.angle = normalizedPredictions(1,1,1) - 0.5;
affordance.toMarkingLL = normalizedPredictions(2,1,1) * 4.5 - 7;
affordance.toMarkingML = normalizedPredictions(3,1,1) * 5.5 - 2;
affordance.toMarkingMR = normalizedPredictions(4,1,1) * 4.5 + 2.5;
affordance.toMarkingRR = normalizedPredictions(5,1,1) * 5.5 - 9.5;
affordance.distLL = normalizedPredictions(6,1,1) * 5 - 5.5;
affordance.distMM = normalizedPredictions(7,1,1) * 5 - 0.5;
affordance.distRR = normalizedPredictions(8,1,1) * 5.5 + 4;
affordance.toMarkingL = normalizedPredictions(9,1,1) * 75;
affordance.toMarkingM = normalizedPredictions(10,1,1) * 75;
affordance.toMarkingR = normalizedPredictions(11,1,1) * 75;
affordance.distL = normalizedPredictions(12,1,1) * 75;
affordance.distR = normalizedPredictions(13,1,1) * 75;
affordance.angle = (normalizedPredictions(0,0,0) - 0.5)*1.1;
affordance.toMarkingLL = (normalizedPredictions(0,0,1) - 1.48181) * 6.8752;
affordance.toMarkingML = (normalizedPredictions(0,0,2) - 0.98)*6.25;
affordance.toMarkingMR = (normalizedPredictions(0,0,3) - 0.02)*6.25;
affordance.toMarkingRR = (normalizedPredictions(0,0,4) + 0.48181)*6.8752;
affordance.distLL = (normalizedPredictions(0,0,5) - 0.12)*95;
affordance.distMM = (normalizedPredictions(0,0,6) - 0.12)*95;
affordance.distRR = (normalizedPredictions(0,0,7) - 0.12)*95;
affordance.toMarkingL = (normalizedPredictions(0,0,8) - 1.34445)*5.6249;
affordance.toMarkingM = (normalizedPredictions(0,0,9) - 0.39091)*6.8752;
affordance.toMarkingR = (normalizedPredictions(0,0,10) + 0.34445)*5.6249;
affordance.distL = (normalizedPredictions(0,0,11) - 0.12)*95;
affordance.distR = (normalizedPredictions(0,0,12) - 0.12)*95;
}
}
\ No newline at end of file
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