Revision 1795
Wheel speed limiting
motionModel.m | ||
---|---|---|
2 | 2 |
|
3 | 3 |
[numRobots,z] = size(v); |
4 | 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 |
|
|
5 | 26 |
x = zeros(numRobots,1); |
6 | 27 |
y = zeros(numRobots,1); |
7 | 28 |
theta = zeros(numRobots,1); |
Also available in: Unified diff