Commit 9d51796d authored by Svetlana's avatar Svetlana

added angle buffer model

parent 5c6b771d
<component name="libraryTable">
<library name="Maven: de.monticore.lang:Tagging:0.0.1">
<CLASSES>
<root url="jar://$MAVEN_REPOSITORY$/de/monticore/lang/Tagging/0.0.1/Tagging-0.0.1.jar!/" />
</CLASSES>
<JAVADOC>
<root url="jar://$MAVEN_REPOSITORY$/de/monticore/lang/Tagging/0.0.1/Tagging-0.0.1-javadoc.jar!/" />
</JAVADOC>
<SOURCES>
<root url="jar://$MAVEN_REPOSITORY$/de/monticore/lang/Tagging/0.0.1/Tagging-0.0.1-sources.jar!/" />
</SOURCES>
</library>
</component>
\ No newline at end of file
<component name="libraryTable">
<library name="Maven: de.monticore.lang.monticar:enum:0.0.10-SNAPSHOT">
<CLASSES>
<root url="jar://$MAVEN_REPOSITORY$/de/monticore/lang/monticar/enum/0.0.10-SNAPSHOT/enum-0.0.10-20180104.160312-1.jar!/" />
</CLASSES>
<JAVADOC>
<root url="jar://$MAVEN_REPOSITORY$/de/monticore/lang/monticar/enum/0.0.10-SNAPSHOT/enum-0.0.10-20180104.160312-1-javadoc.jar!/" />
</JAVADOC>
<SOURCES>
<root url="jar://$MAVEN_REPOSITORY$/de/monticore/lang/monticar/enum/0.0.10-SNAPSHOT/enum-0.0.10-20180104.160312-1-sources.jar!/" />
</SOURCES>
</library>
</component>
\ No newline at end of file
<component name="libraryTable">
<library name="Maven: de.monticore.lang.monticar:struct:0.0.10-SNAPSHOT">
<CLASSES>
<root url="jar://$MAVEN_REPOSITORY$/de/monticore/lang/monticar/struct/0.0.10-SNAPSHOT/struct-0.0.10-20180111.201020-1.jar!/" />
</CLASSES>
<JAVADOC>
<root url="jar://$MAVEN_REPOSITORY$/de/monticore/lang/monticar/struct/0.0.10-SNAPSHOT/struct-0.0.10-20180111.201020-1-javadoc.jar!/" />
</JAVADOC>
<SOURCES>
<root url="jar://$MAVEN_REPOSITORY$/de/monticore/lang/monticar/struct/0.0.10-SNAPSHOT/struct-0.0.10-20180111.201020-1-sources.jar!/" />
</SOURCES>
</library>
</component>
\ No newline at end of file
<component name="libraryTable">
<library name="Maven: de.monticore:monticore-emf-runtime:4.5.4-SNAPSHOT">
<CLASSES>
<root url="jar://$MAVEN_REPOSITORY$/de/monticore/monticore-emf-runtime/4.5.4-SNAPSHOT/monticore-emf-runtime-4.5.4-SNAPSHOT.jar!/" />
</CLASSES>
<JAVADOC>
<root url="jar://$MAVEN_REPOSITORY$/de/monticore/monticore-emf-runtime/4.5.4-SNAPSHOT/monticore-emf-runtime-4.5.4-SNAPSHOT-javadoc.jar!/" />
</JAVADOC>
<SOURCES>
<root url="jar://$MAVEN_REPOSITORY$/de/monticore/monticore-emf-runtime/4.5.4-SNAPSHOT/monticore-emf-runtime-4.5.4-SNAPSHOT-sources.jar!/" />
</SOURCES>
</library>
</component>
\ No newline at end of file
<component name="libraryTable">
<library name="Maven: de.monticore:monticore-grammar:4.5.4-SNAPSHOT">
<CLASSES>
<root url="jar://$MAVEN_REPOSITORY$/de/monticore/monticore-grammar/4.5.4-SNAPSHOT/monticore-grammar-4.5.4-SNAPSHOT.jar!/" />
</CLASSES>
<JAVADOC>
<root url="jar://$MAVEN_REPOSITORY$/de/monticore/monticore-grammar/4.5.4-SNAPSHOT/monticore-grammar-4.5.4-SNAPSHOT-javadoc.jar!/" />
</JAVADOC>
<SOURCES>
<root url="jar://$MAVEN_REPOSITORY$/de/monticore/monticore-grammar/4.5.4-SNAPSHOT/monticore-grammar-4.5.4-SNAPSHOT-sources.jar!/" />
</SOURCES>
</library>
</component>
\ No newline at end of file
<component name="libraryTable">
<library name="Maven: de.monticore:monticore-runtime:4.5.4-SNAPSHOT">
<CLASSES>
<root url="jar://$MAVEN_REPOSITORY$/de/monticore/monticore-runtime/4.5.4-SNAPSHOT/monticore-runtime-4.5.4-SNAPSHOT.jar!/" />
</CLASSES>
<JAVADOC>
<root url="jar://$MAVEN_REPOSITORY$/de/monticore/monticore-runtime/4.5.4-SNAPSHOT/monticore-runtime-4.5.4-SNAPSHOT-javadoc.jar!/" />
</JAVADOC>
<SOURCES>
<root url="jar://$MAVEN_REPOSITORY$/de/monticore/monticore-runtime/4.5.4-SNAPSHOT/monticore-runtime-4.5.4-SNAPSHOT-sources.jar!/" />
</SOURCES>
</library>
</component>
\ No newline at end of file
......@@ -5,20 +5,24 @@ component Mastercomponent {
ports
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;
out Q(0:1)^{3} commandsOut;
instance Imagepreprocessing ip;
instance Dpnet dpnet;
instance Drivercontroller dc;
instance Unnormalizer un;
instance Drivercontroller driverController;
instance Unnormalizer unnorm;
//instance KFMastercomponent kfm;
instance AngleBuffer angleBuffer;
connect imageIn -> dpnet.image;
connect dpnet.predictions -> unnorm.normalizedPredictions;
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;
//connect kfm.predictionsSmoothed -> driverController.affordanceIn;
connect unnorm.affordance -> angleBuffer.inputAffordance;
connect unnorm.affordance -> driverController.affordanceIn;
connect angleBuffer.outputBuffer -> driverController.anglesIn;
connect speedIn -> driverController.speedIn;
connect driverController.commandsOut -> commandsOut;
}
\ No newline at end of file
......@@ -18,6 +18,4 @@ struct Affordance {
Q(0m : 0.1m : 75m) distRR;
Q(0 : 0.1 : 1) fast;
}
package dp.subcomponents;
component AngleBuffer {
ports in Affordance inputAffordance,
out Q^{4} outputBuffer;
implementation Math {
static Q^{4} buffer = [0 0 0 0];
buffer(4) = buffer(3);
buffer(3) = buffer(2);
buffer(2) = buffer(1);
buffer(1) = inputAffordance.angle;
outputBuffer(1) = buffer(1);
outputBuffer(2) = buffer(2);
outputBuffer(3) = buffer(3);
outputBuffer(4) = buffer(4);
}
}
\ No newline at end of file
package dp.subcomponents;
component Dpnet{
ports in Z(0:255)^{3, 210, 280} image,
out Q(-oo:oo)^{14,1,1} predictions;
out Q(-oo:oo)^{14} predictions;
implementation CNN {
......@@ -27,6 +28,5 @@ component Dpnet{
fc() ->
FullyConnected(units=14, no_bias=true) ->
predictions
}
}
......@@ -2,79 +2,75 @@ package dp.subcomponents;
component Drivercontroller {
ports
in Q^{4} anglesIn,
in Affordance affordanceIn,
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} commandsOut;
implementation Math {
//Q Constant = 1;
//Q angle = affordanceIn.angle;
//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));
Q road_width = 8;
Q roadWidth = 8;
// Acceleration
Q slow_down = 100;
Q slowDown = 100;
if (affordanceIn.distMM < 15)
Q vMax = 20;
Q vC = 2.772;
Q vD = 0.693;
slow_down = vMax * (exp(-(vC/vMax) * affordanceIn.distMM - vD));
slowDown = vMax * (exp(-(vC/vMax) * affordanceIn.distMM - vD));
if (slow_down < 0)
slow_down = 0;
if (slowDown < 0)
slowDown = 0;
end
end
Q center_line;
Q coe_steer;
Q pre_ML;
Q pre_MR;
Q centerLine;
Q coeSteer;
Q preML;
Q preMR;
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;
coeSteer = 1.5;
centerLine = (affordanceIn.toMarkingML + affordanceIn.toMarkingMR) / 2;
preML = affordanceIn.toMarkingML;
preMR = affordanceIn.toMarkingMR;
if (affordanceIn.toMarkingM < 1)
coe_steer = 0.4;
coeSteer = 0.4;
end
else
if (-pre_ML > pre_MR)
center_line = (affordanceIn.toMarkingL + affordanceIn.toMarkingM) / 2;
if (-preML > preMR)
centerLine = (affordanceIn.toMarkingL + affordanceIn.toMarkingM) / 2;
else
center_line=(affordanceIn.toMarkingR + affordanceIn.toMarkingM) / 2;
centerLine=(affordanceIn.toMarkingR + affordanceIn.toMarkingM) / 2;
end
coe_steer = 0.3;
coeSteer = 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;
commandsOut(2) = (affordanceIn.angle - centerLine/roadWidth) / 0.541052/coeSteer;
Q desired_speed = 20;
if (desired_speed > slow_down)
desired_speed = slow_down;
Q desiredSpeed = 20;
if (desiredSpeed > slowDown)
desiredSpeed = slowDown;
end
// acceleration and brake
if (desired_speed >= speedIn)
Q accelCmd = 0.2*(desired_speed - speedIn + 1);
if (desiredSpeed >= speedIn)
Q accelCmd = 0.2*(desiredSpeed - speedIn + 1);
if (accelCmd > 1)
accelCmd = 1;
else
commandsOut(1,1,1) = accelCmd;
commandsOut(1) = accelCmd;
end
commandsOut(3,1,1) = 0;
commandsOut(3) = 0;
else
Q brakeCmd = 0.1 * (speedIn - desired_speed);
Q brakeCmd = 0.1 * (speedIn - desiredSpeed);
if (brakeCmd > 1)
commandsOut(3,1,1) = 1;
commandsOut(3) = 1;
else
commandsOut(3,1,1) = brakeCmd;
commandsOut(3) = brakeCmd;
end
commandsOut(1,1,1) = 0;
commandsOut(1) = 0;
end
}
}
\ No newline at end of file
package dp.subcomponents;
component Imagepreprocessing {
ports
in Z(0:255)^{3, 210, 280} imageIn,
out Z(0:255)^{3, 210, 280} imageOut;
implementation Math {
imageOut = imageIn;
}
}
\ No newline at end of file
......@@ -3,8 +3,8 @@ package dp.subcomponents;
component Safetycontroller {
ports
in Z(0:255)^{3, 210, 280} imageIn,
in Q(0:1)^{14,1,1} affordanceIn,
out Q(0:1)^{1,1,1} safetyLevelOut;
in Q(0:1)^{14} affordanceIn,
out Q(0:1) safetyLevelOut;
implementation Math {
safetyLevelOut=affordanceIn;
......
package dp.subcomponents;
component Unnormalizer {
ports in Q^{14,1,1} normalizedPredictions,
ports in Q^{14} normalizedPredictions,
out Affordance affordance;
implementation Math {
......@@ -12,59 +12,59 @@ component Unnormalizer {
Q newMin = -0.5;
Q newMax = 0.5;
Q newRange = newMax - newMin;
affordance.angle = (((normalizedPredictions(0,0,0) - oldMin) * newRange) / oldRange) + newMin;
affordance.angle = (((normalizedPredictions(0) - oldMin) * newRange) / oldRange) + newMin;
newMin = -7;
newMax = -2.5;
newRange = newMax - newMin;
affordance.toMarkingL = (((normalizedPredictions(0,0,1) - oldMin) * newRange) / oldRange) + newMin;
affordance.toMarkingL = (((normalizedPredictions(1) - oldMin) * newRange) / oldRange) + newMin;
newMin = -2;
newMax = 3.5;
newRange = newMax - newMin;
affordance.toMarkingM = (((normalizedPredictions(0,0,2) - oldMin) * newRange) / oldRange) + newMin;
affordance.toMarkingM = (((normalizedPredictions(2) - oldMin) * newRange) / oldRange) + newMin;
newMin = 2.5;
newMax = 7;
newRange = newMax - newMin;
affordance.toMarkingR = (((normalizedPredictions(0,0,3) - oldMin) * newRange) / oldRange) + newMin;
affordance.toMarkingR = (((normalizedPredictions(3) - oldMin) * newRange) / oldRange) + newMin;
newMin = 0;
newMax = 75;
newRange = newMax - newMin;
affordance.distL = (((normalizedPredictions(0,0,4) - oldMin) * newRange) / oldRange) + newMin;
affordance.distR = (((normalizedPredictions(0,0,5) - oldMin) * newRange) / oldRange) + newMin;
affordance.distL = (((normalizedPredictions(4) - oldMin) * newRange) / oldRange) + newMin;
affordance.distR = (((normalizedPredictions(5) - oldMin) * newRange) / oldRange) + newMin;
newMin = -9.5;
newMax = -4;
newRange = newMax - newMin;
affordance.toMarkingLL = (((normalizedPredictions(0,0,6) - oldMin) * newRange) / oldRange) + newMin;
affordance.toMarkingLL = (((normalizedPredictions(6) - oldMin) * newRange) / oldRange) + newMin;
newMin = -5.5;
newMax = -0.5;
newRange = newMax - newMin;
affordance.toMarkingML = (((normalizedPredictions(0,0,7) - oldMin) * newRange) / oldRange) + newMin;
affordance.toMarkingML = (((normalizedPredictions(7) - oldMin) * newRange) / oldRange) + newMin;
newMin = 0.5;
newMax = 5.5;
newRange = newMax - newMin;
affordance.toMarkingMR = (((normalizedPredictions(0,0,8) - oldMin) * newRange) / oldRange) + newMin;
affordance.toMarkingMR = (((normalizedPredictions(8) - oldMin) * newRange) / oldRange) + newMin;
newMin = 4;
newMax = 9.5;
newRange = newMax - newMin;
affordance.toMarkingRR = (((normalizedPredictions(0,0,9) - oldMin) * newRange) / oldRange) + newMin;
affordance.toMarkingRR = (((normalizedPredictions(9) - oldMin) * newRange) / oldRange) + newMin;
newMin = 0;
newMax = 75;
newRange = newMax - newMin;
affordance.distLL = (((normalizedPredictions(0,0,10) - oldMin) * newRange) / oldRange) + newMin;
affordance.distMM = (((normalizedPredictions(0,0,11) - oldMin) * newRange) / oldRange) + newMin;
affordance.distRR = (((normalizedPredictions(0,0,12) - oldMin) * newRange) / oldRange) + newMin;
affordance.distLL = (((normalizedPredictions(10) - oldMin) * newRange) / oldRange) + newMin;
affordance.distMM = (((normalizedPredictions(11) - oldMin) * newRange) / oldRange) + newMin;
affordance.distRR = (((normalizedPredictions(12) - oldMin) * newRange) / oldRange) + newMin;
newMin = 0;
newMax = 1;
newRange = newMax - newMin;
affordance.fast = (((normalizedPredictions(0,0,13) - oldMin) * newRange) / oldRange) + newMin;
affordance.fast = (((normalizedPredictions(13) - oldMin) * newRange) / oldRange) + newMin;
}
}
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