Trying to make party members follow the leader in a top down RPG

So I have a top down rpg game, similar to games like Golden Sun, the early Final Fantasies, Chrono Trgiger, Mother, etc. and I’m trying to find a way for a gameobject (in this case party members) to follow the player around when he moves.

The thing is, I don’t want it to just follow the player, but to trace the player’s steps. So to follow the route the player took.

So far I’ve managed to do so, but the movement looks shaky a lot of the time. If anyone knows of a way to solve the shakiness, I’d appreciate it. Here’s the code I 'm using:

public GameObject target;
public List<Vector3> positions;
public int distance_permitted;
public float speed;

private Vector3 lastLeaderPosition;

// Use this for initialization
void Start () {
    positions.Add(target.transform.position);

}

// Update is called once per frame
void Update () {
    if (lastLeaderPosition != positions[positions.Count - 1])
    {
        positions.Add(target.transform.position);
    }        

    if (positions.Count >= distance_permitted)
    {
        if (gameObject.transform.position != positions[0])
        {
           
            transform.position = Vector3.MoveTowards(transform.position, positions[0], Time.deltaTime * speed);
     
        }
        else
        {
            positions.Remove(positions[0]);
            gameObject.GetComponent<Rigidbody2D>().velocity = new Vector2(0, 0);

        }
    }
    lastLeaderPosition = target.transform.position;
}

I’ve tried moving the object with lerp and smoothdamp, and with velocities applied to a rigidbody, but with lerp and smoothdamp it takes a lot of time to travel between waypoints, and with velocities it keeps overshooting the actual waypoint, and never actually reaches it, so it gets stuck on the first waypoint.

Hi. I had the same problem, but it seems that the follower only moves towards the next point if (gameObject.transform.position != positions[0]) is true which means that it will only move every other update if I’m not mistaken. So I just used transform.position again in the else case:

if (gameObject.transform.position != positions[0])
         {
            
             transform.position = Vector3.MoveTowards(transform.position, positions[0], Time.deltaTime * speed);
      
         }
         else
         {
             positions.Remove(positions[0]);
             gameObject.GetComponent<Rigidbody2D>().velocity = new Vector2(0, 0);
             transform.position = Vector3.MoveTowards(transform.position, positions[0], Time.deltaTime * speed);

         }
     }

I’m also using FixedUpdate to both the “player” script and the follower script.