vrpn 07.35
Virtual Reality Peripheral Network
 
Loading...
Searching...
No Matches
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
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
76bool 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
94bool 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
114vrpn_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++;
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
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
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;
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;
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
398
399void vrpn_Zaber::report_changes(vrpn_uint32 class_of_service)
400{
402
403 vrpn_Analog::report_changes(class_of_service);
404}
405
406void vrpn_Zaber::report(vrpn_uint32 class_of_service)
407{
409
410 vrpn_Analog::report(class_of_service);
411}
412
419
421{
422 char errmsg[256];
423
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}
vrpn_float64 o_channel[vrpn_CHANNEL_MAX]
vrpn_int32 request_channels_m_id
vrpn_Analog_Output(const char *name, vrpn_Connection *c=NULL)
vrpn_float64 channel[vrpn_CHANNEL_MAX]
Definition vrpn_Analog.h:38
struct timeval timestamp
Definition vrpn_Analog.h:41
vrpn_int32 num_channel
Definition vrpn_Analog.h:40
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
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
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.
vrpn_int32 d_sender_id
Sender ID registered with the connection.
void server_mainloop(void)
Handles functions that all servers should provide in their mainloop() (ping/pong, for example) Should...
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.
vrpn_int32 d_ping_message_id
Ask the server if they are there.
Generic connection class not specific to the transport mechanism.
vrpn_Serial_Analog(const char *name, vrpn_Connection *connection, const char *port, int baud=9600, int bits=8, vrpn_SER_PARITY parity=vrpn_SER_PARITY_NONE, bool rts_flow=false)
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
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
virtual int reset(void)
Definition vrpn_Zaber.C:134
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
vrpn_Zaber(const char *name, vrpn_Connection *c, const char *port)
Definition vrpn_Zaber.C:38
virtual void report_changes(vrpn_uint32 class_of_service=vrpn_CONNECTION_RELIABLE)
send report iff changed
Definition vrpn_Zaber.C:399
virtual int get_report(void)
Definition vrpn_Zaber.C:226
virtual void report(vrpn_uint32 class_of_service=vrpn_CONNECTION_RELIABLE)
send report whether or not changed
Definition vrpn_Zaber.C:406
unsigned char d_buffer[512]
Definition vrpn_Zaber.h:23
unsigned d_bufcount
Definition vrpn_Zaber.h:24
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
virtual void mainloop()
Called once through each main loop iteration to handle updates.
Definition vrpn_Zaber.C:420
struct timeval timestamp
Definition vrpn_Zaber.h:26
struct timeval d_last_poll
Definition vrpn_Zaber.h:27
bool send_command(unsigned char devicenum, unsigned char cmd, vrpn_int32 data)
Definition vrpn_Zaber.C:76
unsigned d_expected_chars
Definition vrpn_Zaber.h:22
This structure is what is passed to a vrpn_Connection message callback.
const char * buffer
#define STATUS_SYNCING
#define STATUS_READING
#define STATUS_RESETTING
All types of client/server/peer objects in VRPN should be derived from the vrpn_BaseClass type descri...
@ vrpn_TEXT_ERROR
#define TIMEOUT_TIME_INTERVAL
const vrpn_uint32 vrpn_CONNECTION_RELIABLE
Classes of service for messages, specify multiple by ORing them together Priority of satisfying these...
#define POLL_INTERVAL
Definition vrpn_IDEA.C:26
Header containing macros formerly duplicated in a lot of implementation files.
#define VRPN_MSG_ERROR(msg)
#define VRPN_MSG_WARNING(msg)
int vrpn_write_characters(int comm, const unsigned char *buffer, size_t bytes)
Write the buffer to the serial port.
int vrpn_flush_input_buffer(int comm)
Throw out any characters within the input buffer.
int vrpn_read_available_characters(int comm, unsigned char *buffer, size_t bytes)
vrpn_Serial: Pulls all the serial port routines into one file to make porting to new operating system...
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.
unsigned long vrpn_TimevalDuration(struct timeval endT, struct timeval startT)
Return number of microseconds between startT and endT.
void vrpn_SleepMsecs(double dMilliSecs)
#define vrpn_gettimeofday
Definition vrpn_Shared.h:99