Commit 485bd921 authored by Svetlana's avatar Svetlana

2-lane driver controller in implemented Math

parent 15490e1f
......@@ -3,26 +3,38 @@ import dp.subcomponents.*;
component Mastercomponent {
ports
in Z(0:255)^{3, 210, 280} imageIn,
//in Z(0:255)^{3, 210, 280} imageIn,
in Q^{16} groundTruthAffordance,
in Q(0 m/s:0.1 m/s:100 m/s) speedIn,
out Q(-1:1)^{3} commandsOut;
instance Dpnet dpnet;
//instance Dpnet dpnet;
instance DriverController driverController;
instance Denormalizer denorm;
//instance Denormalizer denorm;
//instance KFMastercomponent kfm;
instance SteeringBuffer steeringBuffer;
instance VectorToAffordance vecToAffordance;
instance LocalizationController locController;
connect imageIn -> dpnet.data;
connect dpnet.predictions -> denorm.normalizedPredictions;
//connect imageIn -> dpnet.data;
//connect dpnet.predictions -> denorm.normalizedPredictions;
//connect denorm.affordance -> kfm.affordanceIn;
//connect kfm.affordanceSmoothed -> driverController.affordanceIn;
connect denorm.affordance -> driverController.affordanceIn;
//connect denorm.affordance -> driverController.affordanceIn;
connect groundTruthAffordance -> vecToAffordance.affordanceIndicators;
connect vecToAffordance.affordance -> driverController.affordanceIn;
//connect vecToAffordance.affordance -> locController.affordanceIn;
connect steeringBuffer.outputBuffer -> driverController.steeringRecordIn;
connect speedIn -> driverController.speedIn;
connect steeringBuffer.timerLeftOut -> driverController.timerLeftIn;
connect steeringBuffer.timerRightOut -> driverController.timerRightIn;
connect steeringBuffer.laneChangeOut -> driverController.laneChangeIn;
connect driverController.commandsOut -> commandsOut;
connect driverController.steerCmd -> steeringBuffer.inSteerCmd;
connect driverController.timerLeftOut -> steeringBuffer.timerLeftIn;
connect driverController.timerRightOut -> steeringBuffer.timerRightIn;
connect driverController.laneChangeOut -> steeringBuffer.laneChangeIn;
}
......@@ -5,34 +5,104 @@ component DriverController {
in Q^{5} steeringRecordIn,
in Affordance affordanceIn,
in Q(0 m/s:0.1 m/s:100 m/s) speedIn,
in Z timerLeftIn,
in Z timerRightIn,
in Z laneChangeIn,
out Q(-1:1)^{3} commandsOut, // [accelCmd, steerCmd, brakeCmd]
out Q steerCmd;
out Q steerCmd,
out Z timerLeftOut,
out Z timerRightOut,
out Z laneChangeOut;
implementation Math {
Q roadWidth = 8;
Q centerLine;
Q coeSteer = 1.0;
Z lane_change = laneChangeIn;
Q preML;
Q preMR;
Q desiredSpeed;
Q slowDown = 100; // Acceleration
Q distLLRecord = 30;
Q distRRRecord = 30;
Q preDistL = 60;
Q preDistR = 60;
Q steer_trend;
Z left_clear = 0;
Z right_clear = 0;
Z timer_set = 60;
Z left_timer = timerLeftIn;
Z right_timer = timerRightIn;
// Car following speed model: if a car is less than 20m ahead
if (affordanceIn.distMM < 20)
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 (preDistL < 20 && affordanceIn.distLL < 20)
left_clear = 0;
left_timer = 0;
else
left_timer += 1;
end
if (preDistR < 20 && affordanceIn.distRR < 20)
right_clear = 0;
right_timer = 0;
else
right_timer += 1;
end
preDistL = affordanceIn.distLL;
preDistR = affordanceIn.distRR;
if (left_timer > timer_set)
left_timer = timer_set;
left_clear = 1;
end
if (right_timer > timer_set)
right_timer = timer_set;
right_clear = 1;
end
if (lane_change == 0 && affordanceIn.distMM < 15)
steer_trend = steeringRecordIn(1) + steeringRecordIn(2) + steeringRecordIn(3) + steeringRecordIn(4) + steeringRecordIn(5);
if (slowDown < 0)
if (affordanceIn.toMarkingLL > -8 && left_clear == 1 && steer_trend >= 0)
lane_change = -2;
coeSteer = 6;
right_clear = 0;
right_timer = 0;
left_clear = 0;
left_timer = 0;
timer_set = 30;
elseif (affordanceIn.toMarkingRR < 8 && right_clear == 1 && steer_trend <= 0)
lane_change = 2;
coeSteer = 6;
right_clear = 0;
right_timer = 0;
left_clear = 0;
left_timer = 0;
timer_set = 30;
else
Q vMax = 20;
Q c = 2.772;
Q d = -0.693;
slowDown = vMax*(1-exp(-c/vMax * affordanceIn.distMM - d));
if slowDown < 0
slowDown = 0;
end
end
elseif (lane_change == 0 && affordanceIn.distMM >= 15)
steer_trend = steeringRecordIn(1) + steeringRecordIn(2) + steeringRecordIn(3) + steeringRecordIn(4) + steeringRecordIn(5);
if (affordanceIn.toMarkingLL < -8 && right_clear == 1 && steer_trend <= 0 && steer_trend > -0.2)
lane_change = 2;
coeSteer = 6;
right_clear = 0;
right_timer = 20;
end
end
if (lane_change == 0)
if (-affordanceIn.toMarkingML + affordanceIn.toMarkingMR < 5.5)
coeSteer = 1.5;
centerLine = (affordanceIn.toMarkingML + affordanceIn.toMarkingMR) / 2;
centerLine = (affordanceIn.toMarkingML+affordanceIn.toMarkingMR)/2;
preML = affordanceIn.toMarkingML;
preMR = affordanceIn.toMarkingMR;
if (affordanceIn.toMarkingM < 1)
......@@ -40,31 +110,72 @@ component DriverController {
end
else
if (-preML > preMR)
centerLine = (affordanceIn.toMarkingL + affordanceIn.toMarkingM) / 2;
centerLine = (affordanceIn.toMarkingL+affordanceIn.toMarkingM)/2;
else
centerLine=(affordanceIn.toMarkingR + affordanceIn.toMarkingM) / 2;
centerLine = (affordanceIn.toMarkingR+affordanceIn.toMarkingM)/2;
end
coeSteer = 0.3;
end
elseif (lane_change == -2)
if (-affordanceIn.toMarkingML+affordanceIn.toMarkingMR < 5.5)
centerLine = (affordanceIn.toMarkingLL+affordanceIn.toMarkingML)/2;
if (affordanceIn.toMarkingL > -5 && affordanceIn.toMarkingM < 1.5)
centerLine = (centerLine+(affordanceIn.toMarkingL+affordanceIn.toMarkingM)/2)/2;
end
else
centerLine = (affordanceIn.toMarkingL+affordanceIn.toMarkingM)/2;
coeSteer = 20;
lane_change = -1;
end
elseif (lane_change == -1)
if (affordanceIn.toMarkingL > -5 && affordanceIn.toMarkingM < 1.5)
centerLine = (affordanceIn.toMarkingL+affordanceIn.toMarkingM)/2;
if (-affordanceIn.toMarkingML+affordanceIn.toMarkingMR < 5.5)
centerLine = (centerLine+(affordanceIn.toMarkingML+affordanceIn.toMarkingMR)/2)/2;
end
else
centerLine = (affordanceIn.toMarkingML+affordanceIn.toMarkingMR)/2;
lane_change = 0;
end
elseif (lane_change == 2)
if (-affordanceIn.toMarkingML+affordanceIn.toMarkingMR < 5.5)
centerLine = (affordanceIn.toMarkingRR+affordanceIn.toMarkingMR)/2;
if (affordanceIn.toMarkingR < 5 && affordanceIn.toMarkingM < 1.5)
centerLine = (centerLine + (affordanceIn.toMarkingR+affordanceIn.toMarkingM)/2)/2;
end
else
centerLine = (affordanceIn.toMarkingR+affordanceIn.toMarkingM)/2;
coeSteer = 20;
lane_change = 1;
end
elseif (lane_change == 1)
if (affordanceIn.toMarkingR < 5 && affordanceIn.toMarkingM < 1.5)
centerLine = (affordanceIn.toMarkingR+affordanceIn.toMarkingM < 5.5)/2;
if (-affordanceIn.toMarkingML+affordanceIn.toMarkingMR < 5.5)
centerLine = (centerLine+(affordanceIn.toMarkingML+affordanceIn.toMarkingMR)/2)/2;
else
centerLine = (affordanceIn.toMarkingML+affordanceIn.toMarkingMR)/2;
lane_change = 0;
end
end
end
// steering control, "shared->steerCmd" [-1,1] is the value sent back to TORCS
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);
if (lane_change == 0 && coeSteer > 1 && steerCmd > 0.1)
steerCmd = steerCmd * (2.5*steerCmd+0.75);
end
commandsOut(2) = steerCmd;
desiredSpeed = 20; // 20m/s = 72 kmh
if (affordanceIn.fast != 1)
desiredSpeed = 20 - abs(steeringRecordIn(1) + steeringRecordIn(2) + steeringRecordIn(3) + steeringRecordIn(4) + steeringRecordIn(5)) * 4.5;
desiredSpeed = 20 - abs(steer_trend) * 4.5;
end
if (desiredSpeed < 10)
desiredSpeed = 10;
end
if (desiredSpeed > slowDown)
desiredSpeed = slowDown;
end
......@@ -87,5 +198,9 @@ component DriverController {
commandsOut(3) = brakeCmd;
commandsOut(1) = 0; // accelCmd
end
timerLeftOut = left_timer;
timerRightOut = right_timer;
laneChangeOut = lane_change;
}
}
package dp.subcomponents;
component DriverController {
ports
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, // [accelCmd, steerCmd, brakeCmd]
out Q steerCmd;
implementation Math {
Q roadWidth = 8;
Q centerLine;
Q coeSteer = 1.0;
Q preML;
Q preMR;
Q desiredSpeed;
Q slowDown = 100; // Acceleration
// Car following speed model: if a car is less than 20m ahead
if (affordanceIn.distMM < 20)
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)
slowDown = 0;
end
end
if (-affordanceIn.toMarkingML + affordanceIn.toMarkingMR < 5.5)
coeSteer = 1.5;
centerLine = (affordanceIn.toMarkingML + affordanceIn.toMarkingMR) / 2;
preML = affordanceIn.toMarkingML;
preMR = affordanceIn.toMarkingMR;
if (affordanceIn.toMarkingM < 1)
coeSteer = 0.4;
end
else
if (-preML > preMR)
centerLine = (affordanceIn.toMarkingL + affordanceIn.toMarkingM) / 2;
else
centerLine=(affordanceIn.toMarkingR + affordanceIn.toMarkingM) / 2;
end
coeSteer = 0.3;
end
// steering control, "shared->steerCmd" [-1,1] is the value sent back to TORCS
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;
desiredSpeed = 20; // 20m/s = 72 kmh
if (affordanceIn.fast != 1)
desiredSpeed = 20 - abs(steeringRecordIn(1) + steeringRecordIn(2) + steeringRecordIn(3) + steeringRecordIn(4) + steeringRecordIn(5)) * 4.5;
end
if (desiredSpeed < 10)
desiredSpeed = 10;
end
if (desiredSpeed > slowDown)
desiredSpeed = slowDown;
end
// acceleration and brake
if (desiredSpeed >= speedIn)
Q accelCmd = 0.2*(desiredSpeed - speedIn + 1);
if (accelCmd > 1)
accelCmd = 1;
end
commandsOut(1) = accelCmd;
commandsOut(3) = 0; // brakeCmd
else
Q brakeCmd = 0.1 * (speedIn - desiredSpeed);
if (brakeCmd > 1)
brakeCmd = 1;
end
commandsOut(3) = brakeCmd;
commandsOut(1) = 0; // accelCmd
end
}
}
......@@ -2,10 +2,30 @@ package dp.subcomponents;
component SteeringBuffer {
ports in Q inSteerCmd,
in Z timerLeftIn,
in Z timerRightIn,
in Z laneChangeIn,
out Z timerLeftOut,
out Z timerRightOut,
out Z laneChangeOut,
out Q^{5} outputBuffer;
implementation Math {
static Q^{5} buffer = [0; 0; 0; 0; 0];
static Q^{2} laneChangeBuffer = [0;0];
static Z^{2} timerLeftBuffer = [0; 0];
static Z^{2} timerRightBuffer = [0; 0];
timerLeftBuffer(1) = timerLeftBuffer(2);
timerRightBuffer(1) = timerRightBuffer(2);
timerLeftOut = timerLeftBuffer(1);
timerRightOut = timerRightBuffer(1);
timerLeftBuffer(2) = timerLeftIn;
timerRightBuffer(2) = timerRightIn;
laneChangeBuffer(1) = laneChangeBuffer(2);
laneChangeOut = laneChangeBuffer(1);
laneChangeBuffer(2) = laneChangeIn;
buffer(5) = buffer(4);
buffer(4) = buffer(3);
......
package dp.subcomponents;
import ChangeRange;
component VectorToAffordance {
ports in Q^{16} affordanceIndicators,
out Affordance affordance;
implementation Math {
affordance.fast = affordanceIndicators(1);
affordance.distL = affordanceIndicators(2);
affordance.distR = affordanceIndicators(3);
affordance.toMarkingL = affordanceIndicators(4);
affordance.toMarkingM = affordanceIndicators(5);
affordance.toMarkingR = affordanceIndicators(6);
affordance.distLL = affordanceIndicators(7);
affordance.distMM = affordanceIndicators(8);
affordance.distRR = affordanceIndicators(9);
affordance.toMarkingLL = affordanceIndicators(10);
affordance.toMarkingML = affordanceIndicators(11);
affordance.toMarkingMR = affordanceIndicators(12);
affordance.toMarkingRR = affordanceIndicators(13);
affordance.angle = affordanceIndicators(15);
}
}
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