public void OnAllFast(
CarTimestamp ct,
double wheelFL,
double wheelFR,
double wheelRR,
double wheelRL,
double rpm,
double steerAngle,
TransmissionGear transGear,
int transDir,
double actTorque,
double reqTorque,
int cylinderDeactivation,
int pedalPos,
double brakePressure)
{
OnWheelspeed(wheelFL, wheelFR, wheelRR, wheelRL, ct);
OnBrake(brakePressure, ct);
if (transGear != lastRecvTransmissionGear)
{
Services.Dataset.ItemAs <DateTime>("trans gear change time").Add(HighResDateTime.Now, ct);
lastRecvTransmissionGear = transGear;
}
}
public OperationalSimVehicleState GetOperationalSimVehicleState(WorldService service)
{
TransmissionGear g = TransmissionGear.Neutral;
switch (gear)
{
case '1': g = TransmissionGear.First; break;
case '2': g = TransmissionGear.Second; break;
case '3': g = TransmissionGear.Third; break;
case '4': g = TransmissionGear.Fourth; break;
case 'r': g = TransmissionGear.Reverse; break;
}
double rps2rpm = 60.0 / (2.0 * Math.PI);
double bleed_off_torque = TahoeParams.bleed_off_power / engine_speed;
return(new OperationalSimVehicleState(
simVehicleState.Position, simVehicleState.Speed, simVehicleState.Heading.ArcTan, steering_wheel,
g, engine_torque + bleed_off_torque, master_cylinder_pressure, engine_speed * rps2rpm, carMode,
0, false, SimulatorClient.GetCurrentTimestamp));
}
public void SetTransmissionGear(TransmissionGear g) {
if (g >= TransmissionGear.First && g <= TransmissionGear.Fourth) {
gear = TransmissionGear.First;
}
else if (g == TransmissionGear.Reverse) {
gear = TransmissionGear.Reverse;
}
}
public TrackingData(double? engineTorque, double? brakePressure, TransmissionGear? gear, double? steeringAngle, CompletionResult result)
{
this.engineTorque = engineTorque;
this.brakePressure = brakePressure;
this.gear = gear;
this.steeringAngle = steeringAngle;
this.result = result;
}
public TransmissionGear GetGear()
{
var gear = new TransmissionGear();
gear.Name = gearName.Text;
gear.Ratio = ratioBox.Value;
return(gear);
}
/// <summary>
/// Gets the engine RPM based on truck speed, and other variables.
/// </summary>
/// <param name="speed">
/// The truck speed in MPH or KPH depending on the <see cref="UnitSystem"/> configured
/// </param>
/// <param name="gear">The current gear the truck is in</param>
/// <returns></returns>
private int GetRpmFromSpeed(int speed, TransmissionGear gear)
{
decimal axelRatio = diffRatio.Value;
decimal distance = (Program.Config.UnitSystem == UnitSystem.Imperial)
? DISTANCE_MILES
: DISTANCE_KILOS;
return((int)Math.Round((axelRatio * speed * gear.Ratio * distance) / TIRE_DIAMETER, 0));
}
public EditGearForm(TransmissionGear gear = null)
{
InitializeComponent();
if (gear != null)
{
gearName.Text = gear.Name;
ratioBox.SetValueInRange(gear.Ratio);
}
}
/// <summary>
/// Gets the truck speed based off of Engine RPM, and a few other variables.
/// </summary>
/// <param name="rpm">The current engine RPM</param>
/// <param name="gear">The current gear the truck is in</param>
/// <returns>The truck speed in MPH or KPH depending on the <see cref="UnitSystem"/> configured</returns>
private int GetSpeedFromRpm(int rpm, TransmissionGear gear)
{
decimal axelRatio = diffRatio.Value;
decimal distance = (Program.Config.UnitSystem == UnitSystem.Imperial)
? DISTANCE_MILES
: DISTANCE_KILOS;
// Now the real math starts
distance = (distance * axelRatio * gear.Ratio) / TIRE_DIAMETER;
return((int)Math.Round(rpm / distance, 0));
}
/// <summary>
/// Gets the gear name, or if the gear does not have a name, generates a name based off
/// of the gear index.
/// </summary>
/// <param name="index">The index of the gear in the list (sorted by ratio desc)</param>
/// <param name="gear">The gear we are fetching the name for</param>
/// <returns></returns>
private string GetGearNameAtIndex(int index, TransmissionGear gear)
{
if (gear.IsReverse)
{
return(Transmission.GetGearNameAtIndex(index, gear, ReverseGears));
}
else
{
return(Transmission.GetGearNameAtIndex(index, gear, ForwardGears));
}
}
public override void SetTransmissionGear(TransmissionGear g)
{
if (g >= TransmissionGear.First && g <= TransmissionGear.Fourth)
{
gear = '1';
}
else if (g == TransmissionGear.Reverse)
{
gear = 'r';
}
}
public OperationalVehicleState(double speed, double steeringAngle, TransmissionGear transGear, double pitch, double brakePressure, double engineTorque, double engineRPM, double pedalPosition, CarTimestamp timestamp)
{
this.speed = speed;
this.steeringAngle = steeringAngle;
this.transGear = transGear;
this.pitch = pitch;
this.engineTorque = engineTorque;
this.engineRPM = engineRPM;
this.brakePressure = brakePressure;
this.pedalPosition = pedalPosition;
this.timestamp = timestamp;
}
public OperationalVehicleState(double speed, double steeringAngle, TransmissionGear transGear, double pitch, double brakePressure, double engineTorque, double engineRPM, double pedalPosition, CarTimestamp timestamp)
{
this.speed = speed;
this.steeringAngle = steeringAngle;
this.transGear = transGear;
this.pitch = pitch;
this.engineTorque = engineTorque;
this.engineRPM = engineRPM;
this.brakePressure = brakePressure;
this.pedalPosition = pedalPosition;
this.timestamp = timestamp;
}
/// <summary>
/// Adds a gear to the Gear List View
/// </summary>
/// <param name="gear"></param>
/// <param name="index"></param>
private void AddGear(TransmissionGear gear, int index)
{
string name = (showPlaceholdersBox.Checked) ? GetGearNameAtIndex(index, gear) : gear.Name;
ListViewItem item = new ListViewItem((index + 1).ToString());
item.SubItems.Add(name);
item.SubItems.Add($"{gear.Ratio}:1");
item.Tag = index;
index = (gear.IsReverse) ? 0 : 1;
gearListView.Groups[index].Items.Add(item);
gearListView.Items.Add(item);
}
public OperationalSimVehicleState(Coordinates position, double speed, double heading, double steeringAngle, TransmissionGear transGear, double engineTorque, double brakePressure, double engineRPM, CarMode carMode, double stopLineDistance, bool stoplineFound, CarTimestamp timestamp)
{
this.position = position;
this.speed = speed;
this.heading = heading;
this.steeringAngle = steeringAngle;
this.transGear = transGear;
this.engineTorque = engineTorque;
this.brakePressure = brakePressure;
this.engineRPM = engineRPM;
this.carMode = carMode;
this.stopLineDistance = stopLineDistance;
this.stoplineFound = stoplineFound;
this.timestamp = timestamp;
}
public OperationalSimVehicleState(Coordinates position, double speed, double heading, double steeringAngle, TransmissionGear transGear, double engineTorque, double brakePressure, double engineRPM, CarMode carMode, double stopLineDistance, bool stoplineFound, CarTimestamp timestamp)
{
this.position = position;
this.speed = speed;
this.heading = heading;
this.steeringAngle = steeringAngle;
this.transGear = transGear;
this.engineTorque = engineTorque;
this.brakePressure = brakePressure;
this.engineRPM = engineRPM;
this.carMode = carMode;
this.stopLineDistance = stopLineDistance;
this.stoplineFound = stoplineFound;
this.timestamp = timestamp;
}
public TahoeSim(Coordinates loc, double heading, double speed, TransmissionGear gear, double throttle, double steering_wheel, double engine_torque, double master_cylinder_pressure, double engine_speed) {
InitializeState();
this.x = loc.X;
this.y = loc.Y;
this.heading = heading;
this.speed = speed;
this.gear = gear;
this.throttle = throttle;
this.steering_wheel = steering_wheel;
this.engine_torque = engine_torque;
this.master_cylinder_pressure = master_cylinder_pressure;
this.engine_speed = engine_speed;
TahoeConstraints();
}
// initialise vehicle state and commands
private void InitializeState() {
// initialise vehicle state
this.x = 0;
this.y = 0;
this.heading = 0;
speed = 0;
gear = TransmissionGear.First; // gear, 'r', '1', '2', '3', '4'
throttle = 0.0; // throttle (between 0 and 1), the fraction of maximum torque commanded
steering_wheel = 0.0; // steering wheel angle (rad., positive left)
engine_torque = 0.0; // engine torque (N-m)
master_cylinder_pressure = 0.0; // master cylinder pressure (Aarons)
engine_speed = 0.0; // engine speed (rad. / sec.)
// initialise commands
commanded_steering_wheel = 0.0;
commanded_engine_torque = 0.0;
commanded_master_cylinder_pressure = 35; // initialize to something so we don't start moving around
// make sure initial state is valid
TahoeConstraints();
}
/// <summary>
/// Method called whenever a gear is modified. This method will move the gear
/// to the Forward or Reverse gear depending.
/// </summary>
/// <param name="oldGear">The gear before the changes</param>
/// <param name="newGear">The new gear values</param>
/// <param name="index">The index the oldGear is at in its corresponding list</param>
/// <returns></returns>
private bool OnGearEdit(TransmissionGear oldGear, TransmissionGear newGear, int index)
{
// Are we switching from forward to reverse (or vise-versa)?
if (oldGear.IsReverse != newGear.IsReverse)
{
if (newGear.IsReverse)
{
ReverseGears.Add(newGear);
ForwardGears.RemoveAt(index);
}
else
{
ForwardGears.Add(newGear);
ReverseGears.RemoveAt(index);
}
}
else
{
var list = (newGear.IsReverse) ? ReverseGears : ForwardGears;
list[index] = newGear;
}
return(true);
}
public void OnAllFast(
CarTimestamp ct,
double wheelFL,
double wheelFR,
double wheelRR,
double wheelRL,
double rpm,
double steerAngle,
TransmissionGear transGear,
int transDir,
double actTorque,
double reqTorque,
int cylinderDeactivation,
int pedalPos,
double brakePressure)
{
OnWheelspeed(wheelFL, wheelFR, wheelRR, wheelRL, ct);
OnBrake(brakePressure, ct);
if (transGear != lastRecvTransmissionGear) {
Services.Dataset.ItemAs<DateTime>("trans gear change time").Add(HighResDateTime.Now, ct);
lastRecvTransmissionGear = transGear;
}
}
public static TrackingCommand GetShiftTransmissionCommand(TransmissionGear gear, double?steeringAngle, double?rateLimit, bool waitBoth)
{
return(new TrackingCommand(new ShiftSpeedCommand(gear), new ConstantSteeringCommandGenerator(steeringAngle, rateLimit, true), waitBoth));
}
public ShiftSpeedCommand(TransmissionGear gear)
{
this.gear = gear;
this.phase = Phase.Braking;
this.result = CompletionResult.Working;
}
void IChannelListener.MessageArrived(string channelName, object message)
{
if (message is OperationalSimVehicleState)
{
OperationalTrace.ThreadTraceSource = TraceSource;
Trace.CorrelationManager.StartLogicalOperation("simstate callback");
try {
OperationalSimVehicleState state = (OperationalSimVehicleState)message;
DatasetSource ds = Services.Dataset;
OperationalTrace.WriteVerbose("received operational sim state, t = {0}", state.Timestamp);
Services.Dataset.MarkOperation("pose rate", LocalCarTimeProvider.LocalNow);
CarTimestamp now = state.Timestamp;
ds.ItemAs <Coordinates>("xy").Add(state.Position, now);
ds.ItemAs <double>("speed").Add(state.Speed, now);
ds.ItemAs <double>("heading").Add(state.Heading, now);
ds.ItemAs <double>("actual steering").Add(state.SteeringAngle, now);
ds.ItemAs <TransmissionGear>("transmission gear").Add(state.TransmissionGear, now);
ds.ItemAs <double>("engine torque").Add(state.EngineTorque, now);
ds.ItemAs <double>("brake pressure").Add(state.BrakePressure, now);
ds.ItemAs <double>("rpm").Add(state.EngineRPM, now);
if (state.TransmissionGear != lastRecvTransGear)
{
ds.ItemAs <DateTime>("trans gear change time").Add(HighResDateTime.Now, now);
lastRecvTransGear = state.TransmissionGear;
}
lastCarMode = state.CarMode;
if (CarModeChanged != null)
{
CarModeChanged(this, new CarModeChangedEventArgs(lastCarMode, now));
}
// build the current relative pose matrix
Matrix4 relativePose = Matrix4.Translation(state.Position.X, state.Position.Y, 0) * Matrix4.YPR(state.Heading, 0, 0);
relativePose = relativePose.Inverse();
// push on to the relative pose stack
Services.RelativePose.PushTransform(now, relativePose);
// push the current absolute pose entry
AbsolutePose absPose = new AbsolutePose(state.Position, state.Heading, now);
Services.AbsolutePose.PushAbsolutePose(absPose);
}
finally {
Trace.CorrelationManager.StopLogicalOperation();
OperationalTrace.ThreadTraceSource = null;
}
}
else if (message is SceneEstimatorUntrackedClusterCollection)
{
// push to obstacle pipeline
Services.ObstaclePipeline.OnUntrackedClustersReceived(((SceneEstimatorUntrackedClusterCollection)message));
}
else if (message is SceneEstimatorTrackedClusterCollection)
{
Services.ObstaclePipeline.OnTrackedClustersReceived((SceneEstimatorTrackedClusterCollection)message);
}
}
public override void SetTransmissionGear(TransmissionGear g)
{
if (g >= TransmissionGear.First && g <= TransmissionGear.Fourth)
{
gear = '1';
}
else if (g == TransmissionGear.Reverse)
{
gear = 'r';
}
}
public static TrackingCommand GetShiftTransmissionCommand(TransmissionGear gear, double? steeringAngle, double? rateLimit, bool waitBoth)
{
return new TrackingCommand(new ShiftSpeedCommand(gear), new ConstantSteeringCommandGenerator(steeringAngle, rateLimit, true), waitBoth);
}
/// <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);
}
}
public ShiftSpeedCommand(TransmissionGear gear)
{
this.gear = gear;
this.phase = Phase.Braking;
this.result = CompletionResult.Working;
}
public abstract void SetTransmissionGear(TransmissionGear gear);
public void SetCommand(double? engineTorque, double? brakePressue, double? steeringAngle, TransmissionGear? gear)
{
// break out if we're in test mode
if (Settings.TestMode) {
return;
}
if (steeringAngle.HasValue) {
int tempSteeringValue = (int)((steeringAngle.Value * 180 / Math.PI) / 0.0625);
lastSteeringValue = (short)tempSteeringValue;
}
else {
lastSteeringValue = null;
}
if (brakePressue.HasValue) {
// do a check on brake values
if (brakePressue.Value < 22)
brakePressue = 22;
if (brakePressue.Value > 65)
brakePressue = 65;
lastBrakeValue = (byte)Math.Round(brakePressue.Value, 0);
}
else {
lastBrakeValue = null;
}
if (engineTorque.HasValue) {
// map the value of throttle to torque
lastThrottleValue = MapThrottle(engineTorque.Value);
}
else {
lastThrottleValue = null;
}
if (gear.HasValue) {
switch (gear.Value) {
case TransmissionGear.First:
case TransmissionGear.Second:
case TransmissionGear.Third:
case TransmissionGear.Fourth:
lastTransmissionValue = ActuationCommandInterface.TRANS_GEAR_DRIVE;
break;
case TransmissionGear.Reverse:
lastTransmissionValue = ActuationCommandInterface.TRANS_GEAR_REV;
break;
case TransmissionGear.Park:
lastTransmissionValue = ActuationCommandInterface.TRANS_GEAR_PARK;
break;
case TransmissionGear.Neutral:
lastTransmissionValue = ActuationCommandInterface.TRANS_GEAR_NEUTRAL;
break;
default:
lastTransmissionValue = null;
break;
}
}
else {
lastTransmissionValue = null;
}
}
void IChannelListener.MessageArrived(string channelName, object message)
{
if (message is OperationalSimVehicleState) {
OperationalTrace.ThreadTraceSource = TraceSource;
Trace.CorrelationManager.StartLogicalOperation("simstate callback");
try {
OperationalSimVehicleState state = (OperationalSimVehicleState)message;
DatasetSource ds = Services.Dataset;
OperationalTrace.WriteVerbose("received operational sim state, t = {0}", state.Timestamp);
Services.Dataset.MarkOperation("pose rate", LocalCarTimeProvider.LocalNow);
CarTimestamp now = state.Timestamp;
ds.ItemAs<Coordinates>("xy").Add(state.Position, now);
ds.ItemAs<double>("speed").Add(state.Speed, now);
ds.ItemAs<double>("heading").Add(state.Heading, now);
ds.ItemAs<double>("actual steering").Add(state.SteeringAngle, now);
ds.ItemAs<TransmissionGear>("transmission gear").Add(state.TransmissionGear, now);
ds.ItemAs<double>("engine torque").Add(state.EngineTorque, now);
ds.ItemAs<double>("brake pressure").Add(state.BrakePressure, now);
ds.ItemAs<double>("rpm").Add(state.EngineRPM, now);
if (state.TransmissionGear != lastRecvTransGear) {
ds.ItemAs<DateTime>("trans gear change time").Add(HighResDateTime.Now, now);
lastRecvTransGear = state.TransmissionGear;
}
lastCarMode = state.CarMode;
if (CarModeChanged != null) {
CarModeChanged(this, new CarModeChangedEventArgs(lastCarMode, now));
}
// build the current relative pose matrix
Matrix4 relativePose = Matrix4.Translation(state.Position.X, state.Position.Y, 0)*Matrix4.YPR(state.Heading, 0, 0);
relativePose = relativePose.Inverse();
// push on to the relative pose stack
Services.RelativePose.PushTransform(now, relativePose);
// push the current absolute pose entry
AbsolutePose absPose = new AbsolutePose(state.Position, state.Heading, now);
Services.AbsolutePose.PushAbsolutePose(absPose);
}
finally {
Trace.CorrelationManager.StopLogicalOperation();
OperationalTrace.ThreadTraceSource = null;
}
}
else if (message is SceneEstimatorUntrackedClusterCollection) {
// push to obstacle pipeline
Services.ObstaclePipeline.OnUntrackedClustersReceived(((SceneEstimatorUntrackedClusterCollection)message));
}
else if (message is SceneEstimatorTrackedClusterCollection) {
Services.ObstaclePipeline.OnTrackedClustersReceived((SceneEstimatorTrackedClusterCollection)message);
}
}
public ConstantSpeedCommandGenerator(double engineTorque, double brakePressure, TransmissionGear? transGear)
{
this.engineTorque = engineTorque;
this.brakePressure = brakePressure;
this.transGear = transGear;
}
// --------------------------------------------------------------------------------
// checks whether the gear is consistent with the current throttle and speed setting
// shifts gears if necessary
private void CheckGear() {
// convert speed to mph
double spd = speed * 2.23693629;
if (throttle < 0.125) {
// gentle acceleration
switch (gear) {
case TransmissionGear.First:
// upshift
if (spd > 10.0)
gear = TransmissionGear.Second;
// no downshift
break;
case TransmissionGear.Second:
// upshift
if (spd > 16.0)
gear = TransmissionGear.Third;
// downshift
if (spd < 7.5)
gear = TransmissionGear.First;
break;
case TransmissionGear.Third:
// upshift
if (spd > 24.0)
gear = TransmissionGear.Fourth;
// downshift
if (spd < 7.5)
gear = TransmissionGear.Second;
break;
case TransmissionGear.Fourth:
// no upshift (no 4-lock)
// downshift
if (spd < 20.0)
gear = TransmissionGear.Third;
break;
}
}
else if (throttle > 0.875) {
// jackass acceleration
switch (gear) {
case TransmissionGear.First:
// upshift
if (spd > 32.5)
gear = TransmissionGear.Second;
// no downshift
break;
case TransmissionGear.Second:
// upshift
if (spd > 63.0)
gear = TransmissionGear.Third;
// downshift
if (spd < 26.0)
gear = TransmissionGear.First;
break;
case TransmissionGear.Third:
// upshift
if (spd > 101.0)
gear = TransmissionGear.Fourth;
// downshift
if (spd < 62.0)
gear = TransmissionGear.Second;
break;
case TransmissionGear.Fourth:
// no upshift (no 4-lock)
// downshift
if (spd < 97.0)
gear = TransmissionGear.Third;
break;
}
}
else {
// midrange acceleration
switch (gear) {
case TransmissionGear.First:
// upshift
if (throttle < (0.875 - 0.125) * (spd - 10.0) / (32.5 - 10.0) + 0.125)
gear = TransmissionGear.Second;
// no downshift
break;
case TransmissionGear.Second:
// upshift
if (throttle < (0.875 - 0.125) * (spd - 16.0) / (63.0 - 16.0) + 0.125)
gear = TransmissionGear.Third;
// downshift
if (speed < 7.5)
gear = TransmissionGear.First;
break;
case TransmissionGear.Third:
// upshift
if (throttle < (0.875 - 0.125) * (spd - 24.0) / (101.0 - 24.0) + 0.125)
gear = TransmissionGear.Fourth;
// downshift
if (throttle > (0.875 - 0.375) * (spd - 7.5) / (43.0 - 7.5) + 0.375)
gear = TransmissionGear.Second;
else if (speed < 7.5)
gear = TransmissionGear.First;
break;
case TransmissionGear.Fourth:
// no upshift (no 4-lock)
// downshift
if (throttle > (0.875 - 0.125) * (spd - 20.0) / (76.0 - 20.0) + 0.125)
gear = TransmissionGear.Third;
break; //
}
}
}
public void SetCommand(double? engineTorque, double? brakePressue, double? steering, TransmissionGear? gear)
{
if (Settings.TestMode)
return;
if (steering.HasValue && Math.Abs(steering.Value) > TahoeParams.SW_max) {
OperationalTrace.WriteWarning("steering out of range: received {0}, limit {1}", steering.Value, TahoeParams.SW_max);
}
//Console.WriteLine("command--steering: {0}, torque: {1}, brake: {2}, gear: {3}", steering, engineTorque, brakePressue, gear);
try {
dynamicsSim.SetSteeringBrakeThrottle(steering, engineTorque, brakePressue);
if (gear.HasValue) {
dynamicsSim.SetTransmissionGear(gear.Value);
}
}
catch (SocketException) {
// get the DynamicsSimFacade
dynamicsSim = (DynamicsSimFacade)CommBuilder.GetObject(DynamicsSimFacade.ServiceName);
throw;
}
}
public OperationalSpeedCommand(double engineTorque, double brakePressure, TransmissionGear? transGear)
{
this.engineTorque = engineTorque;
this.brakePressure = brakePressure;
this.transGear = transGear;
}
