/// <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 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); }
/// <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; }
/// <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; } }
protected bool BeginProcess() { if (cancelled) return false; curTimestamp = Services.RelativePose.CurrentTimestamp; vs = Services.StateProvider.GetVehicleState(); // check if we're currently in drive bool transBypassed = Services.Dataset.ItemAs<bool>("trans bypassed").CurrentValue; if (!Settings.TestMode && !transBypassed && ((!reverseGear && !vs.IsInDrive) || (reverseGear && vs.transGear != TransmissionGear.Reverse))) { BehaviorManager.TraceSource.TraceEvent(TraceEventType.Information, 0, "shifting to drive"); // execute a shift command Services.TrackingManager.QueueCommand(TrackingCommandBuilder.GetShiftTransmissionCommand(reverseGear ? TransmissionGear.Reverse : TransmissionGear.First, null, null, false)); // don't process anything else return false; } InitializePlanningSettings(); // return true to indicate that processing should continue return true; }