P1 = [50,0,0]';
P2 = [50,30,0]';
P3 = [20,30,0]';
P4 = [20,0,0]';
  
Q1 = MoveTo(P1, P2, 3);
Q2 = MoveTo(P2, P3, 3);
Q3 = MoveTo(P3, P4, 3);
Q4 = MoveTo(P4, P1, 3);

Q = [Q1, Q2, Q3, Q4];

pause(1)

t = [1:length(Q)]' * 0.01;
clf
plot(t,Q)