root / branches / 16299_s10 / matlab / motionModel.m @ 1795
History | View | Annotate | Download (850 Bytes)
1 |
function [x, y, theta, r, phi, state] = motionModel(v,omega,xold,yold,thetaold,dt,state) |
---|---|
2 |
|
3 |
[numRobots,z] = size(v); |
4 |
|
5 |
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 |
wheels = inverseTransform * [v(i); omega(i)]; |
17 |
wheels = min(wheels, wheelMax); |
18 |
wheels = max(wheels, wheelMin) |
19 |
q = transform * wheels |
20 |
v(i) = q(1); |
21 |
omega(i) = q(2); |
22 |
|
23 |
end |
24 |
|
25 |
|
26 |
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 |
phi(i) = theta(1) - atan2(y(i)-y(1), x(i)-x(1)); |
40 |
|
41 |
end |
42 |
|
43 |
end |