vrpn  07.33
Virtual Reality Peripheral Network
vrpn_Zaber.C
Go to the documentation of this file.
1 // vrpn_Zaber.C
2 // This is a driver for the Zaber T-LA linear actuators.
3 // They plug in a daisy chain into a serial
4 // line and communicates using RS-232 (this is a raw-mode driver).
5 // You can find out more at www.zaber.com. They have a manual with
6 // a section on the serial interface; this code is based on that.
7 // It was written in May 2002 by Russ Taylor.
8 
9 // INFO about how the device communicates:
10 // There are 6-byte commands. Byte 0 is the device number (with
11 // device 0 meaning "all devices." Byte 1 is the command number.
12 // Byte 2 is the LSB of the 4-byte signed integer data, byte 5 is
13 // the MSB (most-significant byte) of the data.
14 
15 
16 #include <stdio.h> // for sprintf, fprintf, stderr, etc
17 
18 #include "vrpn_BaseClass.h" // for ::vrpn_TEXT_ERROR, etc
19 #include "vrpn_Serial.h" // for vrpn_flush_input_buffer, etc
20 #include "vrpn_Shared.h" // for timeval, vrpn_unbuffer, etc
21 #include "vrpn_Zaber.h"
22 #include "vrpn_MessageMacros.h" // for VRPN_MSG_INFO, VRPN_MSG_WARNING, VRPN_MSG_ERROR
23 
24 #undef VERBOSE
25 
26 // Defines the modes in which the device can find itself.
27 #define STATUS_RESETTING (-1) // Resetting the device
28 #define STATUS_SYNCING (0) // Looking for the first character of report
29 #define STATUS_READING (1) // Looking for the rest of the report
30 
31 #define TIMEOUT_TIME_INTERVAL (2000000L) // max time between reports (usec)
32 #define POLL_INTERVAL (1000000L) // time to poll if no response in a while (usec)
33 
34 
35 // This creates a vrpn_Zaber and sets it to reset mode. It opens
36 // the serial device using the code in the vrpn_Serial_Analog constructor.
37 
38 vrpn_Zaber::vrpn_Zaber (const char * name, vrpn_Connection * c,
39  const char * port):
40  vrpn_Serial_Analog(name, c, port),
41  vrpn_Analog_Output(name, c)
42 {
43  d_last_poll.tv_sec = 0;
44  d_last_poll.tv_usec = 0;
45 
46  num_channel = 0; // This is an estimate; will change when the reset happens
47  o_num_channel = 0;
48 
49  // Set the mode to reset
51 
52  // Register to receive the message to request changes and to receive connection
53  // messages.
54  if (d_connection != NULL) {
56  this, d_sender_id)) {
57  fprintf(stderr,"vrpn_Zaber: can't register handler\n");
58  d_connection = NULL;
59  }
61  this, d_sender_id)) {
62  fprintf(stderr,"vrpn_Zaber: can't register handler\n");
63  d_connection = NULL;
64  }
66  this, d_sender_id)) {
67  fprintf(stderr,"vrpn_Zaber: can't register handler\n");
68  d_connection = NULL;
69  }
70  } else {
71  fprintf(stderr,"vrpn_Zaber: Can't get connection!\n");
72  }
73 
74 }
75 
76 bool vrpn_Zaber::send_command(unsigned char devicenum, unsigned char cmd, vrpn_int32 data)
77 {
78  unsigned char command[128];
79 
80  // Fill in the device number and command number
81  command[0] = devicenum;
82  command[1] = cmd;
83 
84  // Fill in the data in the correct byte order
85  command[2] = static_cast<unsigned char>(data & 0x000000FF);
86  command[3] = static_cast<unsigned char>((data >> 8) & 0x000000FF);
87  command[4] = static_cast<unsigned char>((data >> 16) & 0x000000FF);
88  command[5] = static_cast<unsigned char>((data >> 24) & 0x000000FF);
89 
90  // Send the command to the serial port
91  return (vrpn_write_characters(serial_fd, command, 6) == 6);
92 }
93 
94 bool vrpn_Zaber::send_command(unsigned char devnum, unsigned char cmd, unsigned char d0,
95  unsigned char d1, unsigned char d2, unsigned char d3)
96 {
97  unsigned char command[128];
98 
99  // Fill in the device number and command number
100  command[0] = devnum;
101  command[1] = cmd;
102 
103  // Fill in the data in the correct byte order
104  command[2] = d0;
105  command[3] = d1;
106  command[4] = d2;
107  command[5] = d3;
108 
109  // Send the command to the serial port
110  return (vrpn_write_characters(serial_fd, command, 6) == 6);
111 }
112 
114 vrpn_int32 vrpn_Zaber::convert_bytes_to_reading(const unsigned char *buf)
115 {
116  vrpn_int32 data;
117 
118  data = ((buf[0]) & 0x000000FF)
119  + ((buf[1] << 8) & 0x0000FF00)
120  + ((buf[2] << 16) & 0x00FF0000)
121  + ((buf[3] << 24) & 0xFF000000);
122 
123  return data;
124 }
125 
126 
127 // This routine will reset the Zebers, asking them to renumber themselves and then polling
128 // to see how many there are in the chain.
129 // Commands Responses Meanings
130 // [0][2][0][0][0][0] None Renumber the devices on the chain (wait 1 second)
131 // [X][23][0][0][0][0] [X][23][Position] Tell each to stop and see if get a reply (tells where they are)
132 // [X][40][16][0][0][0] [X][40][16][0][0][0] Set it to report position when moving linearly
133 
135 {
136  struct timeval timeout;
137  unsigned char inbuf[128];
138  int ret;
139  char errmsg[256];
140 
141  //-----------------------------------------------------------------------
142  // Sleep a second and then drain the input buffer to make sure we start
143  // with a fresh slate.
144  vrpn_SleepMsecs(1000);
146 
147  //-----------------------------------------------------------------------
148  // Send the command to request that the units renumber themselves. Then
149  // wait 1 second to give them time to do so.
150  if (!send_command(0,2,0)) {
151  fprintf(stderr,"vrpn_Zaber::reset(): Cannot send renumber command, trying again\n");
152  return -1;
153  }
154  vrpn_SleepMsecs(1000);
155 
156  //-----------------------------------------------------------------------
157  // Go one by one and request the units to stop. This will cause them to
158  // say where they are. When one doesn't respond, we've figured out how
159  // many we have! Also, set it to report position when moving at constant
160  // linear velocity and set the minimum step size and maximum step size
161  // to be slower than the defaults to hopefully give more torque.
162 
163  num_channel = 0;
164  o_num_channel = 0;
165  do {
166  int expected_chars = 4*6;
168  unsigned char channel_id = static_cast<unsigned char>(num_channel+1);
169  send_command(channel_id, 23, 0); vrpn_SleepMsecs(10);
170  send_command(channel_id, 40, 16, 0, 0, 0); vrpn_SleepMsecs(10);
171  send_command(channel_id, 41, 128, 0, 0, 0); vrpn_SleepMsecs(10);
172  send_command(channel_id, 42, 128, 0, 0, 0);
173 
174  timeout.tv_sec = 0;
175  timeout.tv_usec = 30000;
176  ret = vrpn_read_available_characters(serial_fd, inbuf, expected_chars, &timeout);
177  if (ret < 0) {
178  perror("vrpn_Zaber::reset(): Error reading position from device");
179  return -1;
180  }
181  if (ret == 0) {
182  break; //< No more devices found
183  }
184  if (ret != expected_chars) {
185  sprintf(errmsg,"reset: Got %d of %d expected characters for position\n",ret, expected_chars);
186  VRPN_MSG_ERROR(errmsg);
187  return -1;
188  }
189 
190  // Make sure the string we got back is what we expected and set the value
191  // for this channel to what we got from the device.
192  if ( (inbuf[0] != num_channel+1) || (inbuf[1] != 23) ) {
193  VRPN_MSG_ERROR("reset: Bad response to device # request");
194  return -1;
195  }
197 
198  num_channel++;
199  o_num_channel++;
200 
201 // Yes, I know the conditional expression is a constant!
202 #ifdef _WIN32
203 #pragma warning ( disable : 4127 )
204 #endif
205  } while (true);
206 #ifdef _WIN32
207 #pragma warning ( default : 4127 )
208 #endif
209  sprintf(errmsg,"found %d devices",num_channel);
210  VRPN_MSG_WARNING(errmsg);
211 
212  // We're now waiting for any responses from devices
214 
215  VRPN_MSG_WARNING("reset complete (this is good)");
216 
217  vrpn_gettimeofday(&timestamp, NULL); // Set watchdog now
218  return 0;
219 }
220 
221 // This function will read characters until it has a full report, then
222 // put that report into analog fields and call the report methods on these.
223 // The time stored is that of the first character received as part of the
224 // report.
225 
227 {
228  int ret; // Return value from function call to be checked
229 
230  //--------------------------------------------------------------------
231  // If we're SYNCing, then the next character we get should be the start
232  // of a report. If we recognize it, go into READing mode and tell how
233  // many characters we expect total. If we don't recognize it, then we
234  // must have misinterpreted a command or something; reset
235  // and start over
236  //--------------------------------------------------------------------
237 
238  if (status == STATUS_SYNCING) {
239  // Try to get a character. If none, just return.
241  return 0;
242  }
243 
244  d_expected_chars = 6;
245 
246  //XXX How do we know when we're out of sync?
247 
248  // Got the first character of a report -- go into READING mode
249  // and record that we got one character at this time. The next
250  // bit of code will attempt to read the rest of the report.
251  // The time stored here is as close as possible to when the
252  // report was generated.
253  d_bufcount = 1;
256 #ifdef VERBOSE
257  printf("... Got the 1st char\n");
258 #endif
259  }
260 
261  //--------------------------------------------------------------------
262  // Read as many bytes of this report as we can, storing them
263  // in the buffer. We keep track of how many have been read so far
264  // and only try to read the rest.
265  //--------------------------------------------------------------------
266 
269  if (ret == -1) {
270  VRPN_MSG_ERROR("Error reading");
272  return 0;
273  }
274  d_bufcount += ret;
275 #ifdef VERBOSE
276  if (ret != 0) printf("... got %d characters (%d total)\n",ret, d_bufcount);
277 #endif
278  if (d_bufcount < d_expected_chars) { // Not done -- go back for more
279  return 0;
280  }
281 
282  //--------------------------------------------------------------------
283  // We now have enough characters to make a full report. Check to make
284  // sure that its format matches what we expect. If it does, the next
285  // section will parse it. If it does not, we need to go back into
286  // synch mode and ignore this report. A well-formed report has
287  // either 23 or 10 as its command. Also accept command number 1
288  // (reset). Also accept command 20 (go to absolute position). Also
289  // parse command 255 (out of range setting requested -- it puts the
290  // actual position in place).
291  //--------------------------------------------------------------------
292 
293  if ( (d_buffer[1] != 10) && (d_buffer[1] != 23) && (d_buffer[1] != 1)
294  && (d_buffer[1] != 20) && (d_buffer[1] != 255) ) {
296  char msg[1024];
297  sprintf(msg,"Bad command type (%d) in report (ignoring this report)", d_buffer[1]);
298  VRPN_MSG_WARNING(msg);
300  return 0;
301  }
302  if (d_buffer[1] == 255) {
303  VRPN_MSG_WARNING("Requested value out of range");
304  }
305 
306 #ifdef VERBOSE
307  printf("got a complete report (%d of %d)!\n", d_bufcount, d_expected_chars);
308 #endif
309 
310  //--------------------------------------------------------------------
311  // Decode the report and store the values in it into the analog value
312  //--------------------------------------------------------------------
313 
314  unsigned char chan = static_cast<unsigned char>(d_buffer[0] - 1);
315  vrpn_int32 value = convert_bytes_to_reading(&d_buffer[2]);
316  if (chan >= num_channel) { // Unsigned, so can't be < 0
317  char msg[1024];
318  sprintf(msg,"Invalid channel (%d of %d), resetting", chan, num_channel);
319  VRPN_MSG_ERROR(msg);
321  }
322  channel[chan] = value;
323 
324  //--------------------------------------------------------------------
325  // Done with the decoding, send the reports and go back to syncing
326  //--------------------------------------------------------------------
327 
328  report_changes();
330  d_bufcount = 0;
331 
332  return 1;
333 }
334 
336 {
337  const char *bufptr = p.buffer;
338  vrpn_int32 chan_num;
339  vrpn_int32 pad;
340  vrpn_float64 value;
341  vrpn_Zaber *me = (vrpn_Zaber *)userdata;
342 
343  // Read the parameters from the buffer
344  vrpn_unbuffer(&bufptr, &chan_num);
345  vrpn_unbuffer(&bufptr, &pad);
346  vrpn_unbuffer(&bufptr, &value);
347 
348  // Set the position to the appropriate value, if the channel number is in the
349  // range of the ones we have.
350  if ( (chan_num < 0) || (chan_num >= me->o_num_channel) ) {
351  char msg[1024];
352  sprintf(msg,"vrpn_Zaber::handle_request_message(): Index out of bounds (%d of %d), value %lg\n",
353  chan_num, me->num_channel, value);
355  return 0;
356  }
357  me->channel[chan_num] = value;
358  me->send_command(static_cast<unsigned char>(chan_num+1),20,(vrpn_int32)value);
359 
360  return 0;
361 }
362 
364 {
365  int i;
366  const char* bufptr = p.buffer;
367  vrpn_int32 num;
368  vrpn_int32 pad;
369  vrpn_Zaber* me = (vrpn_Zaber *)userdata;
370 
371  // Read the values from the buffer
372  vrpn_unbuffer(&bufptr, &num);
373  vrpn_unbuffer(&bufptr, &pad);
374  if (num > me->o_num_channel) {
375  char msg[1024];
376  sprintf(msg,"vrpn_Zaber::handle_request_channels_message(): Index out of bounds (%d of %d), clipping\n",
377  num, me->o_num_channel);
379  num = me->o_num_channel;
380  }
381  for (i = 0; i < num; i++) {
382  vrpn_unbuffer(&bufptr, &(me->o_channel[i]));
383  me->send_command(static_cast<unsigned char>(i+1),20,(vrpn_int32)me->o_channel[i]);
384  }
385 
386  return 0;
387 }
388 
392 {
393  vrpn_Zaber *me = (vrpn_Zaber *)userdata;
394 
396  return 0;
397 }
398 
399 void vrpn_Zaber::report_changes(vrpn_uint32 class_of_service)
400 {
402 
403  vrpn_Analog::report_changes(class_of_service);
404 }
405 
406 void vrpn_Zaber::report(vrpn_uint32 class_of_service)
407 {
409 
410  vrpn_Analog::report(class_of_service);
411 }
412 
421 {
422  char errmsg[256];
423 
424  server_mainloop();
425 
426  switch(status) {
427  case STATUS_RESETTING:
428  reset();
429  break;
430 
431  case STATUS_SYNCING:
432  case STATUS_READING:
433  {
434  // It turns out to be important to get the report before checking
435  // to see if it has been too long since the last report. This is
436  // because there is the possibility that some other device running
437  // in the same server may have taken a long time on its last pass
438  // through mainloop(). Trackers that are resetting do this. When
439  // this happens, you can get an infinite loop -- where one tracker
440  // resets and causes the other to timeout, and then it returns the
441  // favor. By checking for the report here, we reset the timestamp
442  // if there is a report ready (ie, if THIS device is still operating).
443  while (get_report()) {}; // Keep getting reports so long as there are more
444 
445  struct timeval current_time;
446  vrpn_gettimeofday(&current_time, NULL);
447  if ( vrpn_TimevalDuration(current_time,timestamp) > POLL_INTERVAL) {
448 
450  // Tell unit 1 to stop, which will cause it to respond.
451  send_command(1,23,0);
453  } else {
454  return;
455  }
456  }
457 
459  sprintf(errmsg,"Timeout... current_time=%ld:%ld, timestamp=%ld:%ld",
460  current_time.tv_sec, static_cast<long>(current_time.tv_usec),
461  timestamp.tv_sec, static_cast<long>(timestamp.tv_usec));
462  VRPN_MSG_ERROR(errmsg);
464  }
465  }
466  break;
467 
468  default:
469  VRPN_MSG_ERROR("Unknown mode (internal error)");
470  break;
471  }
472 }
void server_mainloop(void)
Handles functions that all servers should provide in their mainloop() (ping/pong, for example) Should...
int vrpn_write_characters(int comm, const unsigned char *buffer, size_t bytes)
Write the buffer to the serial port.
Definition: vrpn_Serial.C:643
virtual void report_changes(vrpn_uint32 class_of_service=vrpn_CONNECTION_LOW_LATENCY, const struct timeval time=vrpn_ANALOG_NOW)
Send a report only if something has changed (for servers) Optionally, tell what time to stamp the val...
Definition: vrpn_Analog.C:71
void vrpn_SleepMsecs(double dMsecs)
Definition: vrpn_Shared.C:157
#define POLL_INTERVAL
Definition: vrpn_Zaber.C:32
#define STATUS_READING
Definition: vrpn_Zaber.C:29
VRPN_API int vrpn_unbuffer(const char **buffer, timeval *t)
Utility routine for taking a struct timeval from a buffer that was sent as a message.
Definition: vrpn_Shared.C:312
vrpn_int32 request_channels_m_id
#define STATUS_SYNCING
Definition: vrpn_Zaber.C:28
const vrpn_uint32 vrpn_CONNECTION_RELIABLE
Classes of service for messages, specify multiple by ORing them together Priority of satisfying these...
int vrpn_flush_input_buffer(int comm)
Throw out any characters within the input buffer.
Definition: vrpn_Serial.C:435
virtual void report_changes(vrpn_uint32 class_of_service=vrpn_CONNECTION_RELIABLE)
send report iff changed
Definition: vrpn_Zaber.C:399
#define TIMEOUT_TIME_INTERVAL
Definition: vrpn_Zaber.C:31
Header containing macros formerly duplicated in a lot of implementation files.
vrpn_Serial: Pulls all the serial port routines into one file to make porting to new operating system...
const char * buffer
vrpn_float64 o_channel[vrpn_CHANNEL_MAX]
vrpn_float64 channel[vrpn_CHANNEL_MAX]
Definition: vrpn_Analog.h:38
vrpn_int32 d_ping_message_id
Ask the server if they are there.
Generic connection class not specific to the transport mechanism.
bool send_command(unsigned char devicenum, unsigned char cmd, vrpn_int32 data)
Definition: vrpn_Zaber.C:76
virtual void report(vrpn_uint32 class_of_service=vrpn_CONNECTION_LOW_LATENCY, const struct timeval time=vrpn_ANALOG_NOW)
Send a report whether something has changed or not (for servers) Optionally, tell what time to stamp ...
Definition: vrpn_Analog.C:94
#define VRPN_MSG_WARNING(msg)
unsigned d_bufcount
Definition: vrpn_Zaber.h:24
virtual int reset(void)
Definition: vrpn_Zaber.C:134
All types of client/server/peer objects in VRPN should be derived from the vrpn_BaseClass type descri...
vrpn_int32 num_channel
Definition: vrpn_Analog.h:40
int vrpn_read_available_characters(int comm, unsigned char *buffer, size_t bytes)
Definition: vrpn_Serial.C:512
vrpn_Zaber(const char *name, vrpn_Connection *c, const char *port)
Definition: vrpn_Zaber.C:38
virtual void mainloop()
Called once through each main loop iteration to handle updates.
Definition: vrpn_Zaber.C:420
virtual void report(vrpn_uint32 class_of_service=vrpn_CONNECTION_RELIABLE)
send report whether or not changed
Definition: vrpn_Zaber.C:406
int register_autodeleted_handler(vrpn_int32 type, vrpn_MESSAGEHANDLER handler, void *userdata, vrpn_int32 sender=vrpn_ANY_SENDER)
Registers a handler with the connection, and remembers to delete at destruction.
vrpn_Connection * d_connection
Connection that this object talks to.
This structure is what is passed to a vrpn_Connection message callback.
static int VRPN_CALLBACK handle_request_channels_message(void *userdata, vrpn_HANDLERPARAM p)
Responds to a request to change multiple channels at once.
Definition: vrpn_Zaber.C:363
int send_text_message(const char *msg, struct timeval timestamp, vrpn_TEXT_SEVERITY type=vrpn_TEXT_NORMAL, vrpn_uint32 level=0)
Sends a NULL-terminated text message from the device d_sender_id.
unsigned char d_buffer[512]
Definition: vrpn_Zaber.h:23
unsigned d_expected_chars
Definition: vrpn_Zaber.h:22
struct timeval timestamp
Definition: vrpn_Zaber.h:26
#define vrpn_gettimeofday
Definition: vrpn_Shared.h:89
#define STATUS_RESETTING
Definition: vrpn_Zaber.C:27
struct timeval d_last_poll
Definition: vrpn_Zaber.h:27
vrpn_int32 d_sender_id
Sender ID registered with the connection.
static int VRPN_CALLBACK handle_connect_message(void *userdata, vrpn_HANDLERPARAM p)
Responds to a connection request with a report of the values.
Definition: vrpn_Zaber.C:391
static int VRPN_CALLBACK handle_request_message(void *userdata, vrpn_HANDLERPARAM p)
Responds to a request to change one of the values by setting the channel to that value.
Definition: vrpn_Zaber.C:335
unsigned long vrpn_TimevalDuration(struct timeval endT, struct timeval startT)
Return number of microseconds between startT and endT.
Definition: vrpn_Shared.C:129
virtual int get_report(void)
Definition: vrpn_Zaber.C:226
vrpn_int32 convert_bytes_to_reading(const unsigned char *buf)
Convert the four bytes that have been read into a signed integer value.
Definition: vrpn_Zaber.C:114
#define VRPN_MSG_ERROR(msg)
struct timeval timestamp
Definition: vrpn_Analog.h:41