1 #include <iostream>
2
3 // HDAL
4 #include <hdl/hdl.h>
5 #include <hdlu/hdlu.h>
6
7 // Handle to device
8 HDLDeviceHandle deviceHandle;
9
10 // Handle to Contact Callback
11 HDLServoOpExitCode servoOp;
12
13
14 // Variables used only by servo thread
15 double positionServo[3];
16 bool buttonServo;
17 double forceServo[3];
18
19 // Variables used only by application thread
20 double positionApp[3];
21 double positionAppLast[3];
22 bool buttonApp = false;
23
24 // Number of haptic devices
25 int numHapticDevices = 0;
26
27 // Blocking values
28 const bool bNonBlocking = false;
29 const bool bBlocking = true;
30
31
32 // Non blocking callback function - servo loop
33 HDLServoOpExitCode NonBlockingServoOpCallback(void* pUserData)
34 {
35 // Get current state of haptic device
36 hdlToolPosition(positionServo);
37 hdlToolButton(&(buttonServo));
38
39 // Send forces to device
40 // hdlSetToolForce(forceServo);
41
42 // Make sure to continue processing
43 return HDL_SERVOOP_CONTINUE;
44 }
45
46 // Blocking callback function - servo loop
47 HDLServoOpExitCode BlockingServoOpCallback(void* pUserData)
48 {
49 positionApp[0] = positionServo[0];
50 positionApp[1] = positionServo[1];
51 positionApp[2] = positionServo[2];
52 buttonApp = buttonServo;
53
54 return HDL_SERVOOP_EXIT;
55 }
56
57 void testHDLError()
58 {
59 HDLError err = hdlGetError();
60 if (err != HDL_NO_ERROR)
61 {
62 std::cout << "HDLError " << err << std::endl;
63 abort();
64 }
65 }
66
67 int main(void)
68 {
69 // read the number of haptic devices currently connected to the computer
70 numHapticDevices = hdlCountDevices();
71 std::cout << "HDAL: hdlCountDevices = " << numHapticDevices << std::endl;
72
73 // exit if no haptic devices found
74 if (numHapticDevices == 0) return 1;
75
76 // inits "DEFAULT" haptic device
77 std::cout << "HDAL: hdlInitDevice" << std::endl;
78 deviceHandle = hdlInitNamedDevice("DEFAULT");
79 testHDLError();
80
81 if (deviceHandle == HDL_INVALID_HANDLE)
82 {
83 std::cout << "Could not open device: HDL_INVALID_HANDLE" << std::endl;
84 exit(1);
85 }
86
87
88 // starts servo and all haptic devices.
89 std::cout << "HDAL: hdlStart" << std::endl;
90 hdlStart();
91
92 // sets callback for the nonblocking servo loop
93 std::cout << "HDAL: hdlCreateServoOp" << std::endl;
94 servoOp = hdlCreateServoOp(NonBlockingServoOpCallback, NULL, bNonBlocking);
95 testHDLError();
96
97 if (servoOp == HDL_INVALID_HANDLE)
98 {
99 std::cout << "Invalid servo op handle: HDL_INVALID_HANDLE" << std::endl;
100 exit(1);
101 }
102
103 // make a specific haptic device current
104 std::cout << "HDAL: hdlMakeCurrent" << std::endl;
105 hdlMakeCurrent(deviceHandle);
106 testHDLError();
107
108 std::cout << "Press any button on the haptic device to exit." << std::endl;
109
110 // infinite loop reading position from haptic device
111 while (!buttonApp)
112 {
113 // calls the blocking servo loop
114 hdlCreateServoOp(BlockingServoOpCallback, NULL, bBlocking);
115
116 //
117 // compares positionApp with positionAppLast
118 // print position on change
119 //
120 if (positionApp[0] != positionAppLast[0] ||
121 positionApp[1] != positionAppLast[1] ||
122 positionApp[2] != positionAppLast[2])
123 {
124 // update last position
125 positionAppLast[0] = positionApp[0];
126 positionAppLast[1] = positionApp[1];
127 positionAppLast[2] = positionApp[2];
128
129 // print position to standard output
130 std::cout << "x: " << positionApp[0] << " y: " << positionApp[1] << " z: " << positionApp[2] << std::endl;
131 }
132
133
134 }
135
136 return 0;
137 }