/// <summary> /// Figure out standard control to follow /// </summary> /// <param name="lane"></param> /// <param name="state"></param> /// <returns></returns> public ForwardVehicleTrackingControl GetControl(ArbiterLane lane, VehicleState state) { // check exists a vehicle to track if (this.CurrentVehicle != null) { // get the maximum velocity of the segment we're closest to double segV = lane.CurrentMaximumSpeed(state.Front); double segMaxV = segV; // minimum distance double xAbsMin = TahoeParams.VL * 2.0; // retrieve the tracked vehicle's scalar absolute speed double vTarget = this.CurrentVehicle.StateMonitor.Observed.speedValid ? this.CurrentVehicle.Speed : lane.Way.Segment.SpeedLimits.MaximumSpeed; // check if vehicle is moving away from us VehicleDirectionAlongPath vdap = CurrentVehicle.VehicleDirectionAlong(lane); // get the good distance behind the target, is xAbsMin if vehicle velocity is small enough double xGood = xAbsMin + (1.5 * (2 * TahoeParams.VL) * vTarget); // get our current separation double xSeparation = this.currentDistance; // determine the envelope to reason about the vehicle, that is, to slow from vMax to vehicle speed double xEnvelope = (Math.Pow(vTarget, 2.0) - Math.Pow(segMaxV, 2.0)) / (2.0 * -0.5); // the distance to the good double xDistanceToGood; // determine the velocity to follow the vehicle ahead double vFollowing; // check if we are basically stopped in the right place behind another vehicle if (vTarget < 1 && Math.Abs(xSeparation - xGood) < 1) { // stop vFollowing = 0; // good distance xDistanceToGood = 0; } // check if we are within the minimum distance else if (xSeparation <= xGood) { // stop vFollowing = 0; // good distance xDistanceToGood = 0; } // determine if our separation is less than the good distance but not inside minimum else if (xAbsMin < xSeparation && xSeparation < xGood) { // get the distance we are from xGood to xMin double xAlong = xSeparation - xAbsMin; // get the total distance from xGood to xMin double xGoodToMin = xGood - xAbsMin; // slow to 0 by min vFollowing = (((xGoodToMin - xAlong) / xGoodToMin) * (-vTarget)) + vTarget; // good distance xDistanceToGood = 0; } // otherwise xSeparation > xEnvelope else { // good distance xDistanceToGood = xSeparation - xGood; // get differences in max and target velocities vFollowing = SpeedTools.GenerateSpeed(xDistanceToGood, vTarget, segMaxV); } // return return(new ForwardVehicleTrackingControl(true, false, vFollowing, xSeparation, xDistanceToGood, vTarget, xAbsMin, xGood, true)); } else { // return return(new ForwardVehicleTrackingControl(false, false, Double.MaxValue, Double.MaxValue, Double.MaxValue, Double.MaxValue, 0.0, Double.MaxValue, false)); } }
/// <summary> /// Figure out standard control to follow /// </summary> /// <param name="lane"></param> /// <param name="state"></param> /// <returns></returns> public ForwardVehicleTrackingControl GetControl(IFQMPlanable lane, VehicleState state, List <ArbiterWaypoint> ignorable) { // check exists a vehicle to track if (this.CurrentVehicle != null) { // get the maximum velocity of the segment we're closest to double segV = lane.CurrentMaximumSpeed(state.Front); double segMaxV = segV; // flag if the forward vehicles is in a stop waypoint type safety zone bool safetyZone = false; ArbiterWaypoint nextStop = lane.GetNext(state.Front, WaypointType.Stop, ignorable); double distance = nextStop != null?lane.DistanceBetween(state.Front, nextStop.Position) : Double.MaxValue; // check if no stop exists or distance > 30 if (nextStop == null || distance > 30 || distance < 0) { // there is no next stop or the forward vehcile is not in a safety zone safetyZone = false; } // otherwise the tracked vehicle is in a safety zone else { // set the forward vehicle as being in a safety zone safetyZone = true; } // minimum distance double xAbsMin; // if we're in a safety zone if (safetyZone) { // set minimum to 1 times the tahoe's vehicle length xAbsMin = TahoeParams.VL * 1.5; } // otherwise we're not in a safety zone else { // set minimum to 2 times the tahoe's vehicle length xAbsMin = TahoeParams.VL * 2.0; } // retrieve the tracked vehicle's scalar absolute speed double vTarget = CurrentVehicle.StateMonitor.Observed.isStopped ? 0.0 : this.CurrentVehicle.Speed; // check if vehicle is moving away from us VehicleDirectionAlongPath vdap = CurrentVehicle.VehicleDirectionAlong(lane); // if moving perpendic, set speed to 0.0 if (vdap == VehicleDirectionAlongPath.Perpendicular) { vTarget = 0.0; } #region Forward // can use normal tracker if vehicle not oncoming if (true) //vdap != VehicleDirectionAlongPath.Reverse) { // get the good distance behind the target, is xAbsMin if vehicle velocity is small enough double xGood = xAbsMin + (1.5 * (TahoeParams.VL / 4.4704) * vTarget); // get our current separation double xSeparation = this.currentDistance; // determine the envelope to reason about the vehicle, that is, to slow from vMax to vehicle speed double xEnvelope = (Math.Pow(vTarget, 2.0) - Math.Pow(segMaxV, 2.0)) / (2.0 * -0.5); // the distance to the good double xDistanceToGood; // determine the velocity to follow the vehicle ahead double vFollowing; // check if we are basically stopped in the right place behind another vehicle if (vTarget < 1 && Math.Abs(xSeparation - xGood) < 1) { // stop vFollowing = 0; // good distance xDistanceToGood = 0; } // check if we are within the minimum distance else if (xSeparation <= xAbsMin) { // stop vFollowing = 0; // good distance xDistanceToGood = 0; } // determine if our separation is less than the good distance but not inside minimum else if (xAbsMin < xSeparation && xSeparation < xGood) { // get the distance we are from xMin double xAlong = xSeparation - xAbsMin; // get the total distance from xGood to xMin double xGoodToMin = xGood - xAbsMin; // slow to 0 by min vFollowing = (((xGoodToMin - xAlong) / xGoodToMin) * (-vTarget)) + vTarget; // good distance xDistanceToGood = 0; } // our separation is greater than the good distance else { // good distance xDistanceToGood = xSeparation - xGood; // get differences in max and target velocities vFollowing = SpeedTools.GenerateSpeed(xDistanceToGood, vTarget, segMaxV); } // return return(new ForwardVehicleTrackingControl(true, safetyZone, vFollowing, xSeparation, xDistanceToGood, vTarget, xAbsMin, xGood, false)); } #endregion #region Oncoming /* * else * { * // determine the distance for the other vehicle to stop * double xTargetEnvelope = -Math.Pow(vTarget, 2.0) / (2.0 * -0.5); * * // determine the distance for our vehicle to stop * double xOurEnvelope = -Math.Pow(CoreCommon.Communications.GetVehicleSpeed().Value, 2.0) / (2.0 * -0.5); * * // get the good distance behind the target, is xAbsMin if vehicle velocity is small enough * double xGood = xAbsMin + (1.5 * (TahoeParams.VL / 4.4704) * vTarget) + xOurEnvelope + xTargetEnvelope; * * // get our current separation * double xSeparation = this.currentDistance; * * // determine the envelope for us to slow to 0 by the good distance * double xEnvelope = -Math.Pow(segMaxV, 2.0) / (2.0 * -0.5); * * // the distance to the good * double xDistanceToGood; * * // determine the velocity to follow the vehicle ahead * double vFollowing; * * // check if we are basically stopped in the right place behind another vehicle * if (vTarget < 1 && Math.Abs(xSeparation - xGood) < 1) * { * // stop * vFollowing = 0; * * // good distance * xDistanceToGood = 0; * } * // check if we are within the minimum distance * else if (xSeparation <= xGood) * { * // stop * vFollowing = 0; * * // good distance * xDistanceToGood = 0; * } * // our separation is greater than the good distance but within the envelope * else if (xGood <= xSeparation && xSeparation <= xEnvelope + xGood) * { * // get differences in max and target velocities * double vDifference = segMaxV; * * // get the distance we are along the speed envolope from xGood * double xAlong = xEnvelope - (xSeparation - xGood); * * // slow to vTarget by good * vFollowing = (((xEnvelope - xAlong) / xEnvelope) * (vDifference)); * * // good distance * xDistanceToGood = xSeparation - xGood; * } * // otherwise xSeparation > xEnvelope * else * { * // can go max speed * vFollowing = segMaxV; * * // good distance * xDistanceToGood = xSeparation - xGood; * } * * // return * return new ForwardVehicleTrackingControl(true, safetyZone, vFollowing, xSeparation, * xDistanceToGood, vTarget, xAbsMin, xGood, true); * } * */ #endregion } else { // return return(new ForwardVehicleTrackingControl(false, false, Double.MaxValue, Double.MaxValue, Double.MaxValue, Double.MaxValue, 0.0, Double.MaxValue, false)); } }