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 
vrpn_Connection::pack_message
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...
Definition: vrpn_Connection.C:4632
vrpn_Tracker
Definition: vrpn_Tracker.h:49
vrpn_Button::report_changes
virtual void report_changes(void)
Definition: vrpn_Button.C:422
vrpn_BaseClass.h
vrpn_Button_Filter::report_changes
virtual void report_changes(void)
Definition: vrpn_Button.C:382
vrpn_Tracker::encode_to
virtual int encode_to(char *buf)
Definition: vrpn_Tracker.C:533
vrpn_3DMicroscribe::mainloop
virtual void mainloop()
Called once through each main loop iteration to handle updates.
Definition: vrpn_3DMicroscribe.C:283
vrpn_3DMicroscribe::ConvertOriToQuat
void ConvertOriToQuat(float ori[3])
Definition: vrpn_3DMicroscribe.C:215
vrpn_Tracker::d_quat
vrpn_float64 d_quat[4]
Definition: vrpn_Tracker.h:95
STATUS_SYNCING
#define STATUS_SYNCING
Definition: vrpn_3DMicroscribe.C:27
vrpn_3DMicroscribe::clear_values
virtual void clear_values(void)
Set all buttons, analogs and encoders back to 0.
Definition: vrpn_3DMicroscribe.C:115
vrpn_3DMicroscribe::reset
virtual int reset(void)
Set device back to starting config.
Definition: vrpn_3DMicroscribe.C:121
vrpn_Tracker::timestamp
struct timeval timestamp
Definition: vrpn_Tracker.h:100
vrpn_3DMicroscribe::timestamp
struct timeval timestamp
Time of the last report from the device.
Definition: vrpn_3DMicroscribe.h:44
vrpn_Button::num_buttons
vrpn_int32 num_buttons
Definition: vrpn_Button.h:47
vrpn_Button::buttons
unsigned char buttons[vrpn_BUTTON_MAX_BUTTONS]
Definition: vrpn_Button.h:44
vrpn_BaseClassUnique::d_connection
vrpn_Connection * d_connection
Connection that this object talks to.
Definition: vrpn_BaseClass.h:224
vrpn_3DMicroscribe::m_Scale
float m_Scale
Definition: vrpn_3DMicroscribe.h:31
vrpn_3DMicroscribe.h
vrpn_Shared.h
VR_PI
#define VR_PI
Definition: vrpn_3DMicroscribe.C:33
vrpn_BaseClassUnique::d_sender_id
vrpn_int32 d_sender_id
Sender ID registered with the connection.
Definition: vrpn_BaseClass.h:228
vrpn_Button::timestamp
struct timeval timestamp
Definition: vrpn_Button.h:48
vrpn_3DMicroscribe::_numbuttons
int _numbuttons
How many buttons to open.
Definition: vrpn_3DMicroscribe.h:36
vrpn_Tracker::position_m_id
vrpn_int32 position_m_id
Definition: vrpn_Tracker.h:80
vrpn_Connection
Generic connection class not specific to the transport mechanism.
Definition: vrpn_Connection.h:510
vrpn_3DMicroscribe::vrpn_3DMicroscribe
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)
Definition: vrpn_3DMicroscribe.C:38
psin
float psin(float x)
Definition: vrpn_3DMicroscribe.C:35
vrpn_gettimeofday
#define vrpn_gettimeofday
Definition: vrpn_Shared.h:89
STATUS_RESETTING
#define STATUS_RESETTING
Definition: vrpn_3DMicroscribe.C:26
vrpn_Tracker::status
int status
Definition: vrpn_Tracker.h:129
pcos
float pcos(float x)
Definition: vrpn_3DMicroscribe.C:34
vrpn_3DMicroscribe::m_PortNumber
int m_PortNumber
port number
Definition: vrpn_3DMicroscribe.h:32
vrpn_3DMicroscribe::report
virtual void report(vrpn_uint32 class_of_service=vrpn_CONNECTION_LOW_LATENCY)
send report whether or not changed
Definition: vrpn_3DMicroscribe.C:274
vrpn_3DMicroscribe::get_report
virtual int get_report(void)
Try to read reports from the device. Returns 1 if a complete report received, 0 otherwise....
Definition: vrpn_3DMicroscribe.C:176
vrpn_3DMicroscribe::m_BaudRate
long int m_BaudRate
baud rate
Definition: vrpn_3DMicroscribe.h:33
vrpn_MessageMacros.h
Header containing macros formerly duplicated in a lot of implementation files.
vrpn_Tracker::pos
vrpn_float64 pos[3]
Definition: vrpn_Tracker.h:95
STATUS_READING
#define STATUS_READING
Definition: vrpn_3DMicroscribe.C:28
VRPN_MSG_ERROR
#define VRPN_MSG_ERROR(msg)
Definition: vrpn_MessageMacros.h:46
MM_TO_METERS
#define MM_TO_METERS
Definition: vrpn_3DMicroscribe.C:31
vrpn_BaseClassUnique::server_mainloop
void server_mainloop(void)
Handles functions that all servers should provide in their mainloop() (ping/pong, for example) Should...
Definition: vrpn_BaseClass.C:603
vrpn_3DMicroscribe::m_OffSet
float m_OffSet[3]
Definition: vrpn_3DMicroscribe.h:30
vrpn_Button_Filter
All button servers should derive from this class, which provides the ability to turn any of the butto...
Definition: vrpn_Button.h:65