## root / branches / 16299_s10 / matlab / computeTrajectories.m @ 1812

History | View | Annotate | Download (583 Bytes)

1 |
% For now I am ignoring ThetaD and just computing an arc |
---|---|

2 |
% This function expects to NOT have the quees be at index 1 |

3 |
function [V,W] = computeTrajectories(X,Y,Theta,XD,YD,ThetaD) |

4 | |

5 |
Kp = 7; |

6 | |

7 |
% how far away are we |

8 |
dists = sqrt((X-XD).^2 + (Y-YD) .^ 2); |

9 | |

10 | |

11 |
for idx=1:size(X,1) |

12 |
%V(idx) = dists(idx)/Kp; % linear velocity is proportional to distance |

13 |
%W(idx) = (ThetaD(idx) - Theta(idx))/Kp; |

14 |
V(idx) = (cos(Theta(idx))*(XD(idx) - X(idx)) + sin(Theta(idx))*(YD(idx) - Y(idx)))/Kp; |

15 |
W(idx) = (-sin(Theta(idx))*(XD(idx) - X(idx)) + cos(Theta(idx))*(YD(idx) - Y(idx)))/Kp; |

16 |
end |

17 | |

18 | |

19 |
end |