Revision 1821
Individual Wheel Speeds
| branches/16299_s10/matlab/visualizeRobots.m | ||
|---|---|---|
| 26 | 26 |
axis manual |
| 27 | 27 |
set( gca,'nextplot','add' ); |
| 28 | 28 |
for j = 1:n |
| 29 |
%h = plot_arrow( Xs(j,idx),Ys(j,idx),Xs(j,idx) + Speeds(j, idx-1) * arrowScale * cos(Thetas(j,idx)),Ys(j,idx) + Speeds(j, idx-1) * arrowScale * sin(Thetas(j,idx)),...
|
|
| 30 |
h = plot_arrow( Xs(j,idx),Ys(j,idx),Xs(j,idx) + cos(Thetas(j,idx)),Ys(j,idx) + sin(Thetas(j,idx)),...
|
|
| 31 |
'color',color(j,:),'facecolor',color(j,:),'edgecolor',color(j,:) ); |
|
| 29 |
h = plot_arrow( Xs(j,idx),Ys(j,idx),Xs(j,idx) + Speeds(j, idx-1) * arrowScale * cos(Thetas(j,idx)),Ys(j,idx) + Speeds(j, idx-1) * arrowScale * sin(Thetas(j,idx)),'color',color(j,:),'facecolor',color(j,:),'edgecolor',color(j,:) );
|
|
| 30 |
%h = plot_arrow( Xs(j,idx),Ys(j,idx),Xs(j,idx) + cos(Thetas(j,idx)),Ys(j,idx) + sin(Thetas(j,idx)),'color',color(j,:),'facecolor',color(j,:),'edgecolor',color(j,:) );
|
|
| 31 |
|
|
| 32 | 32 |
set( h,'linewidth',3 ); |
| 33 | 33 |
d = plot( Xs(j,idx),Ys(j,idx), '.k'); |
| 34 | 34 |
set( d,'MarkerSize',5 ); |
| branches/16299_s10/matlab/motionModel.m | ||
|---|---|---|
| 11 | 11 |
|
| 12 | 12 |
transform = [ R/2 R/2; -R/L R/L]; |
| 13 | 13 |
inverseTransform = inv(transform); |
| 14 |
wheels = zeros(2, numRobots); |
|
| 14 | 15 |
|
| 15 | 16 |
%Transforms from v, omega into wheel speed, limits wheel speed and adds noise, then translates back. |
| 16 | 17 |
for i=2:numRobots |
| 17 | 18 |
|
| 18 |
wheels = inverseTransform * [v(i); omega(i)] + noise * randn(2,1); |
|
| 19 |
wheels = min(wheels, wheelMax);
|
|
| 20 |
wheels = max(wheels, wheelMin);
|
|
| 21 |
q = transform * wheels; |
|
| 19 |
wheels(:, i) = inverseTransform * [v(i); omega(i)] + noise * randn(2,1);
|
|
| 20 |
wheels(:, i) = min(wheels(:, i), wheelMax);
|
|
| 21 |
wheels(:, i) = max(wheels(:, i), wheelMin);
|
|
| 22 |
q = transform * wheels(:, i);
|
|
| 22 | 23 |
v(i) = q(1); |
| 23 | 24 |
omega(i) = q(2); |
| 24 | 25 |
|
| ... | ... | |
| 34 | 35 |
x = xold + cos(thetaold).*v*dt; |
| 35 | 36 |
y = yold + sin(thetaold).*v*dt; |
| 36 | 37 |
theta = thetaold + omega*dt; |
| 38 |
|
|
| 39 |
disp(x) |
|
| 37 | 40 |
|
| 38 | 41 |
phi = theta - atan2(y-y(1), x-x(1)); |
| 39 | 42 |
|
| branches/16299_s10/matlab/evolveRobots.m | ||
|---|---|---|
| 5 | 5 |
dt = 0.041709; |
| 6 | 6 |
%else: |
| 7 | 7 |
%dt = .01; |
| 8 |
tf = 5.5 %40;
|
|
| 8 |
tf = 20 %40;
|
|
| 9 | 9 |
|
| 10 | 10 |
|
| 11 | 11 |
%shape numbers: |
| ... | ... | |
| 21 | 21 |
%%% output options |
| 22 | 22 |
makeMovie = false; |
| 23 | 23 |
showPlots = true; |
| 24 |
doFeedback = false;
|
|
| 25 |
shape = 4;
|
|
| 24 |
doFeedback = true;
|
|
| 25 |
shape = 1;
|
|
| 26 | 26 |
|
| 27 | 27 |
if shape == 1 |
| 28 | 28 |
n = 65; |
| ... | ... | |
| 136 | 136 |
|
| 137 | 137 |
V(1,end/2:3*end/4) = 1; |
| 138 | 138 |
%changing orientation of the queen |
| 139 |
W(1,floor(end/6):floor(end/6)+20) = pi/2; |
|
| 140 |
W(1,floor(end/3):floor(end/3)+20) = -1*pi/2; |
|
| 139 |
%W(1,floor(end/6):floor(end/6)+20) = pi/2;
|
|
| 140 |
%W(1,floor(end/3):floor(end/3)+20) = -1*pi/2;
|
|
| 141 | 141 |
% start robots at random spots |
| 142 | 142 |
%for i=2:n |
| 143 | 143 |
% X(i,1) = rand*15 - 7.5; |
| ... | ... | |
| 158 | 158 |
|
| 159 | 159 |
color = hsv(n); |
| 160 | 160 |
|
| 161 |
encoderNoise = 1 * randn(2, n);
|
|
| 161 |
encoderNoise = 3 * randn(2, n);
|
|
| 162 | 162 |
|
| 163 | 163 |
|
| 164 | 164 |
% The things that end in s are the sensor values |
| branches/16299_s10/matlab/sensorModel.m | ||
|---|---|---|
| 19 | 19 |
% TODO: model encoder error? |
| 20 | 20 |
% Use the lagged values for position |
| 21 | 21 |
|
| 22 |
noise = noise + encoderNoise .* (encoderNoiseVar * abs(randn(2, n))); |
|
| 23 | 22 |
|
| 23 |
noise = noise + (encoderNoise .* (encoderNoiseVar * abs(randn(2, n))) ./ wheels); |
|
| 24 | 24 |
|
| 25 |
|
|
| 25 | 26 |
R = 3.5; |
| 26 | 27 |
L = 12.75; |
| 27 | 28 |
|
| 28 | 29 |
transform = [ R/2 R/2; -R/L R/L]; |
| 29 | 30 |
|
| 31 |
|
|
| 32 |
|
|
| 30 | 33 |
for i=2:n, |
| 31 | 34 |
|
| 32 |
wheels = wheels + noise(:, i); |
|
| 33 |
q = transform * wheels; |
|
| 35 |
|
|
| 36 |
wheels(:, i) = wheels(:, i) + noise(:, i) ; |
|
| 37 |
q = transform * wheels(:, i); |
|
| 34 | 38 |
v(i) = q(1); |
| 35 | 39 |
omega(i) = q(2); |
| 40 |
% disp(wheels) |
|
| 36 | 41 |
|
| 37 | 42 |
end |
| 38 | 43 |
|
| 44 |
disp(wheels) |
|
| 45 |
|
|
| 39 | 46 |
xSensor = xoldSensed; |
| 40 | 47 |
ySensor = yoldSensed; |
| 41 | 48 |
thetaSensor = thetaoldSensed; |
| branches/16299_s10/latex/math.tex | ||
|---|---|---|
| 10 | 10 |
\item $x$ - true x position |
| 11 | 11 |
\item $\tilde{x}$ - sensed x
|
| 12 | 12 |
\item $\hat{x}$ - desired
|
| 13 |
\item $r$ - true r |
|
| 14 |
\item $\tilde{r}$ - sensed r
|
|
| 15 |
\item $\hat{r}$ - desired
|
|
| 16 |
\item $\phi$ - true phi position |
|
| 17 |
\item $\tilde{\phi}$ - sensed phi
|
|
| 18 |
\item $\hat{\phi}$ - desired
|
|
| 19 |
\item $y$ - true y position |
|
| 20 |
\item $\tilde{y}$ - sensed y
|
|
| 21 |
\item $\hat{y}$ - desired
|
|
| 22 |
|
|
| 23 |
\item $(\hat{r}, \hat{\phi}) ^ n$
|
|
| 24 |
\item $(\hat{x}, \hat{y}) ^ n$
|
|
| 25 |
\item $(\hat{v}, \hat{\omega}) ^ n$
|
|
| 26 |
\item $(\tilde{v}, \tilde{\omega}) ^ n$
|
|
| 27 |
\item $(\tilde{x}, \tilde{y}, \tilde{\Theta}, \tilde{\phi}) ^ n$
|
|
| 28 |
\item $(\hat{\omega_l}, \hat{\omega_r}) ^ n$
|
|
| 29 |
\item $({\omega_l}, {\omega_r}) ^ n$
|
|
| 30 |
\item $(\tilde{\omega_l}, \tilde{\omega_r}) ^ n$
|
|
| 31 |
\item $(\tilde{x}, \tilde{y}, \tilde{\Theta}) ^ n$
|
|
| 32 |
|
|
| 33 |
\item $(\tilde{x}, \tilde{y}, \tilde{\phi}) ^ n$
|
|
| 34 |
\item $\phi ^ n$ |
|
| 35 |
\item $\tilde{\phi} ^ n$
|
|
| 13 | 36 |
\end{itemize}
|
| 14 | 37 |
|
| 15 | 38 |
|
| 39 |
|
|
| 40 |
|
|
| 16 | 41 |
\textbf{motion model}
|
| 17 | 42 |
|
| 18 | 43 |
\[ |
Also available in: Unified diff