vrpn 07.35
Virtual Reality Peripheral Network
Loading...
Searching...
No Matches
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
7#include "vrpn_BaseClass.h" // for ::vrpn_TEXT_ERROR
8#include "vrpn_Shared.h" // for timeval, vrpn_gettimeofday
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
34inline float pcos(float x) {return (float)cos(double(x)*VR_PI/180.0f);}
35inline 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") )
53 else if(!strcmp(Port, "COM2") )
55 else if(!strcmp(Port, "COM3") )
57 else if(!strcmp(Port, "COM4") )
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
65
66 vrpn_gettimeofday(&timestamp, NULL); // Set watchdog now
67
68#ifdef VRPN_USE_MICROSCRIBE
69 int iResult;
70 iResult=ArmStart(NULL);
71 if(ARM_SUCCESS != iResult)
72 {
73 //error starting the MicroScribe drivers
74 VRPN_MSG_ERROR( "Unable to start MicroScribe ArmDll32." );
75 return;
76 }
77
78 //don't use error handlers
79 iResult = ArmSetErrorHandlerFunction(NO_HCI_HANDLER, NULL);
80 iResult = ArmSetErrorHandlerFunction(BAD_PORT_HANDLER, NULL);
81 iResult = ArmSetErrorHandlerFunction(CANT_OPEN_HANDLER, NULL);
82 iResult = ArmSetErrorHandlerFunction(CANT_BEGIN_HANDLER, NULL);
83
84 //connect to the correct port
85 switch(m_PortNumber)
86 {
87 case 1:
88 iResult = ArmConnect(1, m_BaudRate);
89 break;
90 case 2:
91 iResult = ArmConnect(2, m_BaudRate);
92 break;
93 case 3:
94 iResult = ArmConnect(3, m_BaudRate);
95 break;
96 case 4:
97 iResult = ArmConnect(4, m_BaudRate);
98 break;
99 default:
100 iResult = ArmConnect(0, 0); //try all available ports and baud rates
101 break;
102 }
103
104 if(ARM_SUCCESS != iResult)
105 {
106 //error connecting, end the thread
107 ArmEnd();
108 VRPN_MSG_ERROR( "Unable to connect to the MicroScribe." );
109 return;
110 }
111
112#endif
113 // Set the mode to reset
115}
116
118{
119}
120
121// This routine will reset the 3DMicroscribe, zeroing the Origin position,
122// mode.
124{
125#ifdef VRPN_USE_MICROSCRIBE
126 int iResult;
127 // ARM_FULL: ArmDll32 calculates and updates the Cartesian position and orientation of the stylus tip.
128 iResult = ArmSetUpdate(ARM_FULL);
129
130 if(iResult != ARM_SUCCESS)
131 {
132 //error setting the update type, disconnect and end the thread
133 VRPN_MSG_ERROR( "Unable to set the update type for the MicroScribe." );
134 return -1;
135 }
136
137 //use mm instead of inches
138 ArmSetLengthUnits(ARM_MM);
139 //use radians instead of degrees
140 //ArmSetAngleUnits(ARM_RADIANS);
141
142 //get the position of the tip
143 length_3D tipPosition;
144 angle_3D tipVector;
145
146 iResult = ArmGetTipPosition(&tipPosition); //retrieves the current stylus tip position in Cartesian coordinates
147 iResult = ArmGetTipOrientationUnitVector(&tipVector); //retrieves the current stylus tip's unit vector orientation
148
149 if(iResult == ARM_NOT_CONNECTED)
150 {
151 //error connecting
152 VRPN_MSG_ERROR( "MicroScribe connection lost!" );
153 return -1;
154 }
155
156#endif
157 // We're now waiting for a response from the box
159
160 vrpn_gettimeofday(&timestamp, NULL); // Set watchdog now
161 return 0;
162}
163
164
165
166// This function will read characters until it has a full report, then
167// put that report into the time, analog, or button fields and call
168// the report methods on these. The time stored is that of
169// the first character received as part of the report.
170// Reports start with different characters, and the length of the report
171// depends on what the first character of the report is. We switch based
172// on the first character of the report to see how many more to expect and
173// to see how to handle the report.
174// Returns 1 if there is a complete report found, 0 otherwise. This is
175// so that the calling routine can know to check again at the end of complete
176// reports to see if there is more than one report buffered up.
177
179{
180#ifdef VRPN_USE_MICROSCRIBE
181 length_3D tipPosition;
182 angle_3D tipOri;
183 DWORD buts;
184 int iResult = ArmGetTipPosition(&tipPosition); //retrieves the current stylus tip position in Cartesian coordinates
185 iResult = ArmGetTipOrientation(&tipOri); //retrieves the current stylus tip's unit vector orientation
186 iResult = ArmGetButtonsState(&buts);
187 if(iResult == ARM_NOT_CONNECTED)
188 {
189 //error connecting
190 VRPN_MSG_ERROR( "MicroScribe connection lost!" );
191 return 0;
192 }
193
194 //set the position, considering the scale, offset and origin matrix
195 pos[0] = (tipPosition.y * m_Scale + m_OffSet[0])* MM_TO_METERS ;
196 pos[1] = (tipPosition.z * m_Scale + m_OffSet[1])* MM_TO_METERS;
197 pos[2] = (tipPosition.x * m_Scale + m_OffSet[2])* MM_TO_METERS;
198 //vPosition = m_Matrix * vPosition + m_vPlaneOffset; extending the microscribe onto a plane
199
200 //set the orientation, considering the origin matrix
201 float ori[3]={tipOri.y, tipOri.z, tipOri.x};
202 ConvertOriToQuat(ori);
203
204
205 status = STATUS_READING; // ready to process event packet
206 vrpn_gettimeofday(&timestamp, NULL); // set timestamp of this event
207
208 buttons[0] = ((buts & 0x02) != 0); // button 1
209 buttons[1] = ((buts & 0x01) != 0); // button 2
210
211#endif
212
213 report_changes(); // Report updates to VRPN
214 return 0;
215}
216
218{
219 float real0,real1,real2,real;
220 float imag0,imag1,imag2,imag[3];
221
222 real0= pcos(ori[0]/2);
223 real1= pcos(ori[1]/2);
224 real2= pcos(ori[2]/2);
225
226 imag0 = psin(ori[0]/2);
227 imag1 = psin(ori[1]/2);
228 imag2 = psin(ori[2]/2);
229
230 // merge the first two quats
231 real = real0 * real1 ;
232
233 if ( real > 1 )
234 real = 1;
235 else if ( real < -1 )
236 real = -1;
237
238 imag[0] = imag0 * real1;
239 imag[1] = real0 * imag1;
240 imag[2] = imag0 * imag1;
241
242 // merge previous result with last quat
243
244 d_quat[0] = real * real2 - imag[2] * imag2 ;
245
246 if ( d_quat[0] > 1 )
247 d_quat[0] = 1;
248 else if ( d_quat[0] < -1 )
249 d_quat[0] = -1;
250
251 d_quat[1] = imag[0] * real2 + imag[1] * imag2;
252 d_quat[2] = imag[1] * real2 - imag[0] * imag2;
253 d_quat[3] = real * imag2 + imag[2] * real2;
254
255}
256
257void vrpn_3DMicroscribe::report_changes(vrpn_uint32 class_of_service)
258{
261
263 if (d_connection) {
264 char msgbuf[1000];
265 int len = vrpn_Tracker::encode_to(msgbuf);
267 position_m_id, d_sender_id, msgbuf,
268 class_of_service)) {
269 VRPN_MSG_ERROR("Tracker: cannot write message: tossing\n");
270 }
271 } else {
272 VRPN_MSG_ERROR("Tracker: No valid connection\n");
273 }
274}
275
276void vrpn_3DMicroscribe::report(vrpn_uint32 /*class_of_service*/)
277{
280}
281
282// This routine is called each time through the server's main loop. It will
283// take a course of action depending on the current status of the 3DMicroscribe,
284// either trying to reset it or trying to get a reading from it.
286{
288
289 switch(status) {
290 case STATUS_RESETTING:
291 reset();
292 break;
293
294 case STATUS_SYNCING:
295 case STATUS_READING:
296 // Keep getting reports until all full reports are read.
297 while (get_report()) {};
298 break;
299
300 default:
301 fprintf(stderr,"vrpn_3DMicroscribe: Unknown mode (internal error)\n");
302 break;
303 }
304}
305
306
virtual void clear_values(void)
Set all buttons, analogs and encoders back to 0.
virtual void report(vrpn_uint32 class_of_service=vrpn_CONNECTION_LOW_LATENCY)
send report whether or not changed
int _numbuttons
How many buttons to open.
int m_PortNumber
port number
virtual int reset(void)
Set device back to starting config.
long int m_BaudRate
baud rate
virtual void mainloop()
Called once through each main loop iteration to handle updates.
void ConvertOriToQuat(float ori[3])
virtual int get_report(void)
Try to read reports from the device. Returns 1 if a complete report received, 0 otherwise....
struct timeval timestamp
Time of the last report from the device.
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_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...
All button servers should derive from this class, which provides the ability to turn any of the butto...
Definition vrpn_Button.h:66
virtual void report_changes(void)
vrpn_int32 num_buttons
Definition vrpn_Button.h:48
struct timeval timestamp
Definition vrpn_Button.h:49
virtual void report_changes(void)
unsigned char buttons[vrpn_BUTTON_MAX_BUTTONS]
Definition vrpn_Button.h:45
Generic connection class not specific to the transport mechanism.
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...
virtual int encode_to(char *buf)
vrpn_float64 d_quat[4]
vrpn_float64 pos[3]
struct timeval timestamp
vrpn_int32 position_m_id
#define STATUS_SYNCING
#define MM_TO_METERS
#define VR_PI
float pcos(float x)
float psin(float x)
#define STATUS_READING
#define STATUS_RESETTING
All types of client/server/peer objects in VRPN should be derived from the vrpn_BaseClass type descri...
Header containing macros formerly duplicated in a lot of implementation files.
#define VRPN_MSG_ERROR(msg)
#define vrpn_gettimeofday
Definition vrpn_Shared.h:99