root / branches / colonetmk2 / code / projects / swarm / BOMPosRender / src / BOMPosServer.java @ 1456
History | View | Annotate | Download (5.39 KB)
1 | 1456 | rcahoon | import java.awt.*; |
---|---|---|---|
2 | import java.io.*; |
||
3 | import java.net.*; |
||
4 | import java.util.Arrays; |
||
5 | import java.util.Scanner; |
||
6 | import java.util.Vector; |
||
7 | |||
8 | import javax.swing.JApplet; |
||
9 | |||
10 | public class BOMPosServer extends JApplet implements Runnable { |
||
11 | private static final long serialVersionUID = 0L; |
||
12 | |||
13 | private boolean running; |
||
14 | |||
15 | class Client implements Runnable { |
||
16 | final Socket socket; |
||
17 | Scanner stream;
|
||
18 | |||
19 | public Client(Socket s) throws IOException |
||
20 | { |
||
21 | this.socket = s;
|
||
22 | this.stream = new Scanner(this.socket.getInputStream()); |
||
23 | new Thread(this).run(); |
||
24 | } |
||
25 | |||
26 | public void close() |
||
27 | { |
||
28 | try {
|
||
29 | socket.close(); |
||
30 | } catch (IOException e) { |
||
31 | e.printStackTrace(); |
||
32 | } |
||
33 | } |
||
34 | |||
35 | public void run() |
||
36 | { |
||
37 | System.out.println("Client connected"); |
||
38 | try {
|
||
39 | SensorReading[] readings = new SensorReading[16]; |
||
40 | int robot = -1; |
||
41 | int source, dest = -1; |
||
42 | while(this.socket.isConnected()) |
||
43 | { |
||
44 | while(!"st".equals(stream.next())) |
||
45 | System.out.println("bad token"); |
||
46 | source = stream.nextInt(); |
||
47 | int destp = stream.nextInt();
|
||
48 | if (dest >= destp || robot != source)
|
||
49 | { |
||
50 | if (robot > 0) |
||
51 | { |
||
52 | updateMap(robot, readings); |
||
53 | BOMPosServer.this.repaint(); |
||
54 | } |
||
55 | Arrays.fill(readings, null); |
||
56 | robot = source; |
||
57 | } |
||
58 | dest = destp; |
||
59 | float dir = stream.nextInt()*2.0f*(float)Math.PI/160.0f; |
||
60 | int dist = stream.nextInt();
|
||
61 | //System.out.print(source + " " + dest + " " + dir + " " + dist + "\n");
|
||
62 | if (dir > Math.PI*2) |
||
63 | readings[dest] = null;
|
||
64 | else
|
||
65 | readings[dest] = new SensorReading(dir, dist);
|
||
66 | BOMPosServer.this.readings[source][dest] = readings[dest]; |
||
67 | } |
||
68 | } catch (Exception e) { |
||
69 | e.printStackTrace(); |
||
70 | try {
|
||
71 | this.socket.close();
|
||
72 | } catch (IOException ex) {} |
||
73 | } |
||
74 | System.out.println("Client exit"); |
||
75 | } |
||
76 | } |
||
77 | |||
78 | ServerSocket server;
|
||
79 | Vector<Client> clients;
|
||
80 | |||
81 | @Override
|
||
82 | public void run() { |
||
83 | try {
|
||
84 | /*while(!server.isClosed())
|
||
85 | {
|
||
86 | Socket s = server.accept();
|
||
87 | clients.add(new Client(s));
|
||
88 | }*/
|
||
89 | while (running)
|
||
90 | { |
||
91 | new Client(new Socket("localhost", 2000)).run(); |
||
92 | } |
||
93 | } catch (IOException e) { |
||
94 | e.printStackTrace(); |
||
95 | } |
||
96 | /*positions[0].th = 0;
|
||
97 | float a = 0.0f;
|
||
98 | while(running)
|
||
99 | {
|
||
100 | repaint();
|
||
101 | readings[0][1] = new SensorReading(a, 2);
|
||
102 | a += Math.PI/20;
|
||
103 | try {
|
||
104 | Thread.sleep(50);
|
||
105 | } catch (InterruptedException e) {}
|
||
106 | }*/
|
||
107 | } |
||
108 | |||
109 | @Override
|
||
110 | public void init() { |
||
111 | setSize(400, 400); |
||
112 | |||
113 | clients = new Vector<Client>(); |
||
114 | |||
115 | /*try {
|
||
116 | server = new ServerSocket(2000);
|
||
117 | } catch (IOException e) {
|
||
118 | e.printStackTrace();
|
||
119 | }*/
|
||
120 | |||
121 | running = true;
|
||
122 | |||
123 | new Thread(this).start(); |
||
124 | } |
||
125 | |||
126 | @Override
|
||
127 | public void stop() { |
||
128 | for(Client c : clients) {
|
||
129 | c.close(); |
||
130 | } |
||
131 | } |
||
132 | |||
133 | @Override
|
||
134 | public void paint(Graphics g) |
||
135 | { |
||
136 | g.clearRect(0, 0, getWidth(), getHeight()); |
||
137 | |||
138 | for(int i=0; i<16; i++) |
||
139 | { |
||
140 | if (Float.isNaN(positions[i].th)) continue; |
||
141 | //System.out.println(i + ": (" + positions[i].x + "," + positions[i].y + ") " + positions[i].th);
|
||
142 | drawBot(g, i); |
||
143 | } |
||
144 | } |
||
145 | |||
146 | private void drawBot(Graphics g, int i) { |
||
147 | int x = 3*positions[i].x + getWidth()/2; |
||
148 | int y = -3*positions[i].y + getHeight()/2; |
||
149 | g.drawOval(x-8, y-8, 16, 16); |
||
150 | g.drawString(String.valueOf(i), x-4, y+4); |
||
151 | for(int j=0; j<16; j++) |
||
152 | { |
||
153 | if (readings[i][j]==null) continue; |
||
154 | float ang = readings[i][j].dir + positions[i].th;
|
||
155 | g.drawString(String.valueOf(j), x+(int)(12*Math.cos(ang)-4), y-(int)(14*Math.sin(ang)-5)); |
||
156 | } |
||
157 | if (!Float.isNaN(positions[i].th)) |
||
158 | { |
||
159 | g.drawLine(x, y, (int)(x + 8*Math.cos(positions[i].th)), (int)(y - 8*Math.sin(positions[i].th))); |
||
160 | } |
||
161 | } |
||
162 | |||
163 | public int get_num_robots() |
||
164 | { |
||
165 | return 3; |
||
166 | } |
||
167 | |||
168 | static class SensorReading |
||
169 | { |
||
170 | public float dir; |
||
171 | public int dist; |
||
172 | |||
173 | public SensorReading(float dir, int dist) |
||
174 | { |
||
175 | this.dir = dir;
|
||
176 | this.dist = dist;
|
||
177 | } |
||
178 | } |
||
179 | |||
180 | SensorReading[][] readings = new SensorReading[16][16]; |
||
181 | |||
182 | static class R2 |
||
183 | { |
||
184 | public int x; |
||
185 | public int y; |
||
186 | public float th; |
||
187 | |||
188 | public R2(int x, int y, float th) |
||
189 | { |
||
190 | this.x = x;
|
||
191 | this.y = y;
|
||
192 | this.th = th;
|
||
193 | } |
||
194 | } |
||
195 | |||
196 | static R2[] positions; |
||
197 | static {
|
||
198 | positions = new R2[16]; |
||
199 | for(int i=0; i<16; i++) |
||
200 | positions[i] = new R2(0, 0, Float.NaN); |
||
201 | } |
||
202 | |||
203 | public void updateMap(int robot, SensorReading[] readings) |
||
204 | { |
||
205 | float a = 0.0f; |
||
206 | int count = 0; |
||
207 | for(int target=0; target<16; target++) |
||
208 | { |
||
209 | if (readings[target] == null) continue; |
||
210 | |||
211 | a += (Math.atan2(positions[target].y - positions[robot].y, positions[target].x - positions[robot].x) - readings[target].dir + 4*Math.PI) % (2*Math.PI); |
||
212 | count++; |
||
213 | } |
||
214 | if (count > 0) |
||
215 | { |
||
216 | a /= count; |
||
217 | } |
||
218 | positions[robot].th = /*robot==1 ? 0 :*/ a;
|
||
219 | |||
220 | for(int target=0; target<16; target++) |
||
221 | { |
||
222 | if (readings[target] == null) continue; |
||
223 | if (readings[target].dist < 2) continue; |
||
224 | if (readings[target].dist > 200) continue; |
||
225 | /*System.out.println(target + " " + readings[target].dist + " " + readings[target].dir + " (" +
|
||
226 | readings[target].dist*Math.cos(readings[target].dir) + "," + readings[target].dist*Math.sin(readings[target].dir) + ")");*/
|
||
227 | int scale = 2 * get_num_robots(); // readings[target].dist/5; |
||
228 | positions[target].x = ( (scale-1)*positions[target].x + (int)(readings[target].dist*Math.cos(positions[robot].th + readings[target].dir)) )/scale; |
||
229 | positions[target].y = ( (scale-1)*positions[target].y + (int)(readings[target].dist*Math.sin(positions[robot].th + readings[target].dir)) )/scale; |
||
230 | } |
||
231 | |||
232 | System.out.println(robot + ": (" + positions[robot].x + "," + positions[robot].y + ") " + positions[robot].th); |
||
233 | } |
||
234 | } |