//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);
//}
}
