Commit 7b75a741 authored by Svetlana's avatar Svetlana

Corrected SteeringRecord model and updated DriverController

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