Waypoint / Yield help

Hi guys

Basically Im trying to make a object go from waypoint[0] to waypoint[1] and when it reaches both its meant to pause for 5 seconds using yield. But instead it carries on in a loop. Ive tried everything I can think of, no idea what to do.

I based my code off of this guys http://answers.unity3d.com/questions/220547/waypointanimation-help.html if that helps.

 var waypoint : Transform[];
 var speed : float = 4;
 private var currentWaypoint : int;
 var test;
 
 
 function Start() 
 {            
     StartCoroutine("CoStart");
 }
 
 function CoStart() : IEnumerator
 {
     while (true)
         yield CoUpdate();
 }
 
 function CoUpdate () : IEnumerator
 {
     
     if(currentWaypoint < waypoint.length)
         {
             var WPtarget : Vector3 = waypoint[currentWaypoint].position;
             var moveDirection : Vector3 = WPtarget - transform.position;
             var velocity = rigidbody.velocity;
              
              
              
             if(moveDirection.magnitude < 1)
             {                                  
                 test = true; 
                 speed = 0;
                 if(test)
                 {
                     WaitFiveSeconds();
                     currentWaypoint++;
                     
                 }      
             }             
             else
             {
                    if(!test)
                    {
                        speed = 4; 
                       
                    }
             }
             if (currentWaypoint > 1)
             {                
                    currentWaypoint = 0;
             } 
         velocity = moveDirection.normalized*speed; 
         rigidbody.velocity = velocity;        
         }
 }
         
 function WaitFiveSeconds() 
 {
     
     WaitForSeconds(5);         
     test = false;    
 }

Like I said, Ive tried everything I can think off, maybe you guys can help :)