public void GetSteeringCommand(ref double?steeringAngle) { // calculate a sinusoidal curvature double theta = (DateTime.Now.Second + DateTime.Now.Millisecond / 1000.0) / 30 * 2 * Math.PI; double curvature = 0.075 * Math.Sin(theta); steeringAngle = SteeringUtilities.CurvatureToSteeringWheelAngle(curvature, 1); }
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); }