19 #if defined(VRPN_USE_LIBUSB_1_0) 27 #define REPORT_ERROR(msg) { send_text_message(msg, timestamp, vrpn_TEXT_ERROR) ; if (d_connection && d_connection->connected()) d_connection->send_pending_reports(); } 30 static const vrpn_uint16 LUDL_VENDOR = 0x6969;
31 static const vrpn_uint16 LUDL_USBMAC6000 = 0x1235;
34 static const vrpn_uint16 LUDL_GET_LONG_DATA = 84;
35 static const vrpn_uint16 LUDL_SET_LONG_DATA = 83;
36 static const vrpn_uint16 LUDL_MOTOR_ACTION = 65;
39 static const vrpn_uint16 LUDL_MOTOR_POSITION = 5;
40 static const vrpn_uint16 LUDL_MODULE_BUSY = 63;
41 static const vrpn_uint16 LUDL_START_MOTOR_TARGET = 0;
42 static const vrpn_uint16 LUDL_CENTER_HOME = 7;
43 static const vrpn_uint16 SERVO_CHECKING = 241;
46 static const vrpn_uint16 LUDL_INTERFACE_ADDRESS = 32;
51 , _device_handle(NULL)
57 fprintf(stderr,
"vrpn_LUDL_USBMAC6000: can't init LibUSB\n");
62 fprintf(stderr,
"vrpn_LUDL_USBMAC6000: can't find any USBMac6000 devices\n");
64 fprintf(stderr,
" (Did you install a Zadig.exe or other LibUSB-compatible driver?)\n");
67 fprintf(stderr,
" (Did you remember to run as root?)\n");
73 fprintf(stderr,
"vrpn_LUDL_USBMAC6000: can't claim interface for this device\n");
75 fprintf(stderr,
" (Did you remember to run as root?)\n");
106 if (!send_usbmac_command(1, LUDL_SET_LONG_DATA, SERVO_CHECKING, 1)) {
107 REPORT_ERROR(
"vrpn_LUDL_USBMAC6000::vrpn_LUDL_USBMAC6000(): Could not send command 1");
109 if (!send_usbmac_command(2, LUDL_SET_LONG_DATA, SERVO_CHECKING, 1)) {
110 REPORT_ERROR(
"vrpn_LUDL_USBMAC6000::vrpn_LUDL_USBMAC6000(): Could not send command 2");
116 flush_input_from_ludl();
123 fprintf(stderr,
"vrpn_LUDL_USBMAC6000: can't register handler\n");
128 fprintf(stderr,
"vrpn_LUDL_USBMAC6000: can't register handler\n");
133 fprintf(stderr,
"vrpn_LUDL_USBMAC6000: can't register handler\n");
137 fprintf(stderr,
"vrpn_LUDL_USBMAC6000: Can't get connection!\n");
142 fprintf(stderr,
"vrpn_LUDL_USBMAC6000: Out of memory\n");
144 if ( (_axis_destination =
new vrpn_float64[
o_num_channel]) == NULL) {
145 fprintf(stderr,
"vrpn_LUDL_USBMAC6000: Out of memory\n");
149 _axis_moving[i] =
false;
165 if (_axis_moving != NULL) {
delete [] _axis_moving; _axis_moving = NULL; }
166 if (_axis_destination != NULL) {
delete [] _axis_destination; _axis_destination = NULL; }
176 struct timeval zerotime;
178 zerotime.tv_usec = 0;
179 libusb_handle_events_timeout(
_context, &zerotime);
191 if ( (ret != LIBUSB_SUCCESS) && (ret != LIBUSB_ERROR_TIMEOUT) ) {
192 #ifdef libusb_strerror 193 fprintf(stderr,
"vrpn_LUDL_USBMAC6000::check_for_data(): Could not read data: %s\n",
194 libusb_strerror(static_cast<libusb_error>(ret)));
196 fprintf(stderr,
"vrpn_LUDL_USBMAC6000::check_for_data(): Could not read data: code %d\n",
214 struct timeval zerotime;
216 zerotime.tv_usec = 0;
217 libusb_handle_events_timeout(
_context, &zerotime);
224 if (!_axis_moving || !_axis_destination) {
return; }
227 if (_axis_moving[i]) {
228 if (!ludl_axis_moving(i+1)) {
230 _axis_moving[i] =
false;
239 if (ludl_axis_position(1, &position)) {
242 if (ludl_axis_position(2, &position)) {
267 void vrpn_LUDL_USBMAC6000::flush_input_from_ludl(
void)
281 bool vrpn_LUDL_USBMAC6000::send_usbmac_command(
unsigned device,
unsigned command,
unsigned index,
int value)
288 struct timeval zerotime;
290 zerotime.tv_usec = 0;
291 libusb_handle_events_timeout(
_context, &zerotime);
294 sprintf(msg,
"can %u %u %u %i\n", device, command, index, value);
295 int len = strlen(msg);
301 static_cast<vrpn_uint8 *>(static_cast<void*>(msg)),
304 if ((ret != 0) || (sent_len != len)) {
305 #ifdef libusb_strerror 306 fprintf(stderr,
"vrpn_LUDL_USBMAC6000::send_usbmac_command(): Could not send: %s\n",
307 libusb_strerror(static_cast<libusb_error>(ret)));
309 fprintf(stderr,
"vrpn_LUDL_USBMAC6000::send_usbmac_command(): Could not send: code %d\n",
327 if (buffer == NULL) {
return false; }
328 const char *charbuf =
static_cast<const char *
>(
static_cast<const void *
>(buffer));
330 char acolon[32], can[32];
331 int device = 0, command = 0, index = 0, value = 0;
332 if (sscanf(charbuf,
"%s %s %i %i %i %i", acolon, can, &device, &command, &index, &value) <= 0) {
333 REPORT_ERROR(
"vrpn_LUDL_USBMAC6000::interpret_usbmac_ascii_response(): Could not parse response");
337 *device_return = device;
338 *command_return = command;
339 *index_return = index;
340 *value_return = value;
348 bool vrpn_LUDL_USBMAC6000::recenter(
void)
352 if (!send_usbmac_command(1, LUDL_MOTOR_ACTION, LUDL_CENTER_HOME, 100000)) {
353 REPORT_ERROR(
"vrpn_LUDL_USBMAC6000::recenter(): Could not send command 1");
356 printf(
"vrpn_LUDL_USBMAC6000::recenter(): Waiting for X-axis center\n");
360 struct timeval zerotime;
362 zerotime.tv_usec = 0;
363 libusb_handle_events_timeout(
_context, &zerotime);
365 flush_input_from_ludl();
371 while(!ludl_axis_moving(1)) {
373 libusb_handle_events_timeout(
_context, &zerotime);
375 while(ludl_axis_moving(1)) {
377 libusb_handle_events_timeout(
_context, &zerotime);
385 if (!send_usbmac_command(1, LUDL_SET_LONG_DATA, LUDL_MOTOR_POSITION, 694576)) {
386 REPORT_ERROR(
"vrpn_LUDL_USBMAC6000::recenter(): Could not send command 2");
393 if (!send_usbmac_command(2, LUDL_MOTOR_ACTION, LUDL_CENTER_HOME, 100000)) {
394 REPORT_ERROR(
"vrpn_LUDL_USBMAC6000::recenter(): Could not send command 3");
397 printf(
"vrpn_LUDL_USBMAC6000::recenter(): Waiting for Y-axis center\n");
401 libusb_handle_events_timeout(
_context, &zerotime);
403 flush_input_from_ludl();
409 while(!ludl_axis_moving(2)) {
411 libusb_handle_events_timeout(
_context, &zerotime);
413 while(ludl_axis_moving(2)) {
415 libusb_handle_events_timeout(
_context, &zerotime);
423 if (!send_usbmac_command(2, LUDL_SET_LONG_DATA, LUDL_MOTOR_POSITION, 1124201)) {
424 REPORT_ERROR(
"vrpn_LUDL_USBMAC6000::recenter(): Could not send command 4");
436 bool vrpn_LUDL_USBMAC6000::ludl_axis_moving(
unsigned axis)
440 flush_input_from_ludl();
441 if (!send_usbmac_command(LUDL_INTERFACE_ADDRESS, LUDL_GET_LONG_DATA, LUDL_MODULE_BUSY, 0)) {
442 REPORT_ERROR(
"vrpn_LUDL_USBMAC6000::ludl_axis_moving(): Could not send command 1");
448 unsigned watchdog = 0;
451 REPORT_ERROR(
"vrpn_LUDL_USBMAC6000::ludl_axis_moving(): Could not get report");
458 if (++watchdog == 25) {
459 if (!send_usbmac_command(LUDL_INTERFACE_ADDRESS, LUDL_GET_LONG_DATA, LUDL_MODULE_BUSY, 0)) {
460 REPORT_ERROR(
"vrpn_LUDL_USBMAC6000::ludl_axis_moving(): Could not resend command 1");
467 int device, command, index;
469 REPORT_ERROR(
"vrpn_LUDL_USBMAC6000::ludl_axis_moving(): Could not parse report");
473 int axisMaskBit = 0x0001 << axis;
474 return (
status & axisMaskBit) != 0;
478 bool vrpn_LUDL_USBMAC6000::ludl_axis_position(
unsigned axis, vrpn_int32 *position_return)
481 flush_input_from_ludl();
482 if (!send_usbmac_command(axis, LUDL_GET_LONG_DATA, LUDL_MOTOR_POSITION, 0)) {
483 REPORT_ERROR(
"vrpn_LUDL_USBMAC6000::ludl_axis_position(): Could not send command 1");
489 unsigned watchdog = 0;
492 REPORT_ERROR(
"vrpn_LUDL_USBMAC6000::ludl_axis_position(): Could not get report");
499 if (++watchdog == 25) {
500 if (!send_usbmac_command(axis, LUDL_GET_LONG_DATA, LUDL_MOTOR_POSITION, 0)) {
501 REPORT_ERROR(
"vrpn_LUDL_USBMAC6000::ludl_axis_position(): Could not resend command 1");
508 int device, command, index;
510 REPORT_ERROR(
"vrpn_LUDL_USBMAC6000::ludl_axis_position(): Could not parse report");
514 if ( (command != LUDL_GET_LONG_DATA) || (index != LUDL_MOTOR_POSITION) ) {
515 REPORT_ERROR(
"vrpn_LUDL_USBMAC6000::ludl_axis_position(): Bad command or index in report");
518 *position_return = position;
522 bool vrpn_LUDL_USBMAC6000::move_axis_to_position(
int axis,
int position)
525 if (!_axis_destination || !_axis_moving) {
return false; }
530 if (_axis_destination[axis-1] == position) {
535 if (!send_usbmac_command(axis, LUDL_MOTOR_ACTION, LUDL_START_MOTOR_TARGET, position)) {
536 REPORT_ERROR(
"vrpn_LUDL_USBMAC6000::move_axis_to_position(): Could not send command");
548 struct timeval start, now;
550 while (!ludl_axis_moving(axis)) {
553 if (diff.tv_sec > 1) {
556 _axis_destination[axis-1] = position;
565 _axis_destination[axis-1] = position;
566 _axis_moving[axis-1] =
true;
572 const char *bufptr = p.
buffer;
587 sprintf(msg,
"vrpn_LUDL_USBMAC6000::handle_request_message(): Index out of bounds (%d of %d), value %lg\n",
593 me->move_axis_to_position(chan_num + 1, static_cast<int>(value));
600 const char* bufptr = p.
buffer;
610 sprintf(msg,
"vrpn_LUDL_USBMAC6000::handle_request_channels_message(): Index out of bounds (%d of %d), clipping\n",
615 for (i = 0; i < num; i++) {
617 me->move_axis_to_position(i + 1, static_cast<int>(me->
o_channel[i]));
void server_mainloop(void)
Handles functions that all servers should provide in their mainloop() (ping/pong, for example) Should...
virtual void report_changes(vrpn_uint32 class_of_service=vrpn_CONNECTION_LOW_LATENCY, const struct timeval time=vrpn_ANALOG_NOW)
Send a report only if something has changed (for servers) Optionally, tell what time to stamp the val...
void vrpn_SleepMsecs(double dMsecs)
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.
vrpn_int32 request_channels_m_id
void report_changes(vrpn_uint32 class_of_service=vrpn_CONNECTION_RELIABLE)
vrpn_LUDL_USBMAC6000(const char *name, vrpn_Connection *c=0, bool do_recenter=false)
const vrpn_uint32 vrpn_CONNECTION_RELIABLE
Classes of service for messages, specify multiple by ORing them together Priority of satisfying these...
struct timeval _timestamp
bool interpret_usbmac_ascii_response(const vrpn_uint8 *buffer, int *device_return, int *command_return, int *index_return, int *value_return)
static int VRPN_CALLBACK handle_connect_message(void *userdata, vrpn_HANDLERPARAM p)
Responds to a connection request with a report of the values.
vrpn_float64 o_channel[vrpn_CHANNEL_MAX]
timeval vrpn_TimevalDiff(const timeval &tv1, const timeval &tv2)
vrpn_float64 channel[vrpn_CHANNEL_MAX]
#define REPORT_ERROR(msg)
vrpn_int32 d_ping_message_id
Ask the server if they are there.
Generic connection class not specific to the transport mechanism.
virtual void report(vrpn_uint32 class_of_service=vrpn_CONNECTION_LOW_LATENCY, const struct timeval time=vrpn_ANALOG_NOW)
Send a report whether something has changed or not (for servers) Optionally, tell what time to stamp ...
struct libusb_context * _context
All types of client/server/peer objects in VRPN should be derived from the vrpn_BaseClass type descri...
virtual void mainloop()
Called once through each main loop iteration to handle updates. Remote object mainloop() should call ...
static int VRPN_CALLBACK handle_request_channels_message(void *userdata, vrpn_HANDLERPARAM p)
Responds to a request to change multiple channels at once.
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.
This structure is what is passed to a vrpn_Connection message callback.
static int VRPN_CALLBACK handle_request_message(void *userdata, vrpn_HANDLERPARAM p)
Responds to a request to change one of the values by setting the channel to that value.
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.
virtual ~vrpn_LUDL_USBMAC6000()
static const unsigned _INBUFFER_SIZE
#define vrpn_gettimeofday
vrpn_int32 d_sender_id
Sender ID registered with the connection.
vrpn_uint8 _inbuffer[_INBUFFER_SIZE]
void report(vrpn_uint32 class_of_service=vrpn_CONNECTION_RELIABLE)
vrpn_float64 last[vrpn_CHANNEL_MAX]
struct libusb_device_handle * _device_handle