Revision 1797 branches/16299_s10/matlab/computeTrajectories.m

computeTrajectories.m (revision 1797)
1 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
2 3 
function [V,W] = computeTrajectories(X,Y,Theta,XD,YD,ThetaD)
3 4 

  		
  




  4
  5
    
    
Kp = 5;


  		
  




......




  6
  7
    
    
% how far away are we


  		
  




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


  		
  




  8
  9
    
    

  		
  




  
  10
    
    

  		
  




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


  		
  




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


  		
  




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


  		
  












Also available in:
	Unified diff