All Classes Namespaces Files Functions Variables Enumerations Enumerator Pages
multi_urg.c
1 
13 #include "urg_ctrl.h"
14 #include <stdio.h>
15 #include <stdlib.h>
16 
17 
18 static void urg_exit(urg_t *urg, const char *message)
19 {
20  printf("%s: %s\n", message, urg_error(urg));
21  urg_disconnect(urg);
22 
23 #ifdef MSC
24  getchar();
25 #endif
26  exit(1);
27 }
28 
29 
31 int main(int argc, char *argv[])
32 {
33  enum { Times = 10 };
34 
35  urg_t urg[2];
36  long *data[2];
37  int data_max[2];
38  int timestamp;
39  int ret;
40  int n;
41  int i;
42  int k;
43 
44  //const char *devices[] = { "COM3", "COM4" }; /* For Windows */
45  const char *devices[] = { "/dev/ttyACM0", "/dev/ttyACM1" }; /* For Linux */
46 
47  /* Connection */
48  for (i = 0; i < 2; ++i) {
49  ret = urg_connect(&urg[i], devices[i], 115200);
50  if (ret < 0) {
51  urg_exit(&urg[i], "urg_connect()");
52  }
53  /* To clear existing MD command*/
54  urg_laserOff(&urg[i]);
55 
56  /* It will become easy if some frames are skipped. */
57  /* If specified skip is 2, then transferred data becomes half. */
58  //urg_setSkipLines(&urg[i], 2);
59 
60  /* Reserve for receive buffer */
61  data_max[i] = urg_dataMax(&urg[i]);
62  data[i] = (long*)malloc(sizeof(long) * data_max[i]);
63  if (data[i] == NULL) {
64  perror("data buffer");
65  exit(1);
66  }
67  }
68 
69  /* Request for MD data */
70  for (i = 0; i < 2; ++i) {
71  urg_setCaptureTimes(&urg[i], Times);
72 
73  /* Request for data */
74  ret = urg_requestData(&urg[i], URG_MD, URG_FIRST, URG_LAST);
75  if (ret < 0) {
76  urg_exit(&urg[i], "urg_requestData()");
77  }
78  }
79 
80  for (k = 0; k < Times; ++k) {
81  for (i = 0; i < 2; ++i) {
82  /* Ends when data reception is completed */
83  int remain_times = urg_remainCaptureTimes(&urg[i]);
84  printf(" %d: ", i);
85  printf("(%03d/%03d): ", remain_times, Times);
86  if (remain_times <= 0) {
87  printf("\n");
88  continue;
89  }
90 
91  /* Reception */
92  n = urg_receiveData(&urg[i], data[i], data_max[i]);
93  if (n < 0) {
94  // Continue processing, because there is chances of receiving the data next time.
95  printf("%s: %s\n", "urg_receiveData()", urg_error(urg));
96 
97  } else {
98 
99  /* Display */
100  timestamp = urg_recentTimestamp(&urg[i]);
101  printf("timestamp: %d, ", timestamp);
102 #if 0
103  {
104  int j;
105  for (j = 0; j < n; ++j) {
106  /* Neglect if distance data is less than urg_minDistance() */
107  printf("%d:%ld, ", j, data[i][j]);
108  }
109  printf("\n");
110  }
111 #endif
112  printf("\n");
113  }
114  }
115  }
116 
117  /* Disconnect */
118  for (i = 0; i < 2; ++i) {
119  urg_disconnect(&urg[i]);
120  free(data[i]);
121  }
122 
123 #ifdef MSC
124  getchar();
125 #endif
126  return 0;
127 }