root / branches / 16299_s10 / matlab / motionModel.m @ 1802
History | View | Annotate | Download (857 Bytes)
1 | 1712 | bbland | function [x, y, theta, r, phi, state] = motionModel(v,omega,xold,yold,thetaold,dt,state) |
---|---|---|---|
2 | 1708 | bneuman | |
3 | 1713 | bneuman | [numRobots,z] = size(v); |
4 | 1708 | bneuman | |
5 | 1795 | cbrownel | wheelMax = [100; 100]; |
6 | wheelMin = [-100; -100]; |
||
7 | |||
8 | R = 2.5; |
||
9 | L = 12; |
||
10 | |||
11 | transform = [ R/2 R/2; -R/L R/L]; |
||
12 | inverseTransform = inv(transform); |
||
13 | |||
14 | for i=1:numRobots, |
||
15 | |||
16 | 1802 | bneuman | wheels = inverseTransform * [v(i); omega(i)]; |
17 | wheels = min(wheels, wheelMax); |
||
18 | 1797 | bneuman | wheels = max(wheels, wheelMin); |
19 | q = transform * wheels; |
||
20 | 1795 | cbrownel | v(i) = q(1); |
21 | omega(i) = q(2); |
||
22 | |||
23 | end |
||
24 | |||
25 | |||
26 | 1712 | bbland | x = zeros(numRobots,1); |
27 | y = zeros(numRobots,1); |
||
28 | theta = zeros(numRobots,1); |
||
29 | r = zeros(numRobots,1); |
||
30 | phi = zeros(numRobots,1); |
||
31 | |||
32 | for i=1:numRobots, |
||
33 | |||
34 | x(i) = xold(i) + cos(thetaold(i))*v(i)*dt; |
||
35 | y(i) = yold(i) + sin(thetaold(i))*v(i)*dt; |
||
36 | theta(i) = thetaold(i) + omega(i)*dt; |
||
37 | |||
38 | r(i) = sqrt((x(i)-x(1))^2 + (y(i)-y(1))^2); |
||
39 | 1713 | bneuman | phi(i) = theta(1) - atan2(y(i)-y(1), x(i)-x(1)); |
40 | 1712 | bbland | |
41 | 1708 | bneuman | end |
42 | 1712 | bbland | |
43 | end |