Aufgrund eines Security Updates wird GitLab heute zwischen 14:00 und 14:30 Uhr kurzzeitig nicht zur Verfügung stehen. / Due to a security update GitLab will be temporarily unavailable between 2 and 2:30 today.

Commit 425e47ab authored by Svetlana's avatar Svetlana

Corrected SteeringRecord model and updated DriverController

parent 474a5f6c
...@@ -11,7 +11,7 @@ component Mastercomponent { ...@@ -11,7 +11,7 @@ component Mastercomponent {
instance Drivercontroller driverController; instance Drivercontroller driverController;
instance Unnormalizer unnorm; instance Unnormalizer unnorm;
//instance KFMastercomponent kfm; //instance KFMastercomponent kfm;
instance AngleBuffer angleBuffer; instance SteeringBuffer steeringBuffer;
connect imageIn -> dpnet.data; connect imageIn -> dpnet.data;
connect dpnet.predictions -> unnorm.normalizedPredictions; connect dpnet.predictions -> unnorm.normalizedPredictions;
...@@ -19,9 +19,9 @@ component Mastercomponent { ...@@ -19,9 +19,9 @@ component Mastercomponent {
//connect un.affordance -> kfm.predictions; //connect un.affordance -> kfm.predictions;
//connect kfm.predictionsSmoothed -> driverController.affordanceIn; //connect kfm.predictionsSmoothed -> driverController.affordanceIn;
connect unnorm.affordance -> angleBuffer.inputAffordance;
connect unnorm.affordance -> driverController.affordanceIn; connect unnorm.affordance -> driverController.affordanceIn;
connect angleBuffer.outputBuffer -> driverController.anglesIn; connect steeringBuffer.outputBuffer -> driverController.steeringRecordIn;
connect driverController.steerCmd -> steeringBuffer.inSteerCmd;
connect speedIn -> driverController.speedIn; connect speedIn -> driverController.speedIn;
connect driverController.commandsOut -> commandsOut; connect driverController.commandsOut -> commandsOut;
......
package dp.subcomponents;
struct Commands {
Q(0:1) acceleration;
Q(0:1) brake;
Q(-1:1) steering;
}
...@@ -2,21 +2,26 @@ package dp.subcomponents; ...@@ -2,21 +2,26 @@ package dp.subcomponents;
component Drivercontroller { component Drivercontroller {
ports ports
in Q^{5} anglesIn, in Q^{5} steeringRecordIn,
in Affordance affordanceIn, in Affordance affordanceIn,
in Q(0 m/s:0.1 m/s:100 m/s) speedIn, in Q(0 m/s:0.1 m/s:100 m/s) speedIn,
out Q(-1:1)^{3} commandsOut; out Q(-1:1)^{3} commandsOut, // [accelCmd, steerCmd, brakeCmd]
out Q steerCmd;
implementation Math { implementation Math {
Q roadWidth = 8; Q roadWidth = 8;
Q centerLine;
// Acceleration Q coeSteer = 1.0;
Q slowDown = 100; Q preML;
Q preMR;
Q desiredSpeed;
Q slowDown = 100; // Acceleration
if (affordanceIn.distMM < 15) if (affordanceIn.distMM < 15)
Q vMax = 20; Q vMax = 20;
Q vC = 2.772; Q vC = 2.772;
Q vD = -0.693; Q vD = -0.693;
// optimal velocity car-following model
slowDown = vMax * (1-exp(-(vC/vMax) * affordanceIn.distMM - vD)); slowDown = vMax * (1-exp(-(vC/vMax) * affordanceIn.distMM - vD));
if (slowDown < 0) if (slowDown < 0)
...@@ -24,10 +29,6 @@ component Drivercontroller { ...@@ -24,10 +29,6 @@ component Drivercontroller {
end end
end end
Q centerLine;
Q coeSteer;
Q preML;
Q preMR;
if (-affordanceIn.toMarkingML + affordanceIn.toMarkingMR < 5.5) if (-affordanceIn.toMarkingML + affordanceIn.toMarkingMR < 5.5)
coeSteer = 1.5; coeSteer = 1.5;
centerLine = (affordanceIn.toMarkingML + affordanceIn.toMarkingMR) / 2; centerLine = (affordanceIn.toMarkingML + affordanceIn.toMarkingMR) / 2;
...@@ -47,16 +48,17 @@ component Drivercontroller { ...@@ -47,16 +48,17 @@ component Drivercontroller {
Q steerCmd = 0; Q steerCmd = 0;
// steering control, "shared->steerCmd" [-1,1] is the value sent back to TORCS // steering control, "shared->steerCmd" [-1,1] is the value sent back to TORCS
steerCmd = (affordanceIn.angle - centerLine/roadWidth) / 0.541052/coeSteer; steerCmd = (affordanceIn.angle - centerLine/roadWidth) / 0.541052 / coeSteer;
if (coeSteer > 1 && steerCmd > 0.1) // reshape the steering control curve if (coeSteer > 1 && steerCmd > 0.1) // reshape the steering control curve
steerCmd = steerCmd * (2.5 * steerCmd + 0.75); steerCmd = steerCmd * (2.5 * steerCmd + 0.75);
end end
commandsOut(2) = steerCmd; commandsOut(2) = steerCmd;
Q desiredSpeed = 20; desiredSpeed = 20;
//if (affordanceIn.fast != 1) if (affordanceIn.fast != 1)
// desiredSpeed = 20 - abs(anglesIn(0) + anglesIn(1) + anglesIn(2) + anglesIn(3) + anglesIn(4)) * 4.5; desiredSpeed = 20 - abs(steeringRecordIn(0) + steeringRecordIn(1) + steeringRecordIn(2) + steeringRecordIn(3) + steeringRecordIn(4)) * 4.5;
//end end
if (desiredSpeed < 10) if (desiredSpeed < 10)
desiredSpeed = 10; desiredSpeed = 10;
...@@ -75,15 +77,15 @@ component Drivercontroller { ...@@ -75,15 +77,15 @@ component Drivercontroller {
end end
commandsOut(1) = accelCmd; commandsOut(1) = accelCmd;
commandsOut(3) = 0; commandsOut(3) = 0; // brakeCmd
else else
Q brakeCmd = 0.1 * (speedIn - desiredSpeed); Q brakeCmd = 0.1 * (speedIn - desiredSpeed);
if (brakeCmd > 1) if (brakeCmd > 1)
commandsOut(3) = 1; brakeCmd = 1;
end end
commandsOut(3) = brakeCmd; commandsOut(3) = brakeCmd;
commandsOut(1) = 0; commandsOut(1) = 0; // accelCmd
end end
} }
} }
\ No newline at end of file
package dp.subcomponents; package dp.subcomponents;
component AngleBuffer { component SteeringBuffer {
ports in Affordance inputAffordance, ports in Q inSteerCmd,
out Q^{5} outputBuffer; out Q^{5} outputBuffer;
implementation Math { implementation Math {
...@@ -11,7 +11,7 @@ component AngleBuffer { ...@@ -11,7 +11,7 @@ component AngleBuffer {
buffer(4) = buffer(3); buffer(4) = buffer(3);
buffer(3) = buffer(2); buffer(3) = buffer(2);
buffer(2) = buffer(1); buffer(2) = buffer(1);
buffer(1) = inputAffordance.angle; buffer(1) = inSteerCmd;
outputBuffer(1) = buffer(1); outputBuffer(1) = buffer(1);
outputBuffer(2) = buffer(2); outputBuffer(2) = buffer(2);
......
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