/// <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 SmoothingResult PlanPath(PlanningSettings settings)
{
SmootherOptions opts = settings.Options;
// for now, just run the smoothing
opts.init_heading = settings.initHeading;
opts.set_init_heading = true;
opts.min_init_velocity = settings.startSpeed * 0.5;
opts.set_min_init_velocity = true;
opts.max_init_velocity = Math.Max(settings.maxSpeed, settings.startSpeed);
opts.set_max_init_velocity = true;
opts.min_velocity = 0.1;
opts.max_velocity = Math.Max(opts.min_velocity + 0.1, settings.maxSpeed);
opts.k_max = Math.Min(TahoeParams.CalculateCurvature(-TahoeParams.SW_max, settings.startSpeed), TahoeParams.CalculateCurvature(TahoeParams.SW_max, settings.startSpeed)) * 0.97;
opts.generate_details = true; // GenerateDetails;
if (settings.endingHeading != null)
{
opts.set_final_heading = true;
opts.final_heading = settings.endingHeading.Value;
}
else
{
opts.set_final_heading = false;
}
opts.set_final_offset = settings.endingPositionFixed;
opts.final_offset_min = settings.endingPositionMin;
opts.final_offset_max = settings.endingPositionMax;
if (settings.maxEndingSpeed != null)
{
opts.set_final_velocity_max = true;
opts.final_velocity_max = Math.Max(opts.min_velocity + 0.1, settings.maxEndingSpeed.Value);
}
else
{
opts.set_final_velocity_max = false;
}
opts.a_lat_max = 6;
// create the boundary list
List <UrbanChallenge.PathSmoothing.PathPoint> ret = new List <UrbanChallenge.PathSmoothing.PathPoint>();
smoother = new PathSmoother();
OperationalTrace.WriteVerbose("calling smooth path");
SmoothResult result = smoother.SmoothPath(settings.basePath, settings.targetPaths, settings.leftBounds, settings.rightBounds, opts, ret);
if (result != SmoothResult.Sucess)
{
OperationalTrace.WriteWarning("smooth path result: {0}", result);
}
else
{
OperationalTrace.WriteVerbose("smooth path result: {0}", result);
}
AvoidanceDetails details = null;
if (opts.generate_details)
{
details = new AvoidanceDetails();
details.leftBounds = settings.leftBounds;
details.rightBounds = settings.rightBounds;
details.smoothingDetails = smoother.GetSmoothingDetails();
LastAvoidanceDetails = details;
// push out the points
Coordinates[] leftPoints = new Coordinates[details.smoothingDetails.leftBounds.Length];
for (int i = 0; i < leftPoints.Length; i++)
{
leftPoints[i] = details.smoothingDetails.leftBounds[i].point;
}
Coordinates[] rightPoints = new Coordinates[details.smoothingDetails.rightBounds.Length];
for (int i = 0; i < rightPoints.Length; i++)
{
rightPoints[i] = details.smoothingDetails.rightBounds[i].point;
}
Services.UIService.PushPoints(leftPoints, settings.timestamp, "left bound points", true);
Services.UIService.PushPoints(rightPoints, settings.timestamp, "right bound points", true);
}
//if (result == SmoothResult.Sucess) {
Coordinates[] points = new Coordinates[ret.Count];
double[] speeds = new double[ret.Count];
for (int i = 0; i < ret.Count; i++)
{
points[i] = new Coordinates(ret[i].x, ret[i].y);
speeds[i] = ret[i].v;
}
SmoothedPath path = new SmoothedPath(settings.timestamp, points, speeds);
return(new SmoothingResult(result, path, details));
/*}
* else {
* SmoothedPath path = new SmoothedPath(settings.timestamp, settings.basePath, null);
*
* return new SmoothingResult(result, path);
* }*/
}
/// <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);
}
}
