Advertisement
Guest User

Untitled

a guest
Oct 27th, 2016
53
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 0.97 KB | None | 0 0
  1. x1=linspace(0,10,100); //x1 and y1 represent the leader's path
  2. y1=sin(x1);
  3. plot(x1(1:100),y1(1:100));
  4. hold on;
  5.  
  6. x2=zeros(1,100);
  7. y2=zeros(1,100);
  8.  
  9. x2(1)=-5; //x2, y2 represent the follower's position
  10. x2(2)=-4;
  11. landa=0.1; //represents the euclidean distance between robots
  12.  
  13. theta_leader(2)=atan((y1(2)-y1(1))/(x1(2)-x1(1)));
  14. theta_follower(2)=atan((y2(2)-y2(1))/(x2(2)-x2(1)));
  15. alfa(2)=atan((y1(2)-y2(2))/(x1(2)-x2(2)))-theta_follower(2);
  16. phi(2)=pi-(theta_leader(2) - alfa(2) - theta_follower(2));
  17.  
  18. for i=3:100
  19. landa(i)=0.1;
  20. x2(i)=x1(i)*cos(theta_leader(i-1))-landa(i)*cos(alfa(i-1)+theta_follower(i-1));
  21. y2(i)=y1(i)*sin(theta_leader(i-1))-landa(i)*sin(alfa(i-1)+theta_follower(i-1));
  22.  
  23. theta_leader(i)=atan((y1(i)-y1(i-1))/(x1(i)-x1(i-1)));
  24.  
  25. alfa(i)=atan((y1(i)-y2(i))/(x1(i)-x2(i)))-theta_follower(i-1);
  26. phi(i)=pi-(theta_leader(i) - alfa(i) - theta_follower(i-1));
  27. theta_follower(i)=phi(i)-alfa(i)+theta_leader(i)-3.1415;
  28. end
  29.  
  30. plot(x2,y2,'or');
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement