Revision 1820 branches/16299_s10/matlab/evolveRobots.m
| evolveRobots.m (revision 1820) | ||
|---|---|---|
| 21 | 21 |
%%% output options |
| 22 | 22 |
makeMovie = false; |
| 23 | 23 |
showPlots = true; |
| 24 |
doFeedback = true; |
|
| 24 |
doFeedback = false; |
|
| 25 | 25 |
shape = 4; |
| 26 | 26 |
|
| 27 | 27 |
if shape == 1 |
| ... | ... | |
| 55 | 55 |
if doFeedback == false |
| 56 | 56 |
% if we aren't doing feedback just pick random speeds for everyone so |
| 57 | 57 |
% we can see some open loop motion |
| 58 |
for i=1:n |
|
| 58 |
%HACK |
|
| 59 |
V(2,:) = 2; |
|
| 60 |
for i=3:n |
|
| 59 | 61 |
X(i,1) = rand*10 - 5; |
| 60 | 62 |
Y(i,1) = rand*10 - 5; |
| 61 | 63 |
V(i,:) = rand*10; |
| ... | ... | |
| 156 | 158 |
|
| 157 | 159 |
color = hsv(n); |
| 158 | 160 |
|
| 159 |
encoderNoise = .01 * randn(2, n); |
|
| 161 |
encoderNoise = 1 * randn(2, n); |
|
| 160 | 162 |
|
| 161 | 163 |
|
| 162 | 164 |
% The things that end in s are the sensor values |
| ... | ... | |
| 190 | 192 |
xoldSensor = 0; |
| 191 | 193 |
yoldSensor = 0; |
| 192 | 194 |
thetaoldSensor = 0; |
| 195 |
noise = 0; |
|
| 193 | 196 |
|
| 194 | 197 |
% Run through each timestep |
| 195 | 198 |
for t = 0:dt:tf |
| ... | ... | |
| 202 | 205 |
%Phi |
| 203 | 206 |
|
| 204 | 207 |
% Update the sensor values using the sensor model |
| 205 |
[Xs(:,idx), Ys(:,idx), Thetas(:,idx), Phis(:,idx), sensorState, encoderNoise, xoldSensor, yoldSensor, thetaoldSensor] = ... |
|
| 206 |
sensorModel(X(:,idx), Y(:,idx), Theta(:,idx), Phi(:,idx), sensorState, n, encoderNoise, wheels, xoldSensor, yoldSensor, thetaoldSensor, dt); |
|
| 208 |
[Xs(:,idx), Ys(:,idx), Thetas(:,idx), Phis(:,idx), sensorState, encoderNoise, xoldSensor, yoldSensor, thetaoldSensor, noise] = ... |
|
| 209 |
sensorModel(X(:,idx), Y(:,idx), Theta(:,idx), Phi(:,idx), sensorState, n, encoderNoise, wheels, xoldSensor, yoldSensor, thetaoldSensor, dt, noise); |
|
| 207 | 210 |
%Phis |
| 208 | 211 |
|
| 209 | 212 |
% visualize real position |
| ... | ... | |
| 230 | 233 |
figure; |
| 231 | 234 |
hold on; |
| 232 | 235 |
title('true X vs. sensed X');
|
| 233 |
plot(1:idx-1, X(1,:),1:idx-1, Xs(1,:)) |
|
| 236 |
% plot(1:idx-1, X(1,:),1:idx-1, Xs(1,:)) |
|
| 234 | 237 |
plot(1:idx-1, X(2,:),1:idx-1, Xs(2,:)) |
| 235 | 238 |
end |
| 236 | 239 |
|
Also available in: Unified diff