root / trunk / code / projects / diagnostic_station / server / DiagnosticsServer.java @ 1606
History | View | Annotate | Download (23.1 KB)
1 |
import java.io.IOException; |
---|---|
2 |
import java.util.ArrayList; |
3 |
import java.util.Scanner; |
4 |
|
5 |
public class DiagnosticsServer { |
6 |
|
7 |
/*
|
8 |
* The following are used to differentiate between tests
|
9 |
*/
|
10 |
public static final int ALL_TESTS = 0; |
11 |
public static final int RANGEFINDER = 1; |
12 |
public static final int ENCODER = 2; |
13 |
public static final int MOTOR = 3; |
14 |
public static final int BOM = 4; |
15 |
public static final int BOM_EMITTER = 5; |
16 |
public static final int BOM_DETECTOR = 6; |
17 |
/*
|
18 |
* The number of different tests
|
19 |
*/
|
20 |
public static final int NUM_TESTS = 7; |
21 |
/*
|
22 |
* How many there are of each type
|
23 |
* For example, there are 16 BOM components and 2 motors
|
24 |
*/
|
25 |
public static final int NUM_BOMS = 16; |
26 |
public static final int NUM_RANGEFINDERS = 5; |
27 |
public static final int NUM_MOTORS = 2; |
28 |
public static final int NUM_ENCODERS = 2; |
29 |
/*
|
30 |
* Sometimes, we want to test all elements of a specific one
|
31 |
* For example, all five rangefinders instead of just one of them
|
32 |
*/
|
33 |
public static final int ALL_ELEMENTS = -1; |
34 |
/*
|
35 |
* Specifies the lowest possible number for a type
|
36 |
* For example, numbering starts at 0 for BOM components and 1 for motors
|
37 |
*/
|
38 |
public static final int RANGEFINDER_OFFSET = 1; |
39 |
public static final int ENCODER_OFFSET = 1; |
40 |
public static final int MOTOR_OFFSET = 1; |
41 |
public static final int BOM_OFFSET = 0; |
42 |
|
43 |
/*
|
44 |
* Station -> server message types
|
45 |
*/
|
46 |
private static final int PARSE_ERROR = 0; |
47 |
private static final int DATA = 1; |
48 |
private static final int COMMENT = 2; |
49 |
private static final int READY = 3; |
50 |
|
51 |
/*
|
52 |
* The following bitmasks are used to specify the status of a reading
|
53 |
* from the station
|
54 |
*/
|
55 |
// Emitter status
|
56 |
private static final int EMITTER = 1; |
57 |
private static final int DETECTOR = 2; |
58 |
private static final int EMITTER_TOP = 4; |
59 |
private static final int EMITTER_RIGHT = 8; |
60 |
private static final int EMITTER_LEFT = 16; |
61 |
private static final int DETECTOR_IN = 64; |
62 |
private static final int DETECTOR_OUT = 128; |
63 |
// Rangefinder status
|
64 |
private static final int RANGEFINDER_IN = 1; |
65 |
private static final int RANGEFINDER_OUT = 2; |
66 |
// Motor status
|
67 |
private static final int MOTOR_FORWARD = 1; |
68 |
private static final int MOTOR_BACKWARD = 2; |
69 |
private static final int MOTOR_ACCELERATING = 4; |
70 |
private static final int MOTOR_DECELERATING = 8; |
71 |
private static final int MOTOR_CONST_ACCEL = 16; |
72 |
// Encoder status
|
73 |
private static final int ENCODER_FORWARD = 1; |
74 |
private static final int ENCODER_BACKWARD = 2; |
75 |
|
76 |
/*
|
77 |
* The following arrays of ArrayLists are used to store data received
|
78 |
* from testing
|
79 |
* For example, the 0th element of motorPWMData stores an ArrayList of
|
80 |
* PWM information received from the first motor.
|
81 |
* The 0th element of motorStatus stores an ArrayList of the same length
|
82 |
* Each element is a bitmask with information such as the direction of
|
83 |
* acceleration and whether the motor was running forwards or backwards
|
84 |
*/
|
85 |
private ArrayList[] bomData = new ArrayList[NUM_BOMS]; |
86 |
private ArrayList[] bomStatus = new ArrayList[NUM_BOMS]; |
87 |
private ArrayList[] rangefinderWallData = new ArrayList[NUM_RANGEFINDERS]; |
88 |
private ArrayList[] rangefinderRangeData = new ArrayList[NUM_RANGEFINDERS]; |
89 |
private ArrayList[] rangefinderStatus = new ArrayList[NUM_RANGEFINDERS]; |
90 |
private ArrayList[] motorPWMData = new ArrayList[NUM_MOTORS]; |
91 |
private ArrayList[] motorVelocityData = new ArrayList[NUM_MOTORS]; |
92 |
private ArrayList[] motorStatus = new ArrayList[NUM_MOTORS]; |
93 |
private ArrayList[] encoderDynamoData = new ArrayList[NUM_ENCODERS]; |
94 |
private ArrayList[] encoderValueData = new ArrayList[NUM_ENCODERS]; |
95 |
private ArrayList[] encoderStatus = new ArrayList[NUM_ENCODERS]; |
96 |
|
97 |
// The SerialComm object controls the connection to the station
|
98 |
private SerialComm serial;
|
99 |
// We send information to the gui
|
100 |
private ServerGUI gui;
|
101 |
|
102 |
/*
|
103 |
* Initializes gui, serial, and data arrays
|
104 |
*/
|
105 |
public DiagnosticsServer(){
|
106 |
gui = new ServerGUI(this); |
107 |
try{
|
108 |
serial = new SerialComm(gui, "COM19", SerialComm.BAUDRATE_DEFAULT, |
109 |
SerialComm.DATABITS_DEFAULT, SerialComm.STOPBITS_2, |
110 |
SerialComm.PARITY_DEFAULT); |
111 |
} |
112 |
catch(IOException e){ |
113 |
System.out.println("Error connecting to serial"); |
114 |
System.out.println(e.getMessage());
|
115 |
System.out.println(e.getStackTrace());
|
116 |
return;
|
117 |
} |
118 |
for(int i=0; i<NUM_BOMS; i++){ |
119 |
bomData[i] = new ArrayList<Integer>(); |
120 |
bomStatus[i] = new ArrayList<Integer>(); |
121 |
} |
122 |
for(int i=0; i<NUM_RANGEFINDERS; i++){ |
123 |
rangefinderWallData[i] = new ArrayList<Integer>(); |
124 |
rangefinderRangeData[i] = new ArrayList<Integer>(); |
125 |
rangefinderStatus[i] = new ArrayList<Integer>(); |
126 |
} |
127 |
for(int i=0; i<NUM_MOTORS; i++){ |
128 |
motorPWMData[i] = new ArrayList<Integer>(); |
129 |
motorVelocityData[i] = new ArrayList<Integer>(); |
130 |
motorStatus[i] = new ArrayList<Integer>(); |
131 |
} |
132 |
for(int i=0; i<NUM_ENCODERS; i++){ |
133 |
encoderDynamoData[i] = new ArrayList<Integer>(); |
134 |
encoderValueData[i] = new ArrayList<Integer>(); |
135 |
encoderStatus[i] = new ArrayList<Integer>(); |
136 |
} |
137 |
} |
138 |
/*
|
139 |
* The returned array has an even number of entries
|
140 |
* The first of each pair is an ArrayList of X entries
|
141 |
* The second of each pair is an ArrayList of the corresponding Y entries
|
142 |
*/
|
143 |
public ArrayList[] getData(int testType, int testNum){ |
144 |
ArrayList data[] = new ArrayList[2]; |
145 |
int arrSize = 2; |
146 |
|
147 |
switch(testType){
|
148 |
case ALL_TESTS:
|
149 |
data = null;
|
150 |
break;
|
151 |
case RANGEFINDER:
|
152 |
if(testNum == ALL_ELEMENTS){
|
153 |
arrSize = 2 * NUM_RANGEFINDERS;
|
154 |
data = new ArrayList[arrSize]; |
155 |
for(int i=0; i<arrSize/2; i++){ |
156 |
data[i*2] = rangefinderWallData[i];
|
157 |
data[i*2 + 1] = rangefinderRangeData[i]; |
158 |
} |
159 |
} |
160 |
else{
|
161 |
data = new ArrayList[arrSize]; |
162 |
data[0] = rangefinderWallData[testNum];
|
163 |
data[1] = rangefinderRangeData[testNum];
|
164 |
} |
165 |
break;
|
166 |
case ENCODER:
|
167 |
if(testNum == ALL_ELEMENTS){
|
168 |
arrSize = 2 * NUM_ENCODERS;
|
169 |
data = new ArrayList[arrSize]; |
170 |
for(int i=0; i<arrSize/2; i++){ |
171 |
data[i*2] = encoderDynamoData[i];
|
172 |
data[i*2 + 1] = encoderValueData[i]; |
173 |
} |
174 |
} |
175 |
else{
|
176 |
data = new ArrayList[arrSize]; |
177 |
data[0] = encoderDynamoData[testNum];
|
178 |
data[1] = encoderValueData[testNum];
|
179 |
} |
180 |
break;
|
181 |
case MOTOR:
|
182 |
if(testNum == ALL_ELEMENTS){
|
183 |
arrSize = 2 * NUM_MOTORS;
|
184 |
data = new ArrayList[arrSize]; |
185 |
for(int i=0; i<arrSize/2; i++){ |
186 |
data[i*2] = motorPWMData[i];
|
187 |
data[i*2 + 1] = motorVelocityData[i]; |
188 |
} |
189 |
} |
190 |
else{
|
191 |
data = new ArrayList[arrSize]; |
192 |
data[0] = motorPWMData[testNum];
|
193 |
data[1] = motorVelocityData[testNum];
|
194 |
} |
195 |
break;
|
196 |
case BOM:
|
197 |
if(testNum == ALL_ELEMENTS){
|
198 |
arrSize = 2 * NUM_BOMS;
|
199 |
data = new ArrayList[arrSize]; |
200 |
for(int i=0; i<arrSize/2; i++){ |
201 |
ArrayList<Integer> trials = new ArrayList<Integer>(); |
202 |
for(int j=1; j<=bomData[i].size(); j++) |
203 |
trials.add(j); |
204 |
data[i*2] = trials;
|
205 |
data[i*2 + 1] = bomData[i]; |
206 |
} |
207 |
} |
208 |
else{
|
209 |
data = new ArrayList[arrSize]; |
210 |
ArrayList<Integer> trials = new ArrayList<Integer>(); |
211 |
for(int j=1; j<=bomData[testNum].size(); j++) |
212 |
trials.add(j); |
213 |
data[0] = trials;
|
214 |
data[1] = bomData[testNum];
|
215 |
} |
216 |
break;
|
217 |
case BOM_EMITTER:
|
218 |
if(testNum == ALL_ELEMENTS){
|
219 |
arrSize = 2 * NUM_BOMS;
|
220 |
data = new ArrayList[arrSize]; |
221 |
for(int i=0; i<arrSize/2; i++){ |
222 |
ArrayList<Integer> bomEmitterData = getSubset(bomData[i], |
223 |
bomStatus[i], BOM_EMITTER); |
224 |
ArrayList<Integer> trials = new ArrayList<Integer>(); |
225 |
for(int j=1; j<=bomEmitterData.size(); j++) |
226 |
trials.add(j); |
227 |
data[i*2] = trials;
|
228 |
data[i*2 + 1] = bomEmitterData; |
229 |
} |
230 |
} |
231 |
else{
|
232 |
data = new ArrayList[arrSize]; |
233 |
ArrayList<Integer> trials = new ArrayList<Integer>(); |
234 |
ArrayList<Integer> bomEmitterData = getSubset(bomData[testNum], |
235 |
bomStatus[testNum], BOM_EMITTER); |
236 |
for(int j=1; j<=bomEmitterData.size(); j++) |
237 |
trials.add(j); |
238 |
data[0] = trials;
|
239 |
data[1] = bomEmitterData;
|
240 |
} |
241 |
break;
|
242 |
case BOM_DETECTOR:
|
243 |
if(testNum == ALL_ELEMENTS){
|
244 |
arrSize = 2 * NUM_BOMS;
|
245 |
data = new ArrayList[arrSize]; |
246 |
for(int i=0; i<arrSize/2; i++){ |
247 |
ArrayList<Integer> bomDetectorData = getSubset(bomData[i], |
248 |
bomStatus[i], BOM_DETECTOR); |
249 |
ArrayList<Integer> trials = new ArrayList<Integer>(); |
250 |
for(int j=1; j<=bomDetectorData.size(); j++) |
251 |
trials.add(j); |
252 |
data[i*2] = trials;
|
253 |
data[i*2 + 1] = bomDetectorData; |
254 |
} |
255 |
} |
256 |
else{
|
257 |
data = new ArrayList[arrSize]; |
258 |
ArrayList<Integer> trials = new ArrayList<Integer>(); |
259 |
ArrayList<Integer> bomDetectorData = getSubset(bomData[testNum], |
260 |
bomStatus[testNum], BOM_DETECTOR); |
261 |
for(int j=1; j<=bomDetectorData.size(); j++) |
262 |
trials.add(j); |
263 |
data[0] = trials;
|
264 |
data[1] = bomDetectorData;
|
265 |
} |
266 |
break;
|
267 |
} |
268 |
return data;
|
269 |
} |
270 |
|
271 |
/*
|
272 |
* The first entry is the chart title
|
273 |
* The next two entries are the X and Y axis labels
|
274 |
* The following entries are the labels for each series
|
275 |
*/
|
276 |
public String[] getLabels(int testType, int testNum){ |
277 |
String labels[]; |
278 |
int arrSize = 4; |
279 |
|
280 |
switch(testType){
|
281 |
case ALL_TESTS:
|
282 |
labels = new String[arrSize]; |
283 |
labels[0] = ""; |
284 |
labels[1] = ""; |
285 |
labels[2] = ""; |
286 |
labels[3] = ""; |
287 |
break;
|
288 |
case RANGEFINDER:
|
289 |
if(testNum == -1) |
290 |
arrSize += NUM_RANGEFINDERS - 1;
|
291 |
labels = new String[arrSize]; |
292 |
labels[0] = "Rangefinder Data"; |
293 |
labels[1] = "Wall Data"; |
294 |
labels[2] = "Rangefinder Output"; |
295 |
if(testNum == -1){ |
296 |
for(int i=0; i<NUM_RANGEFINDERS; i++) |
297 |
labels[i+3] = "" + (i + RANGEFINDER_OFFSET); |
298 |
} |
299 |
else
|
300 |
labels[3] = "" + (testNum + RANGEFINDER_OFFSET); |
301 |
break;
|
302 |
case ENCODER:
|
303 |
if(testNum == -1) |
304 |
arrSize += NUM_ENCODERS - 1;
|
305 |
labels = new String[arrSize]; |
306 |
labels[0] = "Encoder Data"; |
307 |
labels[1] = "Dynamo Data"; |
308 |
labels[2] = "Encoder Output"; |
309 |
if(testNum == -1){ |
310 |
for(int i=0; i<NUM_ENCODERS; i++) |
311 |
labels[i+3] = "" + (i + ENCODER_OFFSET); |
312 |
} |
313 |
else
|
314 |
labels[3] = "" + (testNum + ENCODER_OFFSET); |
315 |
break;
|
316 |
case MOTOR:
|
317 |
if(testNum == -1) |
318 |
arrSize += NUM_MOTORS - 1;
|
319 |
labels = new String[arrSize]; |
320 |
labels[0] = "Motor Data"; |
321 |
labels[1] = "PWM Data"; |
322 |
labels[2] = "Motor Velocity Data"; |
323 |
if(testNum == -1){ |
324 |
for(int i=0; i<NUM_MOTORS; i++) |
325 |
labels[i+3] = "" + (i + MOTOR_OFFSET); |
326 |
} |
327 |
else
|
328 |
labels[3] = "" + (testNum + MOTOR_OFFSET); |
329 |
break;
|
330 |
case BOM:
|
331 |
if(testNum == -1) |
332 |
arrSize += NUM_BOMS - 1;
|
333 |
labels = new String[arrSize]; |
334 |
labels[0] = "Bom Data"; |
335 |
labels[1] = "Test Number"; |
336 |
labels[2] = "BOM Reading"; |
337 |
if(testNum == -1){ |
338 |
for(int i=0; i<NUM_BOMS; i++) |
339 |
labels[i+3] = "" + (i + BOM_OFFSET); |
340 |
} |
341 |
else
|
342 |
labels[3] = "" + (testNum + BOM_OFFSET); |
343 |
break;
|
344 |
case BOM_EMITTER:
|
345 |
if(testNum == -1) |
346 |
arrSize += NUM_BOMS - 1;
|
347 |
labels = new String[arrSize]; |
348 |
labels[0] = "Bom Emitter Data"; |
349 |
labels[1] = "Test Number"; |
350 |
labels[2] = "BOM Emitter Reading"; |
351 |
if(testNum == -1){ |
352 |
for(int i=0; i<NUM_BOMS; i++) |
353 |
labels[i+3] = "" + (i + BOM_OFFSET); |
354 |
} |
355 |
else
|
356 |
labels[3] = "" + (testNum + BOM_OFFSET); |
357 |
break;
|
358 |
case BOM_DETECTOR:
|
359 |
if(testNum == -1) |
360 |
arrSize += NUM_BOMS - 1;
|
361 |
labels = new String[arrSize]; |
362 |
labels[0] = "Bom Detector Data"; |
363 |
labels[1] = "Test Number"; |
364 |
labels[2] = "BOM Detector Reading"; |
365 |
if(testNum == -1){ |
366 |
for(int i=0; i<NUM_BOMS; i++) |
367 |
labels[i+3] = "" + (i + BOM_OFFSET); |
368 |
} |
369 |
else
|
370 |
labels[3] = "" + (testNum + BOM_OFFSET); |
371 |
break;
|
372 |
default:
|
373 |
labels = new String[arrSize]; |
374 |
labels[0] = ""; |
375 |
labels[1] = ""; |
376 |
labels[2] = ""; |
377 |
labels[3] = ""; |
378 |
} |
379 |
return labels;
|
380 |
} |
381 |
|
382 |
/*
|
383 |
* Starts running the given test on all elements
|
384 |
*/
|
385 |
public void startTest(int testType) throws Exception{ |
386 |
startTest(testType, ALL_ELEMENTS); |
387 |
} |
388 |
|
389 |
/*
|
390 |
* Starts running the specified test on the specified element
|
391 |
*/
|
392 |
public void startTest(int testType, int num) throws Exception{ |
393 |
String command = getStartTestCommand(testType, num);
|
394 |
if(command == null){ |
395 |
System.out.println("Illegal test requested\n"); |
396 |
return;
|
397 |
} |
398 |
gui.addTextToConsole(command); |
399 |
serial.puts(command); |
400 |
boolean ready = false; |
401 |
while(!ready){
|
402 |
String input = getInput();
|
403 |
int parseStatus = parseInput(input);
|
404 |
if(parseStatus == READY)
|
405 |
ready = true;
|
406 |
} |
407 |
printData(testType); |
408 |
} |
409 |
|
410 |
/*
|
411 |
* Stops the running test
|
412 |
*/
|
413 |
public void stopTest() throws Exception{ |
414 |
serial.puts("stop\r\n");
|
415 |
} |
416 |
|
417 |
/*
|
418 |
* Prints out the received data - good for debugging if the grapher is malfunctioning
|
419 |
*/
|
420 |
public void printData(int test){ |
421 |
switch(test){
|
422 |
case ALL_TESTS:
|
423 |
printData(RANGEFINDER); |
424 |
printData(ENCODER); |
425 |
printData(MOTOR); |
426 |
printData(BOM); |
427 |
break;
|
428 |
case RANGEFINDER:
|
429 |
System.out.println("Rangefinder Wall Data:"); |
430 |
print(rangefinderWallData); |
431 |
System.out.println("Rangefinder Range Data:"); |
432 |
print(rangefinderRangeData); |
433 |
break;
|
434 |
case ENCODER:
|
435 |
System.out.println("Encoder Dynamo Data:"); |
436 |
print(encoderDynamoData); |
437 |
System.out.println("Encoder Value Data:"); |
438 |
print(encoderValueData); |
439 |
break;
|
440 |
case MOTOR:
|
441 |
System.out.println("Motor PWM Data:"); |
442 |
print(motorPWMData); |
443 |
System.out.println("Motor Velocity Data:"); |
444 |
print(motorVelocityData); |
445 |
break;
|
446 |
case BOM:
|
447 |
printData(BOM_EMITTER); |
448 |
printData(BOM_DETECTOR); |
449 |
break;
|
450 |
case BOM_EMITTER:
|
451 |
System.out.println("BOM emitter Data:"); |
452 |
printStatus(bomData, bomStatus, EMITTER); |
453 |
break;
|
454 |
case BOM_DETECTOR:
|
455 |
System.out.println("BOM detector Data:"); |
456 |
printStatus(bomData, bomStatus, DETECTOR|DETECTOR_OUT); |
457 |
break;
|
458 |
} |
459 |
} |
460 |
|
461 |
/*
|
462 |
* Picks out the given subset of the given data
|
463 |
*/
|
464 |
private ArrayList<Integer> getSubset(ArrayList data, ArrayList status, int bitmask){ |
465 |
ArrayList<Integer> arr = new ArrayList<Integer>(); |
466 |
for(int i=0; i< status.size(); i++){ |
467 |
if(((Integer)status.get(i) & bitmask) == bitmask){ |
468 |
arr.add((Integer)data.get(i));
|
469 |
} |
470 |
} |
471 |
return arr;
|
472 |
} |
473 |
|
474 |
/*
|
475 |
* Prints out the current status
|
476 |
*/
|
477 |
private void printStatus(ArrayList data[], ArrayList status[], int bitmask){ |
478 |
for(int i=0; i<status.length; i++){ |
479 |
System.out.print(i + ": "); |
480 |
printStatus(data[i], status[i], bitmask); |
481 |
System.out.println();
|
482 |
} |
483 |
} |
484 |
|
485 |
/*
|
486 |
* Prints the current status
|
487 |
*/
|
488 |
private void printStatus(ArrayList data, ArrayList status, int bitmask){ |
489 |
System.out.print("["); |
490 |
boolean first = true; |
491 |
for(int i=0; i< status.size(); i++){ |
492 |
if(((Integer)status.get(i) & bitmask) == bitmask){ |
493 |
if(!first)
|
494 |
System.out.print(", "); |
495 |
System.out.print(data.get(i));
|
496 |
first = false;
|
497 |
} |
498 |
} |
499 |
System.out.print("]"); |
500 |
} |
501 |
|
502 |
/*
|
503 |
* Prints an ArrayList of arrays
|
504 |
*/
|
505 |
private void print(ArrayList arr[]){ |
506 |
for(int i=0; i<arr.length; i++){ |
507 |
System.out.print(i + ": "); |
508 |
print(arr[i]); |
509 |
System.out.println();
|
510 |
} |
511 |
} |
512 |
|
513 |
/*
|
514 |
* Prints an array
|
515 |
*/
|
516 |
private void print(ArrayList arr){ |
517 |
System.out.print("["); |
518 |
for(int i=0; i< arr.size(); i++){ |
519 |
System.out.print(arr.get(i));
|
520 |
if(i < arr.size() - 1) |
521 |
System.out.print(", "); |
522 |
} |
523 |
System.out.print("]"); |
524 |
} |
525 |
|
526 |
/*
|
527 |
* Returns the command to pass to the server, or null if the
|
528 |
* request is malformed
|
529 |
*/
|
530 |
public String getStartTestCommand(int testType, int num){ |
531 |
String command = "start_test"; |
532 |
switch(testType){
|
533 |
case ALL_TESTS:
|
534 |
command += " all\n";
|
535 |
return command;
|
536 |
case RANGEFINDER:
|
537 |
command += " rangefinder";
|
538 |
break;
|
539 |
case ENCODER:
|
540 |
command += " encoder";
|
541 |
break;
|
542 |
case MOTOR:
|
543 |
command += " motor";
|
544 |
break;
|
545 |
case BOM:
|
546 |
command += " bom";
|
547 |
break;
|
548 |
case BOM_EMITTER:
|
549 |
command += " bom emitter";
|
550 |
break;
|
551 |
case BOM_DETECTOR:
|
552 |
command += " bom detector";
|
553 |
break;
|
554 |
default:
|
555 |
return null; |
556 |
} |
557 |
if(num == -1) |
558 |
command += " all";
|
559 |
else
|
560 |
command += " " + num;
|
561 |
return command + "\n"; |
562 |
} |
563 |
|
564 |
/*
|
565 |
* Sends the given message to the station
|
566 |
*/
|
567 |
public void send_message(String s) throws Exception{ |
568 |
serial.puts(s); |
569 |
} |
570 |
|
571 |
/*
|
572 |
* Prints out all data received (this method hangs permanently)
|
573 |
*/
|
574 |
public String getInput() throws Exception{ |
575 |
while(true){ |
576 |
String rec = serial.gets();
|
577 |
if(rec != null) |
578 |
return rec;
|
579 |
} |
580 |
} |
581 |
|
582 |
/*
|
583 |
* Prints any received data (hangs for ms milliseconds)
|
584 |
*/
|
585 |
public String getInput(int ms) throws Exception{ |
586 |
long time = System.currentTimeMillis(); |
587 |
while(true){ |
588 |
if(System.currentTimeMillis() - time > ms){ |
589 |
System.out.println("Operation timed out\n"); |
590 |
return null; |
591 |
} |
592 |
String rec = serial.gets();
|
593 |
if(rec != null) |
594 |
return rec;
|
595 |
} |
596 |
} |
597 |
|
598 |
/*
|
599 |
* Parses the input to determine which kind of request is being received
|
600 |
*/
|
601 |
private int parseInput(String s){ |
602 |
if(s.charAt(0) == '#' || s.charAt(0) == '\n' || s.charAt(0) == '\r'){ |
603 |
System.out.print(s);
|
604 |
return COMMENT;
|
605 |
} |
606 |
else if(s.startsWith("data")){ |
607 |
if(parseData(s.substring(4))) |
608 |
return DATA;
|
609 |
else{
|
610 |
System.out.println("Error parsing data >" + s + "<"); |
611 |
return PARSE_ERROR;
|
612 |
} |
613 |
} |
614 |
else if(s.startsWith("ready")) |
615 |
return READY;
|
616 |
else{
|
617 |
System.out.println("Unable to parse message >" + s + "<"); |
618 |
return PARSE_ERROR;
|
619 |
} |
620 |
} |
621 |
|
622 |
/*
|
623 |
* Parses data messages
|
624 |
*/
|
625 |
private boolean parseData(String s){ |
626 |
try{
|
627 |
Scanner scan = new Scanner(s); |
628 |
String testType = scan.next();
|
629 |
if(testType.equals("bom")){ |
630 |
ArrayList<Integer> dataArr = new ArrayList<Integer>(); |
631 |
ArrayList<Integer> statusArr = new ArrayList<Integer>(); |
632 |
|
633 |
String emDet = scan.next();
|
634 |
int num = scan.nextInt();
|
635 |
if(num >= NUM_BOMS){
|
636 |
System.out.println("Invalid bom index: " + num); |
637 |
return false; |
638 |
} |
639 |
|
640 |
if(emDet.equals("emitter")){ |
641 |
while(scan.hasNext()){
|
642 |
int mask = EMITTER;
|
643 |
String data = scan.next();
|
644 |
if(data.startsWith("top")) |
645 |
mask |= EMITTER_TOP; |
646 |
else if(data.startsWith("left")) |
647 |
mask |= EMITTER_LEFT; |
648 |
else if(data.startsWith("right")) |
649 |
mask |= EMITTER_RIGHT; |
650 |
else
|
651 |
return false; |
652 |
|
653 |
int slashIdx = data.indexOf("/"); |
654 |
int value = Integer.valueOf(data.substring( |
655 |
slashIdx+1));
|
656 |
dataArr.add(value); |
657 |
statusArr.add(mask); |
658 |
} |
659 |
bomData[num].addAll(dataArr); |
660 |
bomStatus[num].addAll(statusArr); |
661 |
return true; |
662 |
} |
663 |
else if(emDet.equals("detector")){ |
664 |
while(scan.hasNext()){
|
665 |
int mask = DETECTOR;
|
666 |
String data = scan.next();
|
667 |
int dataStart;
|
668 |
if(data.startsWith("in")){ |
669 |
mask |= DETECTOR_IN; |
670 |
dataStart = 3;
|
671 |
} |
672 |
else if(data.startsWith("out")){ |
673 |
mask |= DETECTOR_OUT; |
674 |
dataStart = 4;
|
675 |
} |
676 |
else
|
677 |
return false; |
678 |
if(data.charAt(dataStart-1) != '/') |
679 |
return false; |
680 |
int value = Integer.valueOf(data.substring(dataStart)); |
681 |
dataArr.add(value); |
682 |
statusArr.add(mask); |
683 |
} |
684 |
bomData[num].addAll(dataArr); |
685 |
bomStatus[num].addAll(statusArr); |
686 |
return true; |
687 |
} |
688 |
else
|
689 |
return false; |
690 |
} |
691 |
else if(testType.equals("rangefinder")){ |
692 |
ArrayList<Integer> dataWallArr = new ArrayList<Integer>(); |
693 |
ArrayList<Integer> dataRangeArr = new ArrayList<Integer>(); |
694 |
ArrayList<Integer> statusArr = new ArrayList<Integer>(); |
695 |
|
696 |
int num = scan.nextInt();
|
697 |
if(num >= NUM_RANGEFINDERS){
|
698 |
System.out.println("Invalid rangefinder index: " + num); |
699 |
return false; |
700 |
} |
701 |
|
702 |
String dir = scan.next();
|
703 |
int mask;
|
704 |
if(dir.equals("in")) |
705 |
mask = RANGEFINDER_IN; |
706 |
else if(dir.equals("out")) |
707 |
mask = RANGEFINDER_OUT; |
708 |
else
|
709 |
return false; |
710 |
|
711 |
while(scan.hasNext()){
|
712 |
String data = scan.next();
|
713 |
|
714 |
int slashIdx = data.indexOf("/"); |
715 |
int wallDist = Integer.valueOf(data.substring(0, |
716 |
slashIdx)); |
717 |
int rangefinderReading = Integer.valueOf(data.substring( |
718 |
slashIdx+1));
|
719 |
|
720 |
dataWallArr.add(wallDist); |
721 |
dataRangeArr.add(rangefinderReading); |
722 |
statusArr.add(mask); |
723 |
} |
724 |
rangefinderWallData[num].addAll(dataWallArr); |
725 |
rangefinderRangeData[num].addAll(dataRangeArr); |
726 |
rangefinderStatus[num].addAll(statusArr); |
727 |
return true; |
728 |
} |
729 |
else if(testType.equals("motor")){ |
730 |
ArrayList<Integer> dataPWMArr = new ArrayList<Integer>(); |
731 |
ArrayList<Integer> dataVelocityArr = new ArrayList<Integer>(); |
732 |
ArrayList<Integer> statusArr = new ArrayList<Integer>(); |
733 |
|
734 |
int num = scan.nextInt();
|
735 |
if(num >= NUM_MOTORS){
|
736 |
System.out.println("Invalid motor index: " + num); |
737 |
return false; |
738 |
} |
739 |
|
740 |
String dir = scan.next();
|
741 |
int mask;
|
742 |
boolean forward = true; |
743 |
if(dir.equals("forward")) |
744 |
mask = MOTOR_FORWARD; |
745 |
else if(dir.equals("backward")){ |
746 |
mask = MOTOR_BACKWARD; |
747 |
forward = false;
|
748 |
} |
749 |
else
|
750 |
return false; |
751 |
|
752 |
String accel = scan.next();
|
753 |
if(accel.equals("increasing")) |
754 |
mask |= MOTOR_ACCELERATING; |
755 |
else if(accel.equals("decreasing")) |
756 |
mask |= MOTOR_DECELERATING; |
757 |
else if(accel.equals("constant")) |
758 |
mask |= MOTOR_CONST_ACCEL; |
759 |
else
|
760 |
return false; |
761 |
|
762 |
while(scan.hasNext()){
|
763 |
String data = scan.next();
|
764 |
|
765 |
int slashIdx = data.indexOf("/"); |
766 |
int pwm = Integer.valueOf(data.substring(0, |
767 |
slashIdx)); |
768 |
int velocity = Integer.valueOf(data.substring( |
769 |
slashIdx+1));
|
770 |
if(forward)
|
771 |
dataPWMArr.add(pwm); |
772 |
else
|
773 |
dataPWMArr.add(-1 * pwm);
|
774 |
dataVelocityArr.add(velocity); |
775 |
statusArr.add(mask); |
776 |
} |
777 |
motorPWMData[num].addAll(dataPWMArr); |
778 |
motorVelocityData[num].addAll(dataVelocityArr); |
779 |
motorStatus[num].addAll(statusArr); |
780 |
return true; |
781 |
} |
782 |
else if(testType.equals("encoder")){ |
783 |
ArrayList<Integer> dataDynamoArr = new ArrayList<Integer>(); |
784 |
ArrayList<Integer> dataEncoderArr = new ArrayList<Integer>(); |
785 |
ArrayList<Integer> statusArr = new ArrayList<Integer>(); |
786 |
|
787 |
int num = scan.nextInt();
|
788 |
if(num >= NUM_ENCODERS){
|
789 |
System.out.println("Invalid encoder index: " + num); |
790 |
return false; |
791 |
} |
792 |
|
793 |
String dir = scan.next();
|
794 |
int mask;
|
795 |
if(dir.equals("forward")) |
796 |
mask = ENCODER_FORWARD; |
797 |
else if(dir.equals("backward")) |
798 |
mask = ENCODER_BACKWARD; |
799 |
else
|
800 |
return false; |
801 |
|
802 |
while(scan.hasNext()){
|
803 |
String data = scan.next();
|
804 |
|
805 |
int slashIdx = data.indexOf("/"); |
806 |
int dynamo = Integer.valueOf(data.substring(0, |
807 |
slashIdx)); |
808 |
int encoder = Integer.valueOf(data.substring( |
809 |
slashIdx+1));
|
810 |
|
811 |
dataDynamoArr.add(dynamo); |
812 |
dataEncoderArr.add(encoder); |
813 |
statusArr.add(mask); |
814 |
} |
815 |
encoderDynamoData[num].addAll(dataDynamoArr); |
816 |
encoderValueData[num].addAll(dataEncoderArr); |
817 |
encoderStatus[num].addAll(statusArr); |
818 |
return true; |
819 |
} |
820 |
else
|
821 |
return false; |
822 |
} |
823 |
catch(Exception e){ |
824 |
return false; |
825 |
} |
826 |
} |
827 |
|
828 |
/*
|
829 |
* Pings the station
|
830 |
*/
|
831 |
public void ping() throws Exception{ |
832 |
serial.puts("ping\n");
|
833 |
System.out.println(serial.gets());
|
834 |
} |
835 |
|
836 |
/*
|
837 |
* Receives pings and returns a pong
|
838 |
*/
|
839 |
public void receive_ping() throws Exception{ |
840 |
String in = serial.gets();
|
841 |
if(in == "#ping\n") |
842 |
serial.puts("#pong\n");
|
843 |
else
|
844 |
System.out.println("Message received was not a ping. " + |
845 |
"Message was:\n" + in);
|
846 |
} |
847 |
} |