Commit d5763822 authored by Svetlana's avatar Svetlana
Browse files

refactorings in DriverController

parent e6473717
......@@ -7,40 +7,42 @@ component DriverController {
in Q(0 m/s:0.1 m/s:100 m/s) speedIn,
in Z timerLeftIn,
in Z timerRightIn,
in Z laneChangeIn,
in LaneChange laneChangeIn,
in Z lanesCountIn,
out Q(-1:1)^{3} commandsOut, // [accelCmd, steerCmd, brakeCmd]
out Q steerCmd,
out Z timerLeftOut,
out Z timerRightOut,
out Z laneChangeOut;
out LaneChange 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 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;
B leftClear = false;
B rightClear = false;
Z timerSet = 60;
Z leftTimer = timerLeftIn;
Z rightTimer = timerRightIn;
Q steerTrend = steeringRecordIn(1) + steeringRecordIn(2) + steeringRecordIn(3) + steeringRecordIn(4) + steeringRecordIn(5); // am I turning or not;
//parameters of car following modlel
Q vMax = 20;
Q vC = 2.772;
Q vD = -0.693;
Q carFollowingSlowDown = vMax * (1-exp(-(vC/vMax) * affordanceIn.distMM - vD)); // optimal velocity for car-following model
// CASE 1: one lane
if (lanesCountIn == 1)
if (affordanceIn.distMM < 20)
Q vMax = 20;
Q vC = 2.772;
Q vD = -0.693;
if (affordanceIn.distMM < 20) // car in front of me, slow down
// optimal velocity car-following model
slowDown = vMax * (1-exp(-(vC/vMax) * affordanceIn.distMM - vD));
slowDown = carFollowingSlowDown;
if (slowDown < 0)
slowDown = 0;
end
......@@ -71,275 +73,108 @@ component DriverController {
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
// CASE 2: two or three lanes
elseif (lanesCountIn == 2 || lanesCountIn == 3)
if (preDistL < 20 && affordanceIn.distLL < 20) // left lane is occupied or not
leftClear = false;
leftTimer = 0;
else
Q brakeCmd = 0.1 * (speedIn - desiredSpeed);
if (brakeCmd > 1)
brakeCmd = 1;
end
commandsOut(3) = brakeCmd;
commandsOut(1) = 0; // accelCmd
end
elseif (lanesCountIn == 2)
if (preDistL < 20 && affordanceIn.distLL < 20)
left_clear = 0;
left_timer = 0;
else
left_timer += 1;
leftTimer += 1;
end
if (preDistR < 20 && affordanceIn.distRR < 20)
right_clear = 0;
right_timer = 0;
if (preDistR < 20 && affordanceIn.distRR < 20) // right lane is occupied or not
rightClear = false;
rightTimer = 0;
else
right_timer += 1;
rightTimer += 1;
end
preDistL = affordanceIn.distLL;
preDistR = affordanceIn.distRR;
if (left_timer > timer_set)
left_timer = timer_set;
left_clear = 1;
if (leftTimer > timerSet) // left lane is clear
leftTimer = timerSet;
leftClear = true;
end
if (right_timer > timer_set)
right_timer = timer_set;
right_clear = 1;
if (rightTimer > timerSet) // right lane is clear
rightTimer = timerSet;
rightClear = true;
end
if (lane_change == 0 && affordanceIn.distMM < 15)
steer_trend = steeringRecordIn(1) + steeringRecordIn(2) + steeringRecordIn(3) + steeringRecordIn(4) + steeringRecordIn(5);
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;
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;
if (lanesCountIn == 2)
if (laneChangeIn == NO_CHANGE && affordanceIn.distMM < 15) // if current lane is occupied
if (affordanceIn.toMarkingLL > -8 && leftClear == true && steerTrend >= 0) // move to left lane
laneChangeIn = TO_LEFT;
coeSteer = 6;
rightClear = false;
rightTimer = 0;
leftClear = false;
leftTimer = 0;
timerSet = 30;
elseif (affordanceIn.toMarkingRR < 8 && rightClear == true && steerTrend <= 0) // move to right lane
laneChangeIn = TO_RIGHT;
coeSteer = 6;
rightClear = false;
rightTimer = 0;
leftClear = false;
leftTimer = 0;
timerSet = 30;
else // follow car in front
slowDown = carFollowingSlowDown;
if slowDown < 0
slowDown = 0;
end
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;
elseif (laneChangeIn == NO_CHANGE && affordanceIn.distMM >= 15) // prefer to stay in the right lane
if (affordanceIn.toMarkingLL < -8 && rightClear == true && steerTrend <= 0 && steerTrend > -0.2) // in right lane, so move to central lane
laneChangeIn = TO_RIGHT;
coeSteer = 6;
rightClear = false;
rightTimer = 20;
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;
elseif (lanesCountIn == 3)
if (laneChangeIn == NO_CHANGE && affordanceIn.distMM < 15) // current lane is occupied
if (affordanceIn.toMarkingLL > -8 && leftClear == true && steerTrend >= 0 && steerTrend < 0.2) // move to left lane
laneChangeIn = TO_LEFT;
coeSteer = 6;
rightClear = false;
rightTimer = 0;
leftClear = false;
leftTimer = 30;
timerSet = 60;
elseif (affordanceIn.toMarkingRR < 8 && rightClear == true && steerTrend <= 0 && steerTrend > -0.2) // move to right lane
laneChangeIn = TO_RIGHT;
coeSteer = 6;
rightClear = false;
rightTimer = 30;
leftClear = false;
leftTimer = 0;
timerSet = 60;
else // car following
slowDown = carFollowingSlowDown;
if slowDown < 0
slowDown = 0;
end
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)/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;
elseif (laneChangeIn == NO_CHANGE && affordanceIn.distMM >= 15) // prefer to stay in the central line
if (affordanceIn.toMarkingRR > 8 && leftClear == true && steerTrend >= 0 && steerTrend < 0.2) // in right lane, so move to central lane
laneChangeIn = TO_LEFT;
coeSteer = 6;
leftClear = false;
leftTimer = 30;
elseif (affordanceIn.toMarkingLL < -8 && rightClear == true && steerTrend <= 0 && steerTrend > -0.2) // in left lane, so move to central lane
laneChangeIn = TO_RIGHT;
coeSteer = 6;
rightClear = false;
rightTimer = 30;
end
end
end
steerCmd = (affordanceIn.angle - centerLine/roadWidth) / 0.541052 / coeSteer;
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(steer_trend) * 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
elseif (lanesCountIn == 3)
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 (affordanceIn.toMarkingLL > -8 && left_clear == 1 && steer_trend >= 0 && steer_trend < 0.2)
lane_change = -2;
coeSteer = 6;
right_clear = 0;
right_timer = 0;
left_clear = 0;
left_timer = 30;
timer_set = 60;
elseif (affordanceIn.toMarkingRR < 8 && right_clear == 1 && steer_trend <= 0 && steer_trend > -0.2)
lane_change = 2;
coeSteer = 6;
right_clear = 0;
right_timer = 30;
left_clear = 0;
left_timer = 0;
timer_set = 60;
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.toMarkingRR > 8 && left_clear == 1 && steer_trend >= 0 && steer_trend < 0.2)
lane_change = -2;
coeSteer = 6;
left_clear = 0;
left_timer = 30;
elseif (affordanceIn.toMarkingLL < -8 && right_clear == 1 && steer_trend <= 0 && steer_trend > -0.2)
lane_change = 2;
coeSteer = 6;
right_clear = 0;
right_timer = 30;
end
end
if (lane_change == 0)
// go through all possible lane change maneuvers
if (laneChangeIn == NO_CHANGE) //car following
if (-affordanceIn.toMarkingML + affordanceIn.toMarkingMR < 5.5)
coeSteer = 1.5;
centerLine = (affordanceIn.toMarkingML+affordanceIn.toMarkingMR)/2;
......@@ -356,7 +191,7 @@ component DriverController {
end
coeSteer = 0.3;
end
elseif (lane_change == -2)
elseif (laneChangeIn == TO_LEFT)
if (-affordanceIn.toMarkingML+affordanceIn.toMarkingMR < 5.5)
centerLine = (affordanceIn.toMarkingLL+affordanceIn.toMarkingML)/2;
if (affordanceIn.toMarkingL > -5 && affordanceIn.toMarkingM < 1.5)
......@@ -365,9 +200,9 @@ component DriverController {
else
centerLine = (affordanceIn.toMarkingL+affordanceIn.toMarkingM)/2;
coeSteer = 20;
lane_change = -1;
laneChangeIn = IN_LEFT;
end
elseif (lane_change == -1)
elseif (laneChangeIn == IN_LEFT)
if (affordanceIn.toMarkingL > -5 && affordanceIn.toMarkingM < 1.5)
centerLine = (affordanceIn.toMarkingL+affordanceIn.toMarkingM)/2;
if (-affordanceIn.toMarkingML+affordanceIn.toMarkingMR < 5.5)
......@@ -375,9 +210,9 @@ component DriverController {
end
else
centerLine = (affordanceIn.toMarkingML+affordanceIn.toMarkingMR)/2;
lane_change = 0;
laneChangeIn = NO_CHANGE;
end
elseif (lane_change == 2)
elseif (laneChangeIn == TO_RIGHT)
if (-affordanceIn.toMarkingML+affordanceIn.toMarkingMR < 5.5)
centerLine = (affordanceIn.toMarkingRR+affordanceIn.toMarkingMR)/2;
if (affordanceIn.toMarkingR < 5 && affordanceIn.toMarkingM < 1.5)
......@@ -386,66 +221,67 @@ component DriverController {
else
centerLine = (affordanceIn.toMarkingR+affordanceIn.toMarkingM)/2;
coeSteer = 20;
lane_change = 1;
laneChangeIn = IN_RIGHT;
end
elseif (lane_change == 1)
elseif (laneChangeIn == IN_RIGHT)
if (affordanceIn.toMarkingR < 5 && affordanceIn.toMarkingM < 1.5)
centerLine = (affordanceIn.toMarkingR+affordanceIn.toMarkingM)/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;
laneChangeIn = NO_CHANGE;
end
end
end
steerCmd = (affordanceIn.angle - centerLine/roadWidth) / 0.541052 / coeSteer;
if (lane_change == 0 && coeSteer > 1 && steerCmd > 0.1)
if (laneChangeIn == NO_CHANGE && 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(steer_trend) * 4.5;
end
if (desiredSpeed < 10)
desiredSpeed = 10;
end
if (desiredSpeed > slowDown)
desiredSpeed = slowDown;
end
// DEFAULT CASE - no information on lanes number
else
commandsOut(2) = 0;
end
// compute desired speed
Q desiredSpeed = 20; // 20m/s = 72 kmh
if (affordanceIn.fast != 1)
desiredSpeed = 20 - abs(steerTrend) * 4.5;
end
// acceleration and brake
if (desiredSpeed >= speedIn)
Q accelCmd = 0.2*(desiredSpeed - speedIn + 1);
if (accelCmd > 1)
accelCmd = 1;
end
if (desiredSpeed < 10)
desiredSpeed = 10;
end
commandsOut(1) = accelCmd;
commandsOut(3) = 0; // brakeCmd
else
Q brakeCmd = 0.1 * (speedIn - desiredSpeed);
if (brakeCmd > 1)
brakeCmd = 1;
end
if (desiredSpeed > slowDown)
desiredSpeed = slowDown;
end
commandsOut(3) = brakeCmd;
commandsOut(1) = 0; // accelCmd
// compute 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
commandsOut(1) = 0;
commandsOut(2) = 0;
commandsOut(3) = 0;
Q brakeCmd = 0.1 * (speedIn - desiredSpeed);
if (brakeCmd > 1)
brakeCmd = 1;
end
commandsOut(3) = brakeCmd;
commandsOut(1) = 0; // accelCmd
end
timerLeftOut = left_timer;
timerRightOut = right_timer;
laneChangeOut = lane_change;
timerLeftOut = leftTimer;
timerRightOut = rightTimer;
laneChangeOut = laneChangeIn;
}
}
package dp.subcomponents;
component SafetyNet {
ports
in Q(0:255)^{3, 210, 280} data,
out Q(0:1) safetyLevel;
implementation Math {
safetyLevel=1.0;
}
}
......@@ -4,15 +4,15 @@ component SteeringBuffer {
ports in Q inSteerCmd,
in Z timerLeftIn,