Revision 1812
Fixes wheel speed noise, prettier visualization with scaled arrows, and always displays plots
| motionModel.m | ||
|---|---|---|
| 2 | 2 |
|
| 3 | 3 |
[numRobots,z] = size(v); |
| 4 | 4 |
|
| 5 |
wheelMax = [100; 100];
|
|
| 6 |
wheelMin = [-100; -100];
|
|
| 5 |
wheelMax = [4 * pi;4 * pi];
|
|
| 6 |
wheelMin = [-4 * pi;-4 * pi];
|
|
| 7 | 7 |
|
| 8 |
R = 2.5;
|
|
| 9 |
L = 12; |
|
| 10 |
noise = .5; |
|
| 8 |
R = 3.5;
|
|
| 9 |
L = 12.75;
|
|
| 10 |
noise = 0.5;
|
|
| 11 | 11 |
|
| 12 | 12 |
transform = [ R/2 R/2; -R/L R/L]; |
| 13 | 13 |
inverseTransform = inv(transform); |
| 14 | 14 |
|
| 15 |
% Transforms from v, omega into wheel speed, limits wheel speed and adds noise, then translates back.
|
|
| 16 |
for i=1:numRobots,
|
|
| 15 |
%Transforms from v, omega into wheel speed, limits wheel speed and adds noise, then translates back. |
|
| 16 |
for i=2:numRobots,
|
|
| 17 | 17 |
|
| 18 | 18 |
wheels = inverseTransform * [v(i); omega(i)]; |
| 19 |
wheels = min(wheels, wheelMax) + noise * (rand - .5);
|
|
| 20 |
wheels = max(wheels, wheelMin) + noise * (rand - .5);
|
|
| 19 |
wheels = min(wheels, wheelMax) + noise * randn;
|
|
| 20 |
wheels = max(wheels, wheelMin) + noise * randn;
|
|
| 21 | 21 |
q = transform * wheels; |
| 22 | 22 |
v(i) = q(1); |
| 23 | 23 |
omega(i) = q(2); |
Also available in: Unified diff