Need an advise to upgrade my code!!!

I have a simple code of waypoints, object with this code will go to waypoints positions and everything works fine. But i have a problem: object`s speed falls when its near the next waypoint.Thanks for any help! Here is my code :

using UnityEngine;

using System.Collections;

public class Waypoints2 : MonoBehaviour 
{

	public Vector3[] waypoints = new Vector3[1];
    public float waypointRadius = 1.5f;
    public float damping = 0.1f;
    public bool loop = false;
    public float speed = 2.0f;
    public bool faceHeading = true;
 
    private Vector3 currentHeading,targetHeading;
    private int targetwaypoint;
    private Transform xform;
	
    private bool useRigidbody;
    private Rigidbody rigidmember;
 
    void OnTriggerEnter(Collider collision)
    {
       Destroy(gameObject);
    }
 
 
    // Use this for initialization
    protected void Start ()
    {	
       xform = transform;
	
       currentHeading = xform.forward;
       if(waypoints.Length<=0)
       {
          Debug.Log("No waypoints on "+name);
          enabled = false;
       }
       targetwaypoint = 0;
       if(rigidbody!=null)
       {
          useRigidbody = true;
          rigidmember = rigidbody;
       }
       else
       {
          useRigidbody = false;
       }
    }
 
 
    // calculates a new heading
    protected void FixedUpdate ()
    {
       targetHeading = waypoints[targetwaypoint] - xform.position;
 
       currentHeading = Vector3.Lerp(currentHeading,targetHeading,damping*Time.deltaTime);
    }
 
    // moves us along current heading
     void Update()
    {
 	   if(Freeze.freezed == true && transform.position.x < 180 )
		{
			
		}
		
       if(useRigidbody)
          rigidmember.velocity = currentHeading * speed;
       else
          xform.position +=currentHeading * Time.deltaTime * speed;

       if(faceHeading)
          xform.LookAt(xform.position+currentHeading);
 
       if(Vector3.Distance(xform.position,waypoints[targetwaypoint])<=waypointRadius)
       {
          targetwaypoint++;
          if(targetwaypoint>=waypoints.Length)
          {
             targetwaypoint = 0;
             if(!loop)
                enabled = false;
          }
       }
    }
	
    // draws red line from waypoint to waypoint
    public void OnDrawGizmos()
    {
       Gizmos.color = Color.red;
       if(waypoints==null)
          return;
       for(int i=0;i< waypoints.Length;i++)
       {
          Vector3 pos = waypoints*;*

if(i>0)
{
Vector3 prev = waypoints[i-1];
Gizmos.DrawLine(prev,pos);
}
}
}
}

The problem lies with this line: currentHeading = Vector3.Lerp(currentHeading,targetHeading,damping*Time.deltaTime);


This is my answer from this question

Switch to Vector3.MoveTowards( transform.position, endPos.position, Time.deltaTime ).

Using Vector3.Lerp will never get you to that endPos.position exactly, but it will get very close to the endPos.position. To explain better. I will use Mathf.Lerp (the following is not code, it is a sequence of event):

start = 0;
end = 1;
step = 0.1;
 
start = Mathf.Lerp( start, end, step );
//start becomes 0.1
 
start = Mathf.Lerp( start, end, step );
//start becomes 0.19
 
start = Mathf.Lerp( start, end, step );
//start becomes 0.271
 
start = Mathf.Lerp( start, end, step );
//start becomes 0.3439

What happen here is that the start will become very close to end, but it will never become exactly end. Lerp stands for linear interpolation, something like this ( end-start )*step + start. If you put in 0.5, you will get the middle of both values. If you put in 1, you will go to the end straight away, not matter far it is.


The same thing is happening in your code, the currentHeading will become very close to targetHeading, but never exactly equal to targetHeading; when you use Vector3.Lerp, at the beginning the transition is quite fast, but it will slow-down or smooth down soon after because of the reason mentioned in the section above this. So, instead of a linear motion, you will get something like a ease motion ( faster in the beginning, slower in the end to ease out the transition )