Commit 073225e3 authored by Alexander David Hellwig's avatar Alexander David Hellwig
Browse files

Modeled planner

parent 5fc8ae41
......@@ -17,7 +17,7 @@ component System{
connect isActiveIn -> intersectionController.isActive;
//Planner(s)
ports in Q^{4,10} lanelatIn[2],
ports in Q^{4,10} laneletIn[2],
in Position positionIn[2],
in Q maxVelocityIn[2],
in Q maxAccelerationIn[2],
......@@ -26,7 +26,7 @@ component System{
in B resetVelocityIn[2],
out Q^{3,20} desiredTrajectoryOut[2];
connect lanelatIn[:] -> planner[:].lanelat;
connect laneletIn[:] -> planner[:].lanelet;
connect positionIn[:] -> planner[:].position;
connect maxVelocityIn[:] -> planner[:].maxVelocity;
connect maxAccelerationIn[:] -> planner[:].maxAcceleration;
......
......@@ -3,7 +3,7 @@ import ba.util.Position;
//component Planner<N1 k = 10,N1 m = 20>{
component Planner{
ports in Q^{4,10} lanelat,
ports in Q^{4,10} lanelet,
in Position position,
in Q maxVelocity,
in Q maxAcceleration,
......@@ -14,11 +14,11 @@ component Planner{
ports out Q^{3,20} desiredTrajectory;
instance LaneletToWay<10,20> lanelatToWay;
instance LaneletToWay<10,20> laneletToWay;
instance VelocityController velocityController;
instance TrajectoryComputer<10,20> trajectoryComputer;
connect lanelat -> lanelatToWay.laneletIn;
connect lanelet -> laneletToWay.laneletIn;
connect maxVelocity -> velocityController.maxVelIn;
connect maxAcceleration -> velocityController.maxAccelIn;
......@@ -26,7 +26,7 @@ component Planner{
connect slowDown -> velocityController.slowDownIn;
connect resetVelocity -> velocityController.resetVelIn;
connect lanelatToWay.wayOut -> trajectoryComputer.wayIn;
connect laneletToWay.wayOut -> trajectoryComputer.wayIn;
connect velocityController.curVelOut -> trajectoryComputer.curVelIn;
......
......@@ -22,6 +22,9 @@ component PosToIndex<N1 k = 10>{
closestDist = curDist;
closestInd = i;
elseif(secondClosestInd == -1) || (curDist <= secondClosestDist)
secondClosestDist = curDist;
secondClosestInd = i;
end
end
......
......@@ -15,8 +15,9 @@ component VelocityController{
if resetVelIn
lastVel = maxVelIn;
resVel = maxVelIn;
else
Q deltaVel = 0.5 * maxAccelIn * deltaTimeIn * deltaTimeIn;
Q deltaVel = maxAccelIn * deltaTimeIn;
if slowDownIn
resVel = lastVel - deltaVel;
else
......@@ -25,11 +26,11 @@ component VelocityController{
end
if resVel < 0
curVelOut = 0;
resVel = 0;
elseif resVel > maxVelIn
curVelOut = maxVelIn;
resVel = maxVelIn;
end
curVelOut = resVel;
lastVel = curVelOut;
}
}
\ No newline at end of file
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