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

Advanced driver controller

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