root / branches / 16299_s10 / matlab / evolveRobots.m @ 1812
History | View | Annotate | Download (2.95 KB)
1 | 1797 | bneuman | % n robots |
---|---|---|---|
2 | 1713 | bneuman | % Robot #1 is the queen |
3 | 1812 | cbrownel | n = 12; |
4 | %for movie output use: |
||
5 | dt = 0.041709; |
||
6 | %else: |
||
7 | %dt = .01; |
||
8 | 1797 | bneuman | tf = 40; |
9 | 1708 | bneuman | |
10 | 1716 | bneuman | numsteps = ceil(tf / dt) + 1; |
11 | |||
12 | %%% output options |
||
13 | 1812 | cbrownel | makeMovie = false; |
14 | 1747 | bneuman | showPlots = false; |
15 | 1746 | bneuman | doFeedback = true; |
16 | 1716 | bneuman | |
17 | |||
18 | X = zeros(n,numsteps); %cm |
||
19 | Y = zeros(n,numsteps); %cm |
||
20 | R = zeros(n,numsteps); |
||
21 | Theta = zeros(n,numsteps); %rads |
||
22 | Phi = zeros(n,numsteps); %rads |
||
23 | V = zeros(n,numsteps); %0-255 |
||
24 | W = zeros(n,numsteps); %0-255 |
||
25 | 1713 | bneuman | |
26 | 1746 | bneuman | idx = 2; |
27 | 1716 | bneuman | |
28 | 1715 | llinne | %W(1,1) = pi; |
29 | %X(2,1) = 2.3; |
||
30 | %Y(2,1) = 5.7; |
||
31 | 1708 | bneuman | |
32 | 1715 | llinne | |
33 | 1746 | bneuman | desiredR = zeros(n,1); |
34 | desiredPhi = zeros(n,1); |
||
35 | 1715 | llinne | |
36 | 1746 | bneuman | if doFeedback == false |
37 | % if we aren't doing feedback just pick random speeds for everyone so |
||
38 | % we can see some open loop motion |
||
39 | for i=1:n |
||
40 | 1812 | cbrownel | X(i,1) = rand*10 - 5; |
41 | Y(i,1) = rand*10 - 5; |
||
42 | V(i,:) = rand*10; |
||
43 | W(i,:) = rand*2*pi; |
||
44 | 1746 | bneuman | end |
45 | else |
||
46 | 1812 | cbrownel | desiredR(:) = 20; |
47 | 1746 | bneuman | desiredPhi = (0:(n-1))*(2*pi/(n-1)); |
48 | 1797 | bneuman | |
49 | 1812 | cbrownel | %V(1,1:end/2) = 1; |
50 | 1797 | bneuman | % start robots at random spots |
51 | 1812 | cbrownel | for i=2:n |
52 | X(i,1) = rand*15 - 7.5; |
||
53 | Y(i,1) = rand*15 - 7.5; |
||
54 | end |
||
55 | 1797 | bneuman | |
56 | |||
57 | 1715 | llinne | end |
58 | |||
59 | 1746 | bneuman | % V(2,:) = 5; |
60 | % V(3,:) = -10; |
||
61 | % V(4,:) = 5; |
||
62 | % |
||
63 | % W(2,:) = pi; |
||
64 | % W(3,:) = 0; |
||
65 | % W(4,:) = -pi; |
||
66 | 1715 | llinne | |
67 | |||
68 | 1746 | bneuman | color = jet(n); |
69 | |||
70 | |||
71 | |||
72 | 1708 | bneuman | % The things that end in s are the sensor values |
73 | 1716 | bneuman | Xs = zeros(n,numsteps); |
74 | Ys = zeros(n,numsteps); |
||
75 | Thetas = zeros(n,numsteps); |
||
76 | Phis = zeros(n,numsteps); |
||
77 | 1708 | bneuman | |
78 | 1746 | bneuman | % These are the desired values |
79 | XD = zeros(n,numsteps); |
||
80 | YD = zeros(n,numsteps); |
||
81 | ThetaD = zeros(n,numsteps); |
||
82 | |||
83 | 1709 | bneuman | % These state variables allow the models to maintain some state across calls |
84 | motorState = []; |
||
85 | sensorState = []; |
||
86 | |||
87 | 1716 | bneuman | f = figure; |
88 | 1747 | bneuman | set(f,'NextPlot','replacechildren'); |
89 | winsize = get(f,'Position'); |
||
90 | winsize(1:2) = [0 0]; |
||
91 | 1716 | bneuman | |
92 | 1812 | cbrownel | mov = moviein(numsteps+1,f,winsize); |
93 | mov(:,1) = getframe(f,winsize); |
||
94 | 1747 | bneuman | |
95 | 1708 | bneuman | % Run through each timestep |
96 | for t = 0:dt:tf |
||
97 | 1716 | bneuman | |
98 | 1708 | bneuman | % update the true positions using the motor model |
99 | 1716 | bneuman | [X(:,idx), Y(:,idx), Theta(:,idx), R(:,idx), Phi(:,idx), motorState] = ... |
100 | 1746 | bneuman | motionModel(V(:,idx-1), W(:,idx-1), X(:,idx-1), Y(:,idx-1), Theta(:,idx-1), dt, motorState); |
101 | 1714 | bneuman | %Phi |
102 | 1708 | bneuman | |
103 | % Update the sensor values using the sensor model |
||
104 | 1716 | bneuman | [Xs(:,idx), Ys(:,idx), Thetas(:,idx), Phis(:,idx), sensorState] = ... |
105 | sensorModel(X(:,idx), Y(:,idx), Theta(:,idx), Phi(:,idx), sensorState); |
||
106 | 1714 | bneuman | %Phis |
107 | 1715 | llinne | |
108 | 1797 | bneuman | % visualize real position |
109 | 1812 | cbrownel | mov = visualizeRobots(f,n,X,Y,Theta,V,idx,color,mov,winsize); |
110 | 1747 | bneuman | |
111 | 1715 | llinne | |
112 | 1746 | bneuman | if doFeedback == true |
113 | 1794 | llinne | [XD(:,idx), YD(:,idx), ThetaD(:,idx)] = desiredPosition(Xs(:,idx),Ys(:,idx),Phis(:,idx),desiredR,desiredPhi); |
114 | 1716 | bneuman | |
115 | 1746 | bneuman | % ThetaD(:,idx) |
116 | |||
117 | 1797 | bneuman | % don't compute for the queen |
118 | [V(2:end,idx),W(2:end,idx)] = computeTrajectories(Xs(2:end,idx),Ys(2:end,idx),Thetas(2:end,idx),XD(2:end,idx),YD(2:end,idx),ThetaD(2:end,idx)); |
||
119 | 1746 | bneuman | end |
120 | |||
121 | disp(t); |
||
122 | |||
123 | %pause %(dt); |
||
124 | |||
125 | idx = idx + 1; |
||
126 | 1708 | bneuman | end |
127 | 1716 | bneuman | |
128 | if showPlots |
||
129 | figure; |
||
130 | hold on; |
||
131 | title('true X vs. sensed X'); |
||
132 | 1746 | bneuman | plot(1:idx-1, X(1,:),1:idx-1, Xs(1,:)) |
133 | plot(1:idx-1, X(2,:),1:idx-1, Xs(2,:)) |
||
134 | 1716 | bneuman | end |
135 | 1747 | bneuman | |
136 | if makeMovie |
||
137 | disp('making movie...'); |
||
138 | movie2avi(mov,'movie.avi'); |
||
139 | disp('movie.avi created!'); |
||
140 | end |