Project

General

Profile

Revision 1821

Added by Chase Brownell almost 14 years ago

Individual Wheel Speeds

View differences:

motionModel.m
11 11 

  		




  12
  12
  
    
transform = [ R/2 R/2; -R/L R/L];


  		




  13
  13
  
    
inverseTransform = inv(transform);


  		




  
  14
  
    
wheels = zeros(2, numRobots);


  		




  14
  15
  
    

  		




  15
  16
  
    
%Transforms from v, omega into wheel speed, limits wheel speed and adds noise, then translates back.


  		




  16
  17
  
    
for i=2:numRobots


  		




  17
  18
  
    
    


  		




  18
  
  
    
    wheels = inverseTransform * [v(i); omega(i)] + noise * randn(2,1);


  		




  19
  
  
    
    wheels = min(wheels, wheelMax); 


  		




  20
  
  
    
	wheels = max(wheels, wheelMin);


  		




  21
  
  
    
	q = transform * wheels;


  		




  
  19
  
    
    wheels(:, i) = inverseTransform * [v(i); omega(i)] + noise * randn(2,1);


  		




  
  20
  
    
    wheels(:, i) = min(wheels(:, i), wheelMax); 


  		




  
  21
  
    
	wheels(:, i) = max(wheels(:, i), wheelMin);


  		




  
  22
  
    
	q = transform * wheels(:, i);


  		




  22
  23
  
    
	v(i) = q(1);


  		




  23
  24
  
    
	omega(i) = q(2);


  		




  24
  25
  
    
	


  		




  ......




  34
  35
  
    
x = xold + cos(thetaold).*v*dt;


  		




  35
  36
  
    
y = yold + sin(thetaold).*v*dt;


  		




  36
  37
  
    
theta = thetaold + omega*dt;


  		




  
  38
  
    

  		




  
  39
  
    
disp(x)


  		




  37
  40
  
    
    


  		




  38
  41
  
    
phi = theta - atan2(y-y(1), x-x(1));


  		




  39
  42
  
    

  		












Also available in:  Unified diff