vrpn 07.35
Virtual Reality Peripheral Network
 
Loading...
Searching...
No Matches
vrpn_Poser_Tek4662.C
Go to the documentation of this file.
1#include <math.h> // for floor
2#include <stdio.h> // for fprintf, stderr, NULL
3#include <string.h> // for strlen, memcpy
4
5#include "vrpn_BaseClass.h" // for ::vrpn_TEXT_ERROR
6#include "vrpn_Connection.h" // for vrpn_HANDLERPARAM, etc
7// Include vrpn_Shared.h _first_ to avoid conflicts with sys/time.h
8// and unistd.h
9#include "vrpn_Shared.h" // for timeval, vrpn_unbuffer, etc
10#include "vrpn_Types.h" // for vrpn_float64, vrpn_uint16
11
12#ifdef _WIN32
13#ifndef _WIN32_WCE
14#include <io.h>
15#endif
16#endif
17
18#include "vrpn_Poser_Tek4662.h"
19
20//#define VERBOSE
21
26
27// Plotter motion constants
28const double COUNTS_PER_METER = 1.0 / ( (15.0 / 4095.0) * ( 0.0254 / 1.0 ) );
29const double MAX_X = 0.381; // Range of the X axis is 15 inches
30const double MAX_Y = 0.254; // Range of the Y axis is 10 inches
31const double VELOCITY= (1/0.06144) * (1/0.00254); // Meters per second
32
33// Constants used as characters to communicate to the plotter
34const unsigned char ESC = 27;
35//const unsigned char BELL = '7';
36const unsigned char DEVICE = 'A';
37const unsigned char GS = 29; //< Puts the plotter into graphics mode
38const unsigned char ZERO = 0;
39const unsigned char ZEROES[] = "\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0";
40const unsigned char PLOTTER_ON[] = { ESC, DEVICE, 'E', ZERO };
41const unsigned char RESET[] = { ESC, DEVICE, 'N', ZERO };
42const unsigned char GIN[] = { ESC, DEVICE, 'M', ZERO };
43const unsigned char MOVE_TEMPLATE[] = { GS, 0x20, 0x60, 0x60, 0x20, 0x40 };
44const int DATA_RECORD_LENGTH = 7;
45
46// Constants used to do bit manipulation
47//const unsigned BITFOUR = 1 << 4;
48//const unsigned BITFIVE = 1 << 5;
49const unsigned LOWFIVEBITS = 0x001f;
50const unsigned LOWTWOBITS = 0x0003;
51
53// Server Code
54
56 const char *port, int baud, int bits,
57 vrpn_SER_PARITY parity) :
58 vrpn_Poser(name, c),
59 vrpn_Tracker(name, c),
60 d_serial_fd(-1),
64{
65 // Make sure that we have a valid connection
66 if (d_connection == NULL) {
67 fprintf(stderr,"vrpn_Poser_Tek4662: No connection\n");
68 return;
69 }
70
71 // Check the port name;
72 if (port == NULL) {
73 fprintf(stderr,"vrpn_Poser_Tek4662: NULL port name\n");
75 return;
76 }
77
78 // Open the serial port we're going to use
79 if ( (d_serial_fd=vrpn_open_commport(port, baud, bits, parity)) == -1) {
80 fprintf(stderr,"vrpn_Poser_Tek4662: Cannot Open serial port (%s)\n", port);
82 }
83
84 // Register a handler for the position change callback for this device
87 fprintf(stderr,"vrpn_Poser_Server: can't register position handler\n");
88 d_connection = NULL;
89 }
90
91 // Register a handler for the velocity change callback for this device
94 fprintf(stderr,"vrpn_Poser_Server: can't register velocity handler\n");
95 d_connection = NULL;
96 }
97
98 // Set up the workspace max and min values
99 p_pos_min[0] = 0.0; p_pos_max[0] = MAX_X;
100 p_pos_min[1] = 0.0; p_pos_max[1] = MAX_Y;
101 p_pos_min[2] = 0.0; p_pos_max[2] = 0.0; // There is no Z axis
102 p_vel_min[0] = p_vel_max[0] = VELOCITY; // Meters per second
103 p_vel_min[1] = p_vel_max[1] = VELOCITY; // Meters per second
104 p_vel_min[2] = p_vel_max[2] = 0.0; // No motion in Z
105 p_pos_rot_min[0] = 0.0; p_pos_rot_max[0] = 0.0; // There is no rotation
106 p_pos_rot_min[1] = 0.0; p_pos_rot_max[1] = 0.0; // There is no rotation
107 p_pos_rot_min[2] = 0.0; p_pos_rot_max[2] = 0.0; // There is no rotation
108 p_vel_rot_min[0] = 0.0; p_vel_rot_max[0] = 0.0; // There is no rotation
109 p_vel_rot_min[1] = 0.0; p_vel_rot_max[1] = 0.0; // There is no rotation
110 p_vel_rot_min[2] = 0.0; p_vel_rot_max[2] = 0.0; // There is no rotation
111
112 // Reset the device and find out what time it is
115}
116
117
119{
120 // Close com port when destroyed.
121 if (d_serial_fd != -1) {
123 }
124}
125
126// This parses the pen position and location from a GIN report.
127// It first verifies that the high-order tag bits match what is
128// expected on all bytes (see page 2-27 of the manual). If they
129// do not, it returns false; otherwise, true.
130static bool interpret_GIN_bytes(const unsigned char inbuf[], bool &pen_down, float &x, float &y)
131{
132 // Check the high-order tag bits to make sure they are valid.
133 for (int i = 0; i < 6; i++) {
134 if ( (inbuf[i] & 0x60) != 0x20) { return false; }
135 }
136 if ( (inbuf[6] & 0x60) != 0x40) { return false; }
137
138 // Interpret the pen-down bit.
139 pen_down = (inbuf[6] & (1<<2)) != 0;
140
141 // Unpack the X and Y coordinates from the plotter report as described on
142 // page 2-27 of the manual. Note that in GIN mode, the plotter only uses
143 // the higher-order 12 bits; the lower 4 are set to zero. This means that
144 // we shift three bit past the right and ignore the lowest bit.
145 vrpn_uint16 x_int, y_int;
146 x_int = static_cast<unsigned short>(((inbuf[0] & LOWFIVEBITS) << 7) | ((inbuf[2] & LOWFIVEBITS) << 2) | ((inbuf[4] & LOWFIVEBITS) >> 3));
147 y_int = static_cast<unsigned short>(((inbuf[1] & LOWFIVEBITS) << 7) | ((inbuf[3] & LOWFIVEBITS) << 2) | ((inbuf[5] & LOWFIVEBITS) >> 3));
148
149 // Convert the position from counts to meters. This goes through inches, which is the native
150 // plotter unit.
151 x = (float)( x_int / COUNTS_PER_METER );
152 y = (float)( y_int / COUNTS_PER_METER );
153
154#if 0
155 if (pen_down) { printf("XXX pen down\n"); } else { printf("XXX pen up\n"); }
156 printf("XXX At %f, %f\n", x, y);
157#endif
158 return true;
159}
160
162{
163 // Wait a little and then flush the input buffer so we don't get extra reports
164 // from before the reset.
165 vrpn_SleepMsecs(100);
167
168 // Send a bunch of zeroes to clear out the input buffer,
169 // then a "Plotter on", then a "Reset" to the plotter.
172 vrpn_write_characters( d_serial_fd, PLOTTER_ON, strlen((const char*)PLOTTER_ON) );
173 vrpn_write_characters( d_serial_fd, RESET, strlen((const char*)RESET) );
175
176 // Request a position message from the plotter and then wait
177 // until it responds. Make sure we get a good response. If
178 // so, then send a Tracker message with the specified position
179 // and go into SYNCING mode. If not, then reset again.
180 vrpn_write_characters( d_serial_fd, GIN, strlen((const char*)GIN) );
182 unsigned char inbuf[DATA_RECORD_LENGTH];
183 struct timeval wait_time = { 1, 0 };
184 int bufcount = vrpn_read_available_characters( d_serial_fd, inbuf, sizeof(inbuf), &wait_time);
185 if (bufcount != sizeof(inbuf)) {
186 fprintf(stderr,"vrpn_Poser_Tek4662::reset(): Expected %d characters, got %d\n",
187 static_cast<int>(sizeof(inbuf)), bufcount);
188 } else {
189 // Parse the input to find our position and store it in the tracker
190 // position.
191 float x,y;
192 bool pen_down;
193
195 if (!interpret_GIN_bytes(inbuf, pen_down, x, y)) {
196 send_text_message("vrpn_Poser_Tek4662: Error resetting", timestamp, vrpn_TEXT_ERROR);
197 return;
198 } else {
199 send_text_message("vrpn_Poser_Tek4662: Reset correctly", timestamp, vrpn_TEXT_ERROR);
200 }
201
202 // Set and send tracker position.
203 pos[0] = x; pos[1] = y; pos[2] = 0;
204 d_quat[0] = d_quat[1] = d_quat[2] = 0; d_quat[3] = 1;
205 if (d_connection) {
206 char msgbuf[1000];
207 int len = vrpn_Tracker::encode_to(msgbuf);
208 if (d_connection->pack_message(len, timestamp,
209 position_m_id, d_sender_id, msgbuf,
211 fprintf(stderr,"vrpn_Poser_Tek4662: cannot write message: tossing\n");
212 }
213 } else {
214 fprintf(stderr,"vrpn_Poser_Tek4662: No valid connection\n");
215 }
216
217 // We're waiting for the first character!
220 }
221}
222
224{
225 struct timeval now;
226
227 // Send new positions as needed, in coordination with the callback handler.
229
230 // Figure out the integer location corresponding to the new poser
231 // position requested. We've already tested to make sure that the
232 // position is in bounds.
233 vrpn_uint16 x_int, y_int;
234 x_int = (vrpn_uint16)floor( p_pos[0] * COUNTS_PER_METER );
235 y_int = (vrpn_uint16)floor( p_pos[1] * COUNTS_PER_METER );
236
237 // Send a command to the plotter to move to the new location.
238 // Request the new location be reported.
239 // The first two characters tell it to go into graph mode; the
240 // last five include the correct upper-two bits for each byte,
241 // with the rest filled in as described on page 2-26 in the manual.
242 unsigned char MOVE[sizeof(MOVE_TEMPLATE)];
243 memcpy(MOVE, MOVE_TEMPLATE, sizeof(MOVE));
244 // Five MSB of y into 5 LSB
245 MOVE[1] |= y_int >> 7;
246 // Two LSB of y into bits 2-3, two LSB of x into bits 0-1
247 MOVE[2] |= (x_int & LOWTWOBITS);
248 MOVE[2] |= (y_int & LOWTWOBITS) << 2;
249 // The rest of the Y bits (five in the middle) to LSB
250 MOVE[3] |= (y_int >> 2) & LOWFIVEBITS;
251 // High-order x bits into low-order bits
252 MOVE[4] |= x_int >> 7;
253 // Intermediate X bits into lower-order bits
254 MOVE[5] |= (x_int >> 2) & LOWFIVEBITS;
255 vrpn_write_characters(d_serial_fd, MOVE, sizeof(MOVE));
256 vrpn_write_characters(d_serial_fd, GIN, strlen((const char*)GIN) );
257
258 // Record the fact that we're moving so that we won't send a new
259 // command until the move completes.
260#if 0
261 printf("XXX Going to %f,%f (%d, %d)\n", p_pos[0], p_pos[1], x_int, y_int);
262#endif
265 }
266
267 // Listen for any new reports from the device. Recall that we can get
268 // partial results reported, especially with a 1200-baud or slower serial
269 // connection like we have on this device.
270 // This assumes that the plotter does not tell us where it is until
271 // it finishes moving there. This assumption looks valid from short
272 // tests.
273 if (status == vrpn_Poser_Tek4662_SYNCING) { // Try to get first byte
274 // Zero timeout, poll for any available characters
275 struct timeval timeout = {0, 0};
276 if (1 == vrpn_read_available_characters(d_serial_fd, d_inbuf, 1, &timeout)) {
277 d_inbufcounter = 1; //< Ignore the status byte for the following record
280 } else {
281 d_inbufcounter = 0;
282 }
283 }
285 // Zero timeout, poll for any available characters
286 struct timeval timeout = {0, 0};
289
290 if (result < 0) {
291 send_text_message("vrpn_Poser_Tek4662: Error reading", timestamp, vrpn_TEXT_ERROR);
293 } else {
294 d_inbufcounter += result;
296 d_inbufcounter = 0;
298
299 // Parse the input to find our position and store it in the tracker
300 // position.
301 float x,y;
302 bool pen_down;
303
304 if (!interpret_GIN_bytes(d_inbuf, pen_down, x, y)) {
305 send_text_message("vrpn_Poser_Tek4662: Error parsing position", timestamp, vrpn_TEXT_ERROR);
306 return;
307 }
308
309 // Set and send tracker position.
310 pos[0] = x; pos[1] = y; pos[2] = 0;
311 d_quat[0] = d_quat[1] = d_quat[2] = 0; d_quat[3] = 1;
312 if (d_connection) {
313 char msgbuf[1000];
314 int len = vrpn_Tracker::encode_to(msgbuf);
315 if (d_connection->pack_message(len, timestamp,
316 position_m_id, d_sender_id, msgbuf,
318 fprintf(stderr,"vrpn_Poser_Tek4662: cannot write message: tossing\n");
319 }
320 } else {
321 fprintf(stderr,"vrpn_Poser_Tek4662: No valid connection\n");
322 }
324 }
325 }
326 }
327
328 // Request the position four times per second when we're outside of
329 // the position command. Remember to increment the number of outstanding
330 // requests so the position-request code keeps working. This will let
331 // the user move the plotter around with the joystick and have the
332 // tracker follow around. It will also keep sending reports so that a
333 // client the connects after reset will know where the plotter is fairly
334 // quickly.
335
337 vrpn_gettimeofday(&now, NULL);
338 if (vrpn_TimevalDuration(now, timestamp) > 250000L) {
339 // Record the fact that we're asking so that we won't send a new
340 // command until the response completes.
341 vrpn_write_characters(d_serial_fd, GIN, strlen((const char*)GIN) );
343 }
344 }
345
346 // We need a watchdog timer to make sure that the plotter doesn't just
347 // die on us in the middle of a move. If we don't hear from it for 5 seconds,
348 // reset it.
349 vrpn_gettimeofday(&now, NULL);
350 if (vrpn_TimevalDuration(now, timestamp) > 5000000L) {
351 send_text_message("vrpn_Poser_Tek4662: Device timeout (resetting)", now, vrpn_TEXT_ERROR);
353 return;
354 }
355}
356
358{
359 // Call the generic server mainloop routine, since this is a server
361
362 // Depending on what mode we're in, do our thing.
363
364 switch (status) {
366 reset();
367 break;
368
371 run();
372 break;
373
375 break;
376
377 default:
378 fprintf(stderr,"vrpn_Poser_Tek4662: Unknown status (%d)\n", status);
380 break;
381 }
382}
383
386{
388 const char* params = (p.buffer);
389 int i;
390 // Fill in the parameters to the poser from the message
391 if (p.payload_len != (7 * sizeof(vrpn_float64)) ) {
392 fprintf(stderr,"vrpn_Poser_Server: change message payload error\n");
393 fprintf(stderr," (got %d, expected %d)\n",
394 p.payload_len, static_cast<int>(7 * sizeof(vrpn_float64)) );
395 return -1;
396 }
397 me->p_timestamp = p.msg_time;
398
399 for (i = 0; i < 3; i++) {
400 vrpn_unbuffer(&params, &me->p_pos[i]);
401 }
402 for (i = 0; i < 4; i++) {
403 vrpn_unbuffer(&params, &me->p_quat[i]);
404 }
405
406 // Check the pose against the max and min values of the workspace
407 for (i = 0; i < 3; i++) {
408 if (me->p_pos[i] < me->p_pos_min[i]) {
409 me->p_pos[i] = me->p_pos_min[i];
410 }
411 else if (me->p_pos[i] > me->p_pos_max[i]) {
412 me->p_pos[i] = me->p_pos_max[i];
413 }
414 }
415
416 // Set up so that run() will move the plotter to the requested location.
417 me->d_new_location_requested = true;
418
419 return 0;
420}
421
424{
426 const char* params = (p.buffer);
427 int i;
428
429 // Fill in the parameters to the poser from the message
430 if (p.payload_len != (8 * sizeof(vrpn_float64)) ) {
431 fprintf(stderr,"vrpn_Poser_Server: velocity message payload error\n");
432 fprintf(stderr," (got %d, expected %d)\n",
433 p.payload_len, static_cast<int>(8 * sizeof(vrpn_float64)) );
434 return -1;
435 }
436 me->p_timestamp = p.msg_time;
437
438 for (i = 0; i < 3; i++) {
439 vrpn_unbuffer(&params, &me->p_vel[i]);
440 }
441 for (i = 0; i < 4; i++) {
442 vrpn_unbuffer(&params, &me->p_vel_quat[i]);
443 }
444 vrpn_unbuffer(&params, &me->p_vel_quat_dt);
445
446 // Check the velocity against the max and min values of the workspace
447 for (i = 0; i < 3; i++) {
448 if (me->p_vel[i] < me->p_vel_min[i]) {
449 me->p_vel[i] = me->p_vel_min[i];
450 }
451 else if (me->p_vel[i] > me->p_vel_max[i]) {
452 me->p_vel[i] = me->p_vel_max[i];
453 }
454 }
455
456 // No response to this message.
457 return 0;
458}
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.
unsigned char d_inbuf[1024]
static int VRPN_CALLBACK handle_change_message(void *userdata, vrpn_HANDLERPARAM p)
static int VRPN_CALLBACK handle_vel_change_message(void *userdata, vrpn_HANDLERPARAM p)
vrpn_Poser_Tek4662(const char *name, vrpn_Connection *c, const char *port, int baud=1200, int bits=8, vrpn_SER_PARITY parity=vrpn_SER_PARITY_NONE)
virtual void mainloop()
Called once through each main loop iteration to handle updates. Remote object mainloop() should call ...
vrpn_float64 p_pos_min[3]
Definition vrpn_Poser.h:47
vrpn_float64 p_pos_rot_max[3]
Definition vrpn_Poser.h:47
vrpn_float64 p_vel_min[3]
Definition vrpn_Poser.h:48
vrpn_float64 p_pos[3]
Definition vrpn_Poser.h:39
vrpn_float64 p_pos_rot_min[3]
Definition vrpn_Poser.h:47
vrpn_float64 p_vel_rot_min[3]
Definition vrpn_Poser.h:48
vrpn_float64 p_vel_max[3]
Definition vrpn_Poser.h:48
vrpn_float64 p_vel_quat[4]
Definition vrpn_Poser.h:41
vrpn_Poser(const char *name, vrpn_Connection *c=NULL)
Definition vrpn_Poser.C:31
vrpn_float64 p_vel_quat_dt
Definition vrpn_Poser.h:42
vrpn_float64 p_vel_rot_max[3]
Definition vrpn_Poser.h:48
vrpn_float64 p_pos_max[3]
Definition vrpn_Poser.h:47
vrpn_int32 req_position_m_id
Definition vrpn_Poser.h:33
vrpn_float64 p_vel[3]
Definition vrpn_Poser.h:40
struct timeval p_timestamp
Definition vrpn_Poser.h:43
vrpn_int32 req_velocity_m_id
Definition vrpn_Poser.h:35
vrpn_float64 p_quat[4]
Definition vrpn_Poser.h:39
virtual int encode_to(char *buf)
vrpn_float64 d_quat[4]
vrpn_Tracker(const char *name, vrpn_Connection *c=NULL, const char *tracker_cfg_file_name=NULL)
vrpn_float64 pos[3]
struct timeval timestamp
vrpn_int32 position_m_id
This structure is what is passed to a vrpn_Connection message callback.
const char * buffer
struct timeval msg_time
All types of client/server/peer objects in VRPN should be derived from the vrpn_BaseClass type descri...
@ vrpn_TEXT_ERROR
const vrpn_uint32 vrpn_CONNECTION_LOW_LATENCY
const int vrpn_Poser_Tek4662_SYNCING
const unsigned char DEVICE
const unsigned char PLOTTER_ON[]
const unsigned char ZERO
const unsigned char GS
const unsigned char ZEROES[]
const unsigned char ESC
const unsigned char GIN[]
const int vrpn_Poser_Tek4662_RUNNING
const unsigned char MOVE_TEMPLATE[]
const unsigned LOWTWOBITS
const double MAX_Y
const int vrpn_Poser_Tek4662_FAIL
const unsigned char RESET[]
const double VELOCITY
const double COUNTS_PER_METER
const unsigned LOWFIVEBITS
const int vrpn_Poser_Tek4662_RESETTING
const double MAX_X
class VRPN_API vrpn_Connection
int vrpn_write_characters(int comm, const unsigned char *buffer, size_t bytes)
Write the buffer to the serial port.
int vrpn_close_commport(int comm)
int vrpn_flush_input_buffer(int comm)
Throw out any characters within the input buffer.
int vrpn_drain_output_buffer(int comm)
Wait until all of the characters in the output buffer are sent, then return.
int vrpn_read_available_characters(int comm, unsigned char *buffer, size_t bytes)
int vrpn_flush_output_buffer(int comm)
Throw out any characters (do not send) within the output buffer.
int vrpn_open_commport(const char *portname, long baud, int charsize, vrpn_SER_PARITY parity, bool rts_flow)
Open a serial port, given its name and baud rate.
Definition vrpn_Serial.C:54
vrpn_SER_PARITY
Definition vrpn_Serial.h:15
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
#define DATA_RECORD_LENGTH
Definition vrpn_Tng3.C:29