Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- x1=linspace(0,10,100); //x1 and y1 represent the leader's path
- y1=sin(x1);
- plot(x1(1:100),y1(1:100));
- hold on;
- x2=zeros(1,100);
- y2=zeros(1,100);
- x2(1)=-5; //x2, y2 represent the follower's position
- x2(2)=-4;
- landa=0.1; //represents the euclidean distance between robots
- theta_leader(2)=atan((y1(2)-y1(1))/(x1(2)-x1(1)));
- theta_follower(2)=atan((y2(2)-y2(1))/(x2(2)-x2(1)));
- alfa(2)=atan((y1(2)-y2(2))/(x1(2)-x2(2)))-theta_follower(2);
- phi(2)=pi-(theta_leader(2) - alfa(2) - theta_follower(2));
- for i=3:100
- landa(i)=0.1;
- x2(i)=x1(i)*cos(theta_leader(i-1))-landa(i)*cos(alfa(i-1)+theta_follower(i-1));
- y2(i)=y1(i)*sin(theta_leader(i-1))-landa(i)*sin(alfa(i-1)+theta_follower(i-1));
- theta_leader(i)=atan((y1(i)-y1(i-1))/(x1(i)-x1(i-1)));
- alfa(i)=atan((y1(i)-y2(i))/(x1(i)-x2(i)))-theta_follower(i-1);
- phi(i)=pi-(theta_leader(i) - alfa(i) - theta_follower(i-1));
- theta_follower(i)=phi(i)-alfa(i)+theta_leader(i)-3.1415;
- end
- plot(x2,y2,'or');
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement