vrpn  07.33
Virtual Reality Peripheral Network
vrpn_3DMicroscribe.C
Go to the documentation of this file.
1 
2 #include <math.h> // for cos, sin
3 #include <stdio.h> // for fprintf, stderr
4 #include <string.h> // for strcmp, NULL
5 
6 #include "vrpn_3DMicroscribe.h"
7 #include "vrpn_BaseClass.h" // for ::vrpn_TEXT_ERROR
8 #include "vrpn_Shared.h" // for timeval, vrpn_gettimeofday
9 #include "vrpn_MessageMacros.h"
10 
11 #ifdef VRPN_USE_MICROSCRIBE
12 #include "armdll32.h"
13 #endif
14 
15 
16 // turn on for debugging code, leave off otherwise
17 #undef VERBOSE
18 
19 #if defined(VERBOSE)
20 #include <ctype.h> // for isprint()
21 
22 #define DEBUG 1
23 #endif
24 
25 // Defines the modes in which the box can find itself.
26 #define STATUS_RESETTING (-1) // Resetting the device
27 #define STATUS_SYNCING (0) // Looking for the first char of report
28 #define STATUS_READING (1) // Looking for the rest of the report
29 #define MAX_TIME_INTERVAL (2000000) // max time between reports (usec)
30 
31 #define MM_TO_METERS 0.001
32 
33 #define VR_PI 3.14159265359
34 inline float pcos(float x) {return (float)cos(double(x)*VR_PI/180.0f);}
35 inline float psin(float x) {return (float)sin(double(x)*VR_PI/180.0f);}
36 
37 // This creates a vrpn_3DMicroscribe and sets it to reset mode.
39  const char * Port, long int BaudRate,
40  float OffsetX/* = 0.0f*/,
41  float OffsetY/* = 0.0f*/,
42  float OffsetZ/* = 0.0f*/,
43  float Scale/*=1.0f*/):
44  vrpn_Tracker(name, c),
45  vrpn_Button_Filter(name, c),
46  _numbuttons(2)
47 {
48  // Set the parameters in the parent classes
50 
51  if(!strcmp(Port, "COM1") )
52  m_PortNumber=1;
53  else if(!strcmp(Port, "COM2") )
54  m_PortNumber=2;
55  else if(!strcmp(Port, "COM3") )
56  m_PortNumber=3;
57  else if(!strcmp(Port, "COM4") )
58  m_PortNumber=4;
59  m_BaudRate=BaudRate;
60  m_OffSet[0]=OffsetX; m_OffSet[1]=OffsetY; m_OffSet[2]=OffsetZ;
61  m_Scale=Scale;
62 
63  // Set the status of the buttons and analogs to 0 to start
64  clear_values();
65 
66 #ifdef VRPN_USE_MICROSCRIBE
67  int iResult;
68  iResult=ArmStart(NULL);
69  if(ARM_SUCCESS != iResult)
70  {
71  //error starting the MicroScribe drivers
72  VRPN_MSG_ERROR( "Unable to start MicroScribe ArmDll32." );
73  return;
74  }
75 
76  //don't use error handlers
77  iResult = ArmSetErrorHandlerFunction(NO_HCI_HANDLER, NULL);
78  iResult = ArmSetErrorHandlerFunction(BAD_PORT_HANDLER, NULL);
79  iResult = ArmSetErrorHandlerFunction(CANT_OPEN_HANDLER, NULL);
80  iResult = ArmSetErrorHandlerFunction(CANT_BEGIN_HANDLER, NULL);
81 
82  //connect to the correct port
83  switch(m_PortNumber)
84  {
85  case 1:
86  iResult = ArmConnect(1, m_BaudRate);
87  break;
88  case 2:
89  iResult = ArmConnect(2, m_BaudRate);
90  break;
91  case 3:
92  iResult = ArmConnect(3, m_BaudRate);
93  break;
94  case 4:
95  iResult = ArmConnect(4, m_BaudRate);
96  break;
97  default:
98  iResult = ArmConnect(0, 0); //try all available ports and baud rates
99  break;
100  }
101 
102  if(ARM_SUCCESS != iResult)
103  {
104  //error connecting, end the thread
105  ArmEnd();
106  VRPN_MSG_ERROR( "Unable to connect to the MicroScribe." );
107  return;
108  }
109 
110 #endif
111  // Set the mode to reset
113 }
114 
116 {
117 }
118 
119 // This routine will reset the 3DMicroscribe, zeroing the Origin position,
120 // mode.
122 {
123 #ifdef VRPN_USE_MICROSCRIBE
124  int iResult;
125  // ARM_FULL: ArmDll32 calculates and updates the Cartesian position and orientation of the stylus tip.
126  iResult = ArmSetUpdate(ARM_FULL);
127 
128  if(iResult != ARM_SUCCESS)
129  {
130  //error setting the update type, disconnect and end the thread
131  VRPN_MSG_ERROR( "Unable to set the update type for the MicroScribe." );
132  return -1;
133  }
134 
135  //use mm instead of inches
136  ArmSetLengthUnits(ARM_MM);
137  //use radians instead of degrees
138  //ArmSetAngleUnits(ARM_RADIANS);
139 
140  //get the position of the tip
141  length_3D tipPosition;
142  angle_3D tipVector;
143 
144  iResult = ArmGetTipPosition(&tipPosition); //retrieves the current stylus tip position in Cartesian coordinates
145  iResult = ArmGetTipOrientationUnitVector(&tipVector); //retrieves the current stylus tip's unit vector orientation
146 
147  if(iResult == ARM_NOT_CONNECTED)
148  {
149  //error connecting
150  VRPN_MSG_ERROR( "MicroScribe connection lost!" );
151  return -1;
152  }
153 
154 #endif
155  // We're now waiting for a response from the box
157 
158  vrpn_gettimeofday(&timestamp, NULL); // Set watchdog now
159  return 0;
160 }
161 
162 
163 
164 // This function will read characters until it has a full report, then
165 // put that report into the time, analog, or button fields and call
166 // the report methods on these. The time stored is that of
167 // the first character received as part of the report.
168 // Reports start with different characters, and the length of the report
169 // depends on what the first character of the report is. We switch based
170 // on the first character of the report to see how many more to expect and
171 // to see how to handle the report.
172 // Returns 1 if there is a complete report found, 0 otherwise. This is
173 // so that the calling routine can know to check again at the end of complete
174 // reports to see if there is more than one report buffered up.
175 
177 {
178 #ifdef VRPN_USE_MICROSCRIBE
179  length_3D tipPosition;
180  angle_3D tipOri;
181  DWORD buts;
182  int iResult = ArmGetTipPosition(&tipPosition); //retrieves the current stylus tip position in Cartesian coordinates
183  iResult = ArmGetTipOrientation(&tipOri); //retrieves the current stylus tip's unit vector orientation
184  iResult = ArmGetButtonsState(&buts);
185  if(iResult == ARM_NOT_CONNECTED)
186  {
187  //error connecting
188  VRPN_MSG_ERROR( "MicroScribe connection lost!" );
189  return 0;
190  }
191 
192  //set the position, considering the scale, offset and origin matrix
193  pos[0] = (tipPosition.y * m_Scale + m_OffSet[0])* MM_TO_METERS ;
194  pos[1] = (tipPosition.z * m_Scale + m_OffSet[1])* MM_TO_METERS;
195  pos[2] = (tipPosition.x * m_Scale + m_OffSet[2])* MM_TO_METERS;
196  //vPosition = m_Matrix * vPosition + m_vPlaneOffset; extending the microscribe onto a plane
197 
198  //set the orientation, considering the origin matrix
199  float ori[3]={tipOri.y, tipOri.z, tipOri.x};
200  ConvertOriToQuat(ori);
201 
202 
203  status = STATUS_READING; // ready to process event packet
204  vrpn_gettimeofday(&timestamp, NULL); // set timestamp of this event
205 
206  buttons[0] = ((buts & 0x02) != 0); // button 1
207  buttons[1] = ((buts & 0x01) != 0); // button 2
208 
209 #endif
210 
211  report_changes(); // Report updates to VRPN
212  return 0;
213 }
214 
216 {
217  float real0,real1,real2,real;
218  float imag0,imag1,imag2,imag[3];
219 
220  real0= pcos(ori[0]/2);
221  real1= pcos(ori[1]/2);
222  real2= pcos(ori[2]/2);
223 
224  imag0 = psin(ori[0]/2);
225  imag1 = psin(ori[1]/2);
226  imag2 = psin(ori[2]/2);
227 
228  // merge the first two quats
229  real = real0 * real1 ;
230 
231  if ( real > 1 )
232  real = 1;
233  else if ( real < -1 )
234  real = -1;
235 
236  imag[0] = imag0 * real1;
237  imag[1] = real0 * imag1;
238  imag[2] = imag0 * imag1;
239 
240  // merge previous result with last quat
241 
242  d_quat[0] = real * real2 - imag[2] * imag2 ;
243 
244  if ( d_quat[0] > 1 )
245  d_quat[0] = 1;
246  else if ( d_quat[0] < -1 )
247  d_quat[0] = -1;
248 
249  d_quat[1] = imag[0] * real2 + imag[1] * imag2;
250  d_quat[2] = imag[1] * real2 - imag[0] * imag2;
251  d_quat[3] = real * imag2 + imag[2] * real2;
252 
253 }
254 
255 void vrpn_3DMicroscribe::report_changes(vrpn_uint32 class_of_service)
256 {
259 
261  if (d_connection) {
262  char msgbuf[1000];
263  int len = vrpn_Tracker::encode_to(msgbuf);
265  position_m_id, d_sender_id, msgbuf,
266  class_of_service)) {
267  VRPN_MSG_ERROR("Tracker: cannot write message: tossing\n");
268  }
269  } else {
270  VRPN_MSG_ERROR("Tracker: No valid connection\n");
271  }
272 }
273 
274 void vrpn_3DMicroscribe::report(vrpn_uint32 /*class_of_service*/)
275 {
278 }
279 
280 // This routine is called each time through the server's main loop. It will
281 // take a course of action depending on the current status of the 3DMicroscribe,
282 // either trying to reset it or trying to get a reading from it.
284 {
285  server_mainloop();
286 
287  switch(status) {
288  case STATUS_RESETTING:
289  reset();
290  break;
291 
292  case STATUS_SYNCING:
293  case STATUS_READING:
294  // Keep getting reports until all full reports are read.
295  while (get_report()) {};
296  break;
297 
298  default:
299  fprintf(stderr,"vrpn_3DMicroscribe: Unknown mode (internal error)\n");
300  break;
301  }
302 }
303 
304 
void server_mainloop(void)
Handles functions that all servers should provide in their mainloop() (ping/pong, for example) Should...
vrpn_3DMicroscribe(const char *name, vrpn_Connection *c, const char *Port, long int BaudRate, float OffsetX=0.0f, float OffsetY=0.0f, float OffsetZ=0.0f, float Scale=1.0f)
vrpn_int32 num_buttons
Definition: vrpn_Button.h:47
virtual void clear_values(void)
Set all buttons, analogs and encoders back to 0.
int m_PortNumber
port number
struct timeval timestamp
Time of the last report from the device.
Header containing macros formerly duplicated in a lot of implementation files.
#define STATUS_SYNCING
#define VR_PI
vrpn_float64 pos[3]
Definition: vrpn_Tracker.h:95
long int m_BaudRate
baud rate
virtual int reset(void)
Set device back to starting config.
Generic connection class not specific to the transport mechanism.
float psin(float x)
All types of client/server/peer objects in VRPN should be derived from the vrpn_BaseClass type descri...
virtual void report_changes(void)
Definition: vrpn_Button.C:382
virtual void report_changes(void)
Definition: vrpn_Button.C:422
vrpn_Connection * d_connection
Connection that this object talks to.
virtual int pack_message(vrpn_uint32 len, struct timeval time, vrpn_int32 type, vrpn_int32 sender, const char *buffer, vrpn_uint32 class_of_service)
Pack a message that will be sent the next time mainloop() is called. Turn off the RELIABLE flag if yo...
void ConvertOriToQuat(float ori[3])
virtual int encode_to(char *buf)
Definition: vrpn_Tracker.C:533
struct timeval timestamp
Definition: vrpn_Button.h:48
#define STATUS_RESETTING
int _numbuttons
How many buttons to open.
#define vrpn_gettimeofday
Definition: vrpn_Shared.h:89
virtual void mainloop()
Called once through each main loop iteration to handle updates.
All button servers should derive from this class, which provides the ability to turn any of the butto...
Definition: vrpn_Button.h:65
vrpn_int32 d_sender_id
Sender ID registered with the connection.
virtual void report(vrpn_uint32 class_of_service=vrpn_CONNECTION_LOW_LATENCY)
send report whether or not changed
#define STATUS_READING
struct timeval timestamp
Definition: vrpn_Tracker.h:100
float pcos(float x)
#define MM_TO_METERS
virtual int get_report(void)
Try to read reports from the device. Returns 1 if a complete report received, 0 otherwise....
vrpn_float64 d_quat[4]
Definition: vrpn_Tracker.h:95
unsigned char buttons[vrpn_BUTTON_MAX_BUTTONS]
Definition: vrpn_Button.h:44
#define VRPN_MSG_ERROR(msg)
vrpn_int32 position_m_id
Definition: vrpn_Tracker.h:80