/// <summary>
/// Predicts forward by the delay time of the tahoe
/// </summary>
/// <param name="absPose"></param>
/// <param name="vehicleState"></param>
public void ForwardPredict(double planningTime, out AbsolutePose absPose, out OperationalVehicleState vehicleState)
{
// get the current state
OperationalVehicleState currentState = Services.StateProvider.GetVehicleState();
// assume the vehicle will hold it current curvature
double curvature = TahoeParams.CalculateCurvature(currentState.steeringAngle, currentState.speed);
// simple dynamics
// xdot = v*cos(theta)
// ydot = v*sin(theta)
// thetadot = v*c
// do euler integration
double dt = 0.01;
double T = TahoeParams.actuation_delay + planningTime;
int n = (int)Math.Round(T / dt);
double v = currentState.speed;
double x = 0, y = 0, heading = 0;
for (int i = 0; i < n; i++)
{
x += v * Math.Cos(heading) * dt;
y += v * Math.Sin(heading) * dt;
heading += v * curvature * dt;
}
absPose = new AbsolutePose(new Coordinates(x, y), heading, 0);
vehicleState = currentState;
}
public OperationalSpeedCommand GetSpeedCommand()
{
OperationalVehicleState vs = Services.StateProvider.GetVehicleState();
OperationalSpeedCommand cmd = new OperationalSpeedCommand();
cmd.brakePressure = TahoeParams.brake_hold;
cmd.engineTorque = 0;
if (result != CompletionResult.Failed)
{
if (phase == Phase.Braking)
{
// chek if we can transition to shifting
if (Math.Abs(vs.speed) < 0.05 && vs.brakePressure >= TahoeParams.brake_hold - 1)
{
phase = Phase.Shifting;
}
}
if (phase == Phase.Shifting)
{
cmd.transGear = gear;
if (vs.transGear == gear)
{
result = CompletionResult.Completed;
}
}
}
return(cmd);
}
public void Process(object param)
{
if (cancelled)
{
return;
}
DateTime start = HighResDateTime.Now;
OperationalVehicleState vs = Services.StateProvider.GetVehicleState();
LinePath curBasePath = basePath;
LinePath curLeftBound = leftBound;
LinePath curRightBound = rightBound;
// transform the base path to the current iteration
CarTimestamp curTimestamp = Services.RelativePose.CurrentTimestamp;
if (pathTime != curTimestamp)
{
RelativeTransform relTransform = Services.RelativePose.GetTransform(pathTime, curTimestamp);
curBasePath = curBasePath.Transform(relTransform);
curLeftBound = curLeftBound.Transform(relTransform);
curRightBound = curRightBound.Transform(relTransform);
}
// get the distance between the zero point and the start point
double distToStart = Coordinates.Zero.DistanceTo(curBasePath.GetPoint(curBasePath.StartPoint));
// get the sub-path between 5 and 25 meters ahead
double startDist = TahoeParams.FL;
LinePath.PointOnPath startPoint = curBasePath.AdvancePoint(curBasePath.ZeroPoint, ref startDist);
double endDist = 30;
LinePath.PointOnPath endPoint = curBasePath.AdvancePoint(startPoint, ref endDist);
if (startDist > 0)
{
// we've reached the end
Services.BehaviorManager.Execute(new HoldBrakeBehavior(), null, false);
return;
}
// get the sub-path
LinePath subPath = curBasePath.SubPath(startPoint, endPoint);
// add (0,0) as the starting point
subPath.Insert(0, new Coordinates(0, 0));
// do the same for the left, right bound
startDist = TahoeParams.FL;
endDist = 40;
startPoint = curLeftBound.AdvancePoint(curLeftBound.ZeroPoint, startDist);
endPoint = curLeftBound.AdvancePoint(startPoint, endDist);
curLeftBound = curLeftBound.SubPath(startPoint, endPoint);
startPoint = curRightBound.AdvancePoint(curRightBound.ZeroPoint, startDist);
endPoint = curRightBound.AdvancePoint(startPoint, endDist);
curRightBound = curRightBound.SubPath(startPoint, endPoint);
if (cancelled)
{
return;
}
Services.UIService.PushRelativePath(subPath, curTimestamp, "subpath");
Services.UIService.PushRelativePath(curLeftBound, curTimestamp, "left bound");
Services.UIService.PushRelativePath(curRightBound, curTimestamp, "right bound");
// run a path smoothing iteration
lock (this) {
planner = new PathPlanner();
}
////////////////////////////////////////////////////////////////////////////////////////////////////
// start of obstacle manager - hik
bool obstacleManagerEnable = true;
PathPlanner.SmoothingResult result;
if (obstacleManagerEnable == true)
{
// generate fake obstacles (for simulation testing only)
double obsSize = 10.5 / 2;
List <Coordinates> obstaclePoints = new List <Coordinates>();
List <Obstacle> obstacleClusters = new List <Obstacle>();
// fake left obstacles (for simulation only)
int totalLeftObstacles = Math.Min(0, curLeftBound.Count - 1);
for (int i = 0; i < totalLeftObstacles; i++)
{
obstaclePoints.Clear();
obstaclePoints.Add(curLeftBound[i] + new Coordinates(obsSize, obsSize));
obstaclePoints.Add(curLeftBound[i] + new Coordinates(obsSize, -obsSize));
obstaclePoints.Add(curLeftBound[i] + new Coordinates(-obsSize, -obsSize));
obstaclePoints.Add(curLeftBound[i] + new Coordinates(-obsSize, obsSize));
obstacleClusters.Add(new Obstacle());
obstacleClusters[obstacleClusters.Count - 1].obstaclePolygon = new Polygon(obstaclePoints);
}
// fake right obstacles (for simulation only)
int totalRightObstacles = Math.Min(0, curRightBound.Count - 1);
for (int i = 0; i < totalRightObstacles; i++)
{
obstaclePoints.Clear();
obstaclePoints.Add(curRightBound[i] + new Coordinates(obsSize, obsSize));
obstaclePoints.Add(curRightBound[i] + new Coordinates(obsSize, -obsSize));
obstaclePoints.Add(curRightBound[i] + new Coordinates(-obsSize, -obsSize));
obstaclePoints.Add(curRightBound[i] + new Coordinates(-obsSize, obsSize));
obstacleClusters.Add(new Obstacle());
obstacleClusters[obstacleClusters.Count - 1].obstaclePolygon = new Polygon(obstaclePoints);
}
// fake center obstacles (for simulation only)
int totalCenterObstacles = Math.Min(0, subPath.Count - 1);
for (int i = 2; i < totalCenterObstacles; i++)
{
obstaclePoints.Clear();
obstaclePoints.Add(subPath[i] + new Coordinates(obsSize, obsSize));
obstaclePoints.Add(subPath[i] + new Coordinates(obsSize, -obsSize));
obstaclePoints.Add(subPath[i] + new Coordinates(-obsSize, -obsSize));
obstaclePoints.Add(subPath[i] + new Coordinates(-obsSize, obsSize));
obstacleClusters.Add(new Obstacle());
obstacleClusters[obstacleClusters.Count - 1].obstaclePolygon = new Polygon(obstaclePoints);
}
obstaclePoints.Clear();
obstaclePoints.Add(new Coordinates(10000, 10000));
obstaclePoints.Add(new Coordinates(10000, 10001));
obstaclePoints.Add(new Coordinates(10001, 10000));
obstacleClusters.Add(new Obstacle());
obstacleClusters[obstacleClusters.Count - 1].obstaclePolygon = new Polygon(obstaclePoints);
obstaclePoints.Clear();
obstaclePoints.Add(new Coordinates(1000, 1000));
obstaclePoints.Add(new Coordinates(1000, 1001));
obstaclePoints.Add(new Coordinates(1001, 1000));
obstacleClusters.Add(new Obstacle());
obstacleClusters[obstacleClusters.Count - 1].obstaclePolygon = new Polygon(obstaclePoints);
obstaclePoints.Clear();
obstaclePoints.Add(new Coordinates(-1000, -1000));
obstaclePoints.Add(new Coordinates(-1000, -1001));
obstaclePoints.Add(new Coordinates(-1001, -1000));
obstacleClusters.Add(new Obstacle());
obstacleClusters[obstacleClusters.Count - 1].obstaclePolygon = new Polygon(obstaclePoints);
foreach (Obstacle obs in obstacleClusters)
{
obs.cspacePolygon = new Polygon(obs.obstaclePolygon.points);
}
// find obstacle path and left/right classification
LinePath obstaclePath = new LinePath();
//Boolean successFlag;
//double laneWidthAtPathEnd = 10.0;
//#warning this currently doesn't work
/*obstacleManager.ProcessObstacles(subPath, new LinePath[] { curLeftBound }, new LinePath[] { curRightBound },
* obstacleClusters, laneWidthAtPathEnd,
* out obstaclePath, out successFlag);
*/
// prepare left and right lane bounds
double laneMinSpacing = 0.1;
double laneDesiredSpacing = 0.5;
double laneAlphaS = 10000;
List <Boundary> leftBounds = new List <Boundary>();
List <Boundary> rightBounds = new List <Boundary>();
leftBounds.Add(new Boundary(curLeftBound, laneMinSpacing, laneDesiredSpacing, laneAlphaS));
rightBounds.Add(new Boundary(curRightBound, laneMinSpacing, laneDesiredSpacing, laneAlphaS));
// sort out obstacles as left and right
double obstacleMinSpacing = 0.1;
double obstacleDesiredSpacing = 1.0;
double obstacleAlphaS = 10000;
Boundary bound;
int totalObstacleClusters = obstacleClusters.Count;
for (int i = 0; i < totalObstacleClusters; i++)
{
if (obstacleClusters[i].avoidanceStatus == AvoidanceStatus.Left)
{
// obstacle cluster is to the left of obstacle path
bound = new Boundary(obstacleClusters[i].obstaclePolygon.points, obstacleMinSpacing,
obstacleDesiredSpacing, obstacleAlphaS, true);
bound.CheckFrontBumper = true;
leftBounds.Add(bound);
}
else if (obstacleClusters[i].avoidanceStatus == AvoidanceStatus.Right)
{
// obstacle cluster is to the right of obstacle path
bound = new Boundary(obstacleClusters[i].obstaclePolygon.points, obstacleMinSpacing,
obstacleDesiredSpacing, obstacleAlphaS, true);
bound.CheckFrontBumper = true;
rightBounds.Add(bound);
}
else
{
// obstacle cluster is outside grid, hence ignore obstacle cluster
}
}
Stopwatch stopwatch = new Stopwatch();
stopwatch.Start();
// PlanPath function call with obstacle path and obstacles
result = planner.PlanPath(obstaclePath, obstaclePath, leftBounds, rightBounds,
0, maxSpeed, vs.speed, null, curTimestamp, false);
stopwatch.Stop();
Console.WriteLine("============================================================");
Console.WriteLine("With ObstacleManager - Planner - Elapsed (ms): {0}", stopwatch.ElapsedMilliseconds);
Console.WriteLine("============================================================");
}
else
{
Stopwatch stopwatch = new Stopwatch();
stopwatch.Start();
// original PlanPath function call
result = planner.PlanPath(subPath, curLeftBound, curRightBound,
0, maxSpeed, vs.speed, null, curTimestamp);
stopwatch.Stop();
Console.WriteLine("============================================================");
Console.WriteLine("Without ObstacleManager - Planner - Elapsed (ms): {0}", stopwatch.ElapsedMilliseconds);
Console.WriteLine("============================================================");
}
// end of obstacle manager - hik
////////////////////////////////////////////////////////////////////////////////////////////////////
//PathPlanner.PlanningResult result = planner.PlanPath(subPath, curLeftBound, curRightBound, 0, maxSpeed, vs.speed, null, curTimestamp);
//SmoothedPath path = new SmoothedPath(pathTime);
lock (this) {
planner = null;
}
if (cancelled)
{
return;
}
if (result.result == UrbanChallenge.PathSmoothing.SmoothResult.Sucess)
{
// start tracking the path
Services.TrackingManager.QueueCommand(TrackingCommandBuilder.GetSmoothedPathVelocityCommand(result.path));
//Services.TrackingManager.QueueCommand(TrackingCommandBuilder.GetConstantSteeringConstantSpeedCommand(-.5, 2));
/*TrackingCommand cmd = new TrackingCommand(
* new FeedbackSpeedCommandGenerator(new ConstantSpeedGenerator(2, null)),
* new SinSteeringCommandGenerator(),
* true);
* Services.TrackingManager.QueueCommand(cmd);*/
// send the path's we're tracking to the UI
Services.UIService.PushRelativePath(result.path, curTimestamp, "smoothed path");
cancelled = true;
}
}
public static void ComputeSteeringCommand(IRelativePath path, double dt, bool outputDataset, out double steeringWheelAngle)
{
// get steering Parameters
double q1 = q1_param.Value;
double q2 = q2_param.Value;
double r = r_param.Value;
OperationalVehicleState vs = Services.StateProvider.GetVehicleState();
// get the feedback data
SteeringControlData data = path.GetSteeringControlData(new SteeringControlDataOptions(vs.speed * TahoeParams.actuation_delay));
double offtrackError = data.offtrackError;
double headingError = data.headingError;
// for now, just use 0 for the curvature if it is null
// later, we may want to switch to pure-pursuit in this case
double curvature = data.curvature.GetValueOrDefault(0);
// cap the off-track error term
if (offtrackError > offtrack_error_max_param.Value)
{
offtrackError = Math.Sign(offtrackError) * offtrack_error_max_param.Value;
}
// adjust the heading error 180 degrees if we're in reverse
if (vs.transGear == TransmissionGear.Reverse)
{
headingError += Math.PI;
}
// wrap heading error between -pi and pi
headingError = Math.IEEERemainder(headingError, 2 * Math.PI);
// check for numerical problems
if (double.IsNaN(headingError))
{
headingError = 0;
Console.WriteLine("Error: Heading Error is NaN (In OpPathFollowingBehavior:Process)");
}
else if (Math.Abs(headingError) > Math.PI)
{
headingError -= Math.Sign(headingError) * 2 * Math.PI;
}
// compute desired curvature
double desiredCurvature = (offtrackError * Math.Sqrt(q1 / r)) +
(headingError * Math.Sqrt(q2 / r)) +
(curvature);
// reverse desired curvature if in reverse gear
if (vs.transGear == TransmissionGear.Reverse)
{
desiredCurvature = -desiredCurvature;
}
// check desired curvature
if (double.IsNaN(desiredCurvature))
{
Console.WriteLine("Error: Commanded Curvature is NaN (In OpPathFollowingBehavior:Process)");
desiredCurvature = 0;
}
// add the commands to the dataset
if (outputDataset)
{
CarTimestamp now = Services.CarTime.Now;
Services.Dataset.ItemAs <double>("offtrack error").Add(offtrackError, now);
Services.Dataset.ItemAs <double>("heading error").Add(headingError, now);
Services.Dataset.ItemAs <double>("target curvature").Add(curvature, now);
Services.Dataset.ItemAs <double>("commanded curvature").Add(desiredCurvature, now);
//Operational.Instance.Dataset.ItemAs<Coordinates>("path tangent").Add(curPoint.segment.Tangent(curPoint).Normalize(), now);
}
// convert the desired curvature to a steering wheel angle and return it
steeringWheelAngle = SteeringUtilities.CurvatureToSteeringWheelAngle(desiredCurvature, vs.speed);
}
/// <summary>
/// Computes the engine torque and brake pressure needed to achieve a specified acceleration.
/// </summary>
/// <param name="ra">Requested acceleration in m/s^2</param>
/// <param name="vs">Vehicle state</param>
/// <param name="commandedEngineTorque"></param>
/// <param name="commandedBrakePressure"></param>
/// <returns>True if the vehicle state allows the acceleration to be achieved, false otherwise.</returns>
public static bool GetCommandForAcceleration(double ra, OperationalVehicleState vs, out double commandedEngineTorque, out double commandedBrakePressure)
{
// speed, positive for forward motion (m/s)
double v = Math.Abs(vs.speed);
// current gear (not just shifter but also first, second, etc)
TransmissionGear gear = vs.transGear;
// vehicle pitch, from pose (rad)
double phi = vs.pitch;
if (gear == TransmissionGear.Reverse)
{
phi = -phi;
}
CarTimestamp lastTransChangeTimeTS = Services.Dataset.ItemAs <DateTime>("trans gear change time").CurrentTime;
DateTime lastTransChangeTime = Services.Dataset.ItemAs <DateTime>("trans gear change time").CurrentValue;
if (gear > TransmissionGear.Fourth || gear < TransmissionGear.Reverse || (lastTransChangeTimeTS.IsValid && (HighResDateTime.Now - lastTransChangeTime) < TimeSpan.FromSeconds(1) && (gear == TransmissionGear.First || gear == TransmissionGear.Reverse)))
{
// in park or neutral or have not waited sufficient time after shift, just break out of the function
commandedEngineTorque = 0;
commandedBrakePressure = -TahoeParams.bc0;
return(false);
}
// current gear ratio
double Nt = Math.Abs(TahoeParams.Nt[(int)gear]);
// initialize to reasonable default values in case all else fails
commandedEngineTorque = 0;
commandedBrakePressure = -TahoeParams.bc0;
// effective rotating mass
double mr = TahoeParams.mr1 +
TahoeParams.mr2 * Math.Pow(TahoeParams.Nf, 2) +
TahoeParams.mr3 * Math.Pow(TahoeParams.Nf, 2) * Math.Pow(Nt, 2);
// rolling resistance (N)
double Rr = 2 * TahoeParams.m * TahoeParams.g * (TahoeParams.rr1 + TahoeParams.rr2 * v);
double rps2rpm = 60.0 / (2.0 * Math.PI);
// calculate engine speed in rpm
double rpme = vs.engineRPM;
// calculate driveshaft rpm post torque converter
double rpmd = (v / TahoeParams.r_wheel) * TahoeParams.Nf * Nt * rps2rpm;
// calculate speed ratio, torque ratio, and capacity factor from lookups
double sr = rpmd / rpme;
// tr = interp1(srlu, trlu, sr, 'linear', 'extrap');
double tr = TahoeParams.TorqueRatio(sr);
//Dataset.Source.DatasetSource ds = Operational.Instance.Dataset;
// calculate bleed-off torque
double bleed_torque = TahoeParams.bleed_off_power / (rpme / rps2rpm);
// requested acceleration in Newtons
double ra_corr = (TahoeParams.m + mr) * ra;
// drag force
double drag = 0.5 * TahoeParams.rho * TahoeParams.cD * TahoeParams.cxa * Math.Pow(v, 2);
// pitch force
double pitch_corr = -TahoeParams.m * TahoeParams.g * Math.Sin(phi);
// needed wheel torque (N-m)
double wheelTorque = (ra_corr + Rr + drag + pitch_corr) * TahoeParams.r_wheel;
// commanded engine torque (N-m) to achieve desired acceleration
commandedEngineTorque = wheelTorque / (Nt * TahoeParams.Nf * TahoeParams.eta * tr);
DateTime now = DateTime.Now;
//ds.ItemAs<double>("torque multiplier").Add(tr, now);
//ds.ItemAs<double>("speed - Rr").Add(Rr, now);
//ds.ItemAs<double>("speed - ra").Add(ra_corr, now);
//ds.ItemAs<double>("speed - pitch").Add(pitch_corr, now);
//ds.ItemAs<double>("speed - drag").Add(drag, now);
//ds.ItemAs<double>("speed - wheel torque").Add(wheelTorque, now);
//ds.ItemAs<double>("speed - eng torque").Add(commandedEngineTorque, now);
// retrieve the current engine torque, brake pressure
double current_brake_pressure = vs.brakePressure;
// decide to apply master cylinder pressure or engine torque
// check if we want to apply less torque than is the minimum
if (commandedEngineTorque < TahoeParams.Te_min)
{
// assume that the current engine torque is just the minimum delivered engine torque
double current_engine_torque = TahoeParams.Te_min;
double Btrq = Nt * TahoeParams.Nf * TahoeParams.eta / TahoeParams.r_wheel;
// actual acceleration with no extra torque applied (m / s^2)
double aa = -(Rr + 0.5 * TahoeParams.rho * TahoeParams.cD * TahoeParams.cxa * Math.Pow(v, 2) -
TahoeParams.m * TahoeParams.g * Math.Sin(phi)) / (TahoeParams.m + mr) +
(Btrq * tr * (current_engine_torque - bleed_torque)) / (TahoeParams.m + mr);
// residual brake acceleration (m / s^2)
double ba = ra - aa; // if ba > 0, engine braking is enough
// desired master cylinder pressure (GM units, approx 22 to 50)
double mcp = -TahoeParams.bc0;
if (ba < 0)
{
// compute braking coefficent (which was empirically determined to be speed dependent)
double bc = TahoeParams.bc1 + TahoeParams.bcs * Math.Sqrt(v);
mcp = (-(TahoeParams.m + mr) * ba * TahoeParams.r_wheel / bc) - TahoeParams.bc0;
}
// set commanded engine torque to 0
commandedEngineTorque = 0;
// assign master cylinder pressure
commandedBrakePressure = Math.Round(mcp, 0);
return(true);
}
else
{
// we want to command engine torque
// check if the brake is off (if not, don't apply engine torque)
if (current_brake_pressure >= 24)
{
commandedBrakePressure = -TahoeParams.bc0;
commandedEngineTorque = 0;
return(false);
}
// add in the bleed off torque
commandedEngineTorque += bleed_torque;
// zero out the commande brake pressure
commandedBrakePressure = -TahoeParams.bc0;
return(true);
}
}
/// <summary>
/// Computes a commanded torque and brake pressure to bring the vehicle to a stop given the
/// original speed the vehicle was travelling and the remaining distance. This will keep the
/// commanded speed constant until a final approach distance and then linearly ramp down the
/// speed.
/// </summary>
/// <param name="remaining">Remaining distance to stop in meters</param>
/// <param name="origSpeed">Approach speed of stop maneuver in m/s</param>
/// <param name="inFinalApproach">Flag indicating that the final approach is in progress. Only set to true.</param>
/// <returns>Engine torque and brake pressure to be commanded to stop as desired</returns>
public static bool GetStoppingSpeedCommand(double remaining, double origSpeed, OperationalVehicleState vs, ref double commandedSpeed)
{
// check if we're within the stopping tolerance
if ((remaining <= stop_dist_tolerance && Math.Abs(vs.speed) <= stop_speed_tolerance) || remaining <= 0)
{
// return a commanded speed of 0 which flags that we should just stop
commandedSpeed = 0;
// return that we've stoppped
return(Math.Abs(vs.speed) <= stop_speed_tolerance);
}
// get the distance for the final approach
double stoppingDist = 4;
// check if the remaining distance is greater than the stopping distance or not
if (remaining > stoppingDist)
{
// we have not reached the final approach yet, so keep commanding the original speed
commandedSpeed = Math.Max(origSpeed, min_cmd_speed);
}
else
{
// we are within the final approach distance so linearly ramp down the speed
commandedSpeed = GetPowerProfileVelocity(origSpeed, 0, stoppingDist, remaining);
}
return(false);
}
public static void ComputeCommands(SpeedControlData data, OperationalVehicleState vs, out double commandedEngineTorque, out double commandedBrakePressure)
{
Services.Dataset.ItemAs <double>("commanded speed").Add(data.speed.GetValueOrDefault(-1), Services.CarTime.Now);
// speed (m/s)
double v = Math.Abs(vs.speed);
double max_accel;
if (!vs.IsInDrive)
{
// we're in reverse
max_accel = 0.65;
}
else if (v < 10 * 0.44704)
{
// below 10 mph
max_accel = 1.0;
}
else if (v < 15 * 0.44704)
{
// below 15 mph
double coeff = (v - 10 * 0.44704) / ((15 - 10) * 0.44704);
max_accel = 1.0 - coeff * 0.25;
}
else
{
max_accel = 0.75;
}
if (data.speed.HasValue && data.speed.Value <= 0 && (!data.accel.HasValue || data.accel.Value <= 0))
{
DateTime now = HighResDateTime.Now;
if (zero_speed_time == DateTime.MinValue)
{
zero_speed_time = now;
}
if (now - zero_speed_time < TimeSpan.FromMilliseconds(500) || v < 1)
{
commandedEngineTorque = 0;
commandedBrakePressure = TahoeParams.brake_hold;
return;
}
}
else
{
zero_speed_time = DateTime.MinValue;
}
// desired speed (m/s)
double rv = Math.Abs(data.speed.GetValueOrDefault(v));
double d_err = 0;
double err = data.speed.HasValue ? v - rv : 0;
if (data.speed.HasValue && !double.IsNaN(prev_err))
{
d_err = (prev_err - err) * Settings.TrackingPeriod;
Services.Dataset.ItemAs <double>("speed - d error").Add(d_err, Services.CarTime.Now);
}
prev_err = err;
// commanded acceleration (m / s^2)
double ra = data.accel.GetValueOrDefault(0) - config.kp * (err) - config.ki * int_err + config.kd * d_err;
// cap the maximum acceleration
if (ra > max_accel)
{
ra = max_accel;
}
// add in the zero speed integral term
ra += zero_speed_int * Math.Max(0, 1 - v / zero_speed_int_max_apply_speed);
Services.Dataset.ItemAs <double>("requested acceleration").Add(ra, Services.CarTime.Now);
// compute commands to achieve the requested acceleration and determine if we could achieve those commands
bool couldAchieveRA = SpeedUtilities.GetCommandForAcceleration(ra, vs, out commandedEngineTorque, out commandedBrakePressure);
// only accumulate integral error if we could achieve the target acceleration
if (couldAchieveRA)
{
if (ra < max_accel && data.speed.HasValue)
{
int_err *= config.ki_leak;
int_err += (err) * Settings.TrackingPeriod;
if (Math.Abs(int_err) > config.ki_cap)
{
int_err = Math.Sign(int_err) * config.ki_cap;
}
}
if (v < zero_speed_int_min_speed && ra > 0.2)
{
zero_speed_int += zero_speed_int_rate * Settings.TrackingPeriod;
}
}
if (v > zero_speed_int_reset_speed)
{
zero_speed_int = 0;
}
Services.Dataset.ItemAs <double>("speed - int error").Add(int_err, Services.CarTime.Now);
}
