//Calculate the steer angle alpha by using a PDI controller public float GetNewValue(float error, PID_Parameters pid_parameters) { //The value we want to change with the PID controller to minimize rhe error, such as the steering angle float alpha = 0f; //P alpha = -pid_parameters.P * error; //I //The sum is the average of the last 1000 values error_sum = HelpStuff.AddValueToAverage(error_sum, Time.deltaTime * error, 1000f); alpha -= pid_parameters.I * error_sum; //D float d_dt_CTE = (error - error_old) / Time.deltaTime; alpha -= pid_parameters.D * d_dt_CTE; //Save for next loop error_old = error; return(alpha); }
//Smooth the path by taking the average of the surrounding nodes private List <Vector3> SmoothPathSimple(List <Vector3> waypoints) { //First add new waypoints between the old to make it easier to smooth the path waypoints = AddExtraWaypoints(waypoints); waypoints = AddExtraWaypoints(waypoints); //Smooth List <Vector3> waypointsSmooth = new List <Vector3>(); for (int i = 0; i < waypoints.Count; i++) { Vector3 p1 = waypoints[i]; int iPlusOne = HelpStuff.ClampListIndex(i + 1, waypoints.Count); int iMinusOne = HelpStuff.ClampListIndex(i - 1, waypoints.Count); Vector3 p0 = waypoints[iMinusOne]; Vector3 p2 = waypoints[iPlusOne]; waypointsSmooth.Add((p0 + p1 + p2) / 3f); } return(waypointsSmooth); }
//Calculate cross track error CTE from pos to closest point on path //CTE is generally measured from front wheels //From https://www.udacity.com/course/viewer#!/c-cs373/l-48696626/e-48403941/m-48716166 public float CalculateCTE(Vector3 carPos, Vector3 wp_from, Vector3 wp_to) { Vector2 closestPos2D = HelpStuff.GetClosestPointOnLine(wp_from.XZ(), wp_to.XZ(), carPos.XZ()); Vector3 progressCoordinate = closestPos2D.XYZ(); //Display where this coordinate is //Debug.DrawLine(progressCoordinate, carPos); float CTE = (carPos - progressCoordinate).magnitude; //Debug.Log(CTE); //We need to determine if CTE is negative or positive so we can steer in the direction we need //Is the car to the right or to the left of the upcoming waypoint //http://forum.unity3d.com/threads/left-right-test-function.31420/ Vector3 toCarVec = carPos - wp_from; Vector3 toWaypointVec = wp_to - wp_from; Vector3 perp = Vector3.Cross(toCarVec, toWaypointVec); float dir = Vector3.Dot(perp, Vector3.up); //The car is right of the waypoint if (dir > 0f) { CTE *= -1f; } return(CTE); }
//Get a clamped waypoint from a list of waypoints public Vector3 GetWaypointPos(int index, bool isReversing) { int waypointIndex = HelpStuff.ClampListIndex(index, wayPoints.Count); //Vector3 waypointPos = wayPoints[waypointIndex].frontWheelPos; Vector3 waypointPos = isReversing ? wayPoints[waypointIndex].reverseWheelPos : wayPoints[waypointIndex].frontWheelPos; return(waypointPos); }
public void Start() { //float start = 0; for (int i = 0; i < 360; i++) { Debug.Log(HelpStuff.RoundValue(i, 20f)); //start } }
//A fast car can't turn the wheels as much as it can when driving slower float GetMaxSteeringAngle() { float lowSpeedSteerAngle = 45f; float highSpeedSteerAngle = 5f; float speedFactor = Mathf.Clamp01(Mathf.Abs(currentSpeed) / carDataController.carData.maxSpeed); //Lerp exponentially float wheelAngle = highSpeedSteerAngle + HelpStuff.Eerp(lowSpeedSteerAngle, highSpeedSteerAngle, speedFactor); return(wheelAngle); }
//Add one waypoint between each other waypoint private List <Vector3> AddExtraWaypoints(List <Vector3> waypoints) { List <Vector3> waypointsExtra = new List <Vector3>(); for (int i = 0; i < waypoints.Count; i++) { Vector3 p1 = waypoints[i]; int iPlusOne = HelpStuff.ClampListIndex(i + 1, waypoints.Count); Vector3 p2 = waypoints[iPlusOne]; waypointsExtra.Add(p1); waypointsExtra.Add((p1 + p2) * 0.5f); } return(waypointsExtra); }
private void OnDrawGizmos() { //Display the connection between the waypoints, so we know they are sorted in the correct order List <Node> nodes = GetAllWaypoints(); Gizmos.color = Color.black; //So the line doesnt intersect with the ground Vector3 heightChange = new Vector3(0f, 0.01f, 0f); if (nodes != null && nodes.Count > 1) { for (int i = 0; i < nodes.Count; i++) { Node node = nodes[i]; Vector3 p1 = node.rearWheelPos + heightChange; int iPlusOne = HelpStuff.ClampListIndex(i + 1, nodes.Count); Vector3 p2 = nodes[iPlusOne].rearWheelPos + heightChange; Gizmos.DrawLine(p1, p2); //First wp should be bigger so we know where it starts if (i == 0) { Gizmos.DrawSphere(node.rearWheelPos, 0.4f); } else if (i == 1) { Gizmos.DrawSphere(node.rearWheelPos, 0.3f); } else { Gizmos.DrawSphere(node.rearWheelPos, 0.2f); } } } //Calculate heading (y rotation) if you have two waypoints //if (obj1 != null && obj2 != null) //{ // Gizmos.color = Color.blue; // Gizmos.DrawLine(obj1.position, obj1.position + Vector3.forward * 3f); // Gizmos.DrawLine(obj1.position, obj2.position); // //The real angle we want to duplicate with our own calculations // obj1.LookAt(obj2); // //Calculate the same y angle as obj1 has // Vector3 from = Vector3.forward; // Vector3 to = (obj2.position - obj1.position); // float heading = HelpStuff.CalculateAngle(from, to); // Debug.Log(heading + " " + obj1.eulerAngles.y); //} }