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(); }
30static const vrpn_uint16 LUDL_VENDOR = 0x6969;
31static const vrpn_uint16 LUDL_USBMAC6000 = 0x1235;
34static const vrpn_uint16 LUDL_GET_LONG_DATA = 84;
35static const vrpn_uint16 LUDL_SET_LONG_DATA = 83;
36static const vrpn_uint16 LUDL_MOTOR_ACTION = 65;
39static const vrpn_uint16 LUDL_MOTOR_POSITION = 5;
40static const vrpn_uint16 LUDL_MODULE_BUSY = 63;
41static const vrpn_uint16 LUDL_START_MOTOR_TARGET = 0;
42static const vrpn_uint16 LUDL_CENTER_HOME = 7;
43static const vrpn_uint16 SERVO_CHECKING = 241;
46static const vrpn_uint16 LUDL_INTERFACE_ADDRESS = 32;
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");
145 _axis_moving[i] =
false;
162 if (_axis_moving != NULL) {
delete[] _axis_moving; _axis_moving = NULL; }
163 if (_axis_destination != NULL) {
delete[] _axis_destination; _axis_destination = NULL; }
165 fprintf(stderr,
"vrpn_LUDL_USBMAC6000::~vrpn_LUDL_USBMAC6000(): delete failed\n");
178 struct timeval zerotime;
180 zerotime.tv_usec = 0;
181 libusb_handle_events_timeout(
_context, &zerotime);
193 if ( (ret != LIBUSB_SUCCESS) && (ret != LIBUSB_ERROR_TIMEOUT) ) {
194#ifdef libusb_strerror
195 fprintf(stderr,
"vrpn_LUDL_USBMAC6000::check_for_data(): Could not read data: %s\n",
196 libusb_strerror(
static_cast<libusb_error
>(ret)));
198 fprintf(stderr,
"vrpn_LUDL_USBMAC6000::check_for_data(): Could not read data: code %d\n",
216 struct timeval zerotime;
218 zerotime.tv_usec = 0;
219 libusb_handle_events_timeout(
_context, &zerotime);
226 if (!_axis_moving || !_axis_destination) {
return; }
229 if (_axis_moving[i]) {
230 if (!ludl_axis_moving(i+1)) {
232 _axis_moving[i] =
false;
241 if (ludl_axis_position(1, &position)) {
244 if (ludl_axis_position(2, &position)) {
269void vrpn_LUDL_USBMAC6000::flush_input_from_ludl(
void)
283bool vrpn_LUDL_USBMAC6000::send_usbmac_command(
unsigned device,
unsigned command,
unsigned index,
int value)
290 struct timeval zerotime;
292 zerotime.tv_usec = 0;
293 libusb_handle_events_timeout(
_context, &zerotime);
296 sprintf(msg,
"can %u %u %u %i\n", device, command, index, value);
297 int len = strlen(msg);
303 static_cast<vrpn_uint8 *
>(
static_cast<void*
>(msg)),
306 if ((ret != 0) || (sent_len != len)) {
307#ifdef libusb_strerror
308 fprintf(stderr,
"vrpn_LUDL_USBMAC6000::send_usbmac_command(): Could not send: %s\n",
309 libusb_strerror(
static_cast<libusb_error
>(ret)));
311 fprintf(stderr,
"vrpn_LUDL_USBMAC6000::send_usbmac_command(): Could not send: code %d\n",
329 if (buffer == NULL) {
return false; }
330 const char *charbuf =
static_cast<const char *
>(
static_cast<const void *
>(buffer));
332 char acolon[32], can[32];
333 int device = 0, command = 0, index = 0, value = 0;
334 if (sscanf(charbuf,
"%s %s %i %i %i %i", acolon, can, &device, &command, &index, &value) <= 0) {
335 REPORT_ERROR(
"vrpn_LUDL_USBMAC6000::interpret_usbmac_ascii_response(): Could not parse response");
339 *device_return = device;
340 *command_return = command;
341 *index_return = index;
342 *value_return = value;
350bool vrpn_LUDL_USBMAC6000::recenter(
void)
354 if (!send_usbmac_command(1, LUDL_MOTOR_ACTION, LUDL_CENTER_HOME, 100000)) {
355 REPORT_ERROR(
"vrpn_LUDL_USBMAC6000::recenter(): Could not send command 1");
358 printf(
"vrpn_LUDL_USBMAC6000::recenter(): Waiting for X-axis center\n");
362 struct timeval zerotime;
364 zerotime.tv_usec = 0;
365 libusb_handle_events_timeout(
_context, &zerotime);
367 flush_input_from_ludl();
373 while(!ludl_axis_moving(1)) {
375 libusb_handle_events_timeout(
_context, &zerotime);
377 while(ludl_axis_moving(1)) {
379 libusb_handle_events_timeout(
_context, &zerotime);
387 if (!send_usbmac_command(1, LUDL_SET_LONG_DATA, LUDL_MOTOR_POSITION, 694576)) {
388 REPORT_ERROR(
"vrpn_LUDL_USBMAC6000::recenter(): Could not send command 2");
395 if (!send_usbmac_command(2, LUDL_MOTOR_ACTION, LUDL_CENTER_HOME, 100000)) {
396 REPORT_ERROR(
"vrpn_LUDL_USBMAC6000::recenter(): Could not send command 3");
399 printf(
"vrpn_LUDL_USBMAC6000::recenter(): Waiting for Y-axis center\n");
403 libusb_handle_events_timeout(
_context, &zerotime);
405 flush_input_from_ludl();
411 while(!ludl_axis_moving(2)) {
413 libusb_handle_events_timeout(
_context, &zerotime);
415 while(ludl_axis_moving(2)) {
417 libusb_handle_events_timeout(
_context, &zerotime);
425 if (!send_usbmac_command(2, LUDL_SET_LONG_DATA, LUDL_MOTOR_POSITION, 1124201)) {
426 REPORT_ERROR(
"vrpn_LUDL_USBMAC6000::recenter(): Could not send command 4");
438bool vrpn_LUDL_USBMAC6000::ludl_axis_moving(
unsigned axis)
442 flush_input_from_ludl();
443 if (!send_usbmac_command(LUDL_INTERFACE_ADDRESS, LUDL_GET_LONG_DATA, LUDL_MODULE_BUSY, 0)) {
444 REPORT_ERROR(
"vrpn_LUDL_USBMAC6000::ludl_axis_moving(): Could not send command 1");
450 unsigned watchdog = 0;
453 REPORT_ERROR(
"vrpn_LUDL_USBMAC6000::ludl_axis_moving(): Could not get report");
460 if (++watchdog == 25) {
461 if (!send_usbmac_command(LUDL_INTERFACE_ADDRESS, LUDL_GET_LONG_DATA, LUDL_MODULE_BUSY, 0)) {
462 REPORT_ERROR(
"vrpn_LUDL_USBMAC6000::ludl_axis_moving(): Could not resend command 1");
469 int device, command, index;
471 REPORT_ERROR(
"vrpn_LUDL_USBMAC6000::ludl_axis_moving(): Could not parse report");
475 int axisMaskBit = 0x0001 << axis;
476 return (
status & axisMaskBit) != 0;
480bool vrpn_LUDL_USBMAC6000::ludl_axis_position(
unsigned axis, vrpn_int32 *position_return)
483 flush_input_from_ludl();
484 if (!send_usbmac_command(axis, LUDL_GET_LONG_DATA, LUDL_MOTOR_POSITION, 0)) {
485 REPORT_ERROR(
"vrpn_LUDL_USBMAC6000::ludl_axis_position(): Could not send command 1");
491 unsigned watchdog = 0;
494 REPORT_ERROR(
"vrpn_LUDL_USBMAC6000::ludl_axis_position(): Could not get report");
501 if (++watchdog == 25) {
502 if (!send_usbmac_command(axis, LUDL_GET_LONG_DATA, LUDL_MOTOR_POSITION, 0)) {
503 REPORT_ERROR(
"vrpn_LUDL_USBMAC6000::ludl_axis_position(): Could not resend command 1");
510 int device, command, index;
512 REPORT_ERROR(
"vrpn_LUDL_USBMAC6000::ludl_axis_position(): Could not parse report");
516 if ( (command != LUDL_GET_LONG_DATA) || (index != LUDL_MOTOR_POSITION) ) {
517 REPORT_ERROR(
"vrpn_LUDL_USBMAC6000::ludl_axis_position(): Bad command or index in report");
520 *position_return = position;
524bool vrpn_LUDL_USBMAC6000::move_axis_to_position(
int axis,
int position)
527 if (!_axis_destination || !_axis_moving) {
return false; }
532 if (_axis_destination[axis-1] == position) {
537 if (!send_usbmac_command(axis, LUDL_MOTOR_ACTION, LUDL_START_MOTOR_TARGET, position)) {
538 REPORT_ERROR(
"vrpn_LUDL_USBMAC6000::move_axis_to_position(): Could not send command");
550 struct timeval start, now;
552 while (!ludl_axis_moving(axis)) {
555 if (diff.tv_sec > 1) {
558 _axis_destination[axis-1] = position;
567 _axis_destination[axis-1] = position;
568 _axis_moving[axis-1] =
true;
574 const char *bufptr = p.
buffer;
589 sprintf(msg,
"vrpn_LUDL_USBMAC6000::handle_request_message(): Index out of bounds (%d of %d), value %lg\n",
595 me->move_axis_to_position(chan_num + 1,
static_cast<int>(value));
602 const char* bufptr = p.
buffer;
612 sprintf(msg,
"vrpn_LUDL_USBMAC6000::handle_request_channels_message(): Index out of bounds (%d of %d), clipping\n",
617 for (i = 0; i < num; i++) {
619 me->move_axis_to_position(i + 1,
static_cast<int>(me->
o_channel[i]));
vrpn_float64 o_channel[vrpn_CHANNEL_MAX]
vrpn_int32 request_channels_m_id
vrpn_Analog_Output(const char *name, vrpn_Connection *c=NULL)
vrpn_float64 last[vrpn_CHANNEL_MAX]
vrpn_float64 channel[vrpn_CHANNEL_MAX]
vrpn_Analog(const char *name, vrpn_Connection *c=NULL)
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 ...
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...
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.
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...
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.
vrpn_int32 d_ping_message_id
Ask the server if they are there.
Generic connection class not specific to the transport mechanism.
vrpn_uint8 _inbuffer[_INBUFFER_SIZE]
void report_changes(vrpn_uint32 class_of_service=vrpn_CONNECTION_RELIABLE)
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.
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.
virtual void mainloop()
Called once through each main loop iteration to handle updates. Remote object mainloop() should call ...
void report(vrpn_uint32 class_of_service=vrpn_CONNECTION_RELIABLE)
struct libusb_device_handle * _device_handle
struct timeval _timestamp
vrpn_LUDL_USBMAC6000(const char *name, vrpn_Connection *c=0, bool do_recenter=false)
virtual ~vrpn_LUDL_USBMAC6000()
struct libusb_context * _context
static int VRPN_CALLBACK handle_request_channels_message(void *userdata, vrpn_HANDLERPARAM p)
Responds to a request to change multiple channels at once.
static const unsigned _INBUFFER_SIZE
This structure is what is passed to a vrpn_Connection message callback.
All types of client/server/peer objects in VRPN should be derived from the vrpn_BaseClass type descri...
const vrpn_uint32 vrpn_CONNECTION_RELIABLE
Classes of service for messages, specify multiple by ORing them together Priority of satisfying these...
#define REPORT_ERROR(msg)
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.
timeval vrpn_TimevalDiff(const timeval &tv1, const timeval &tv2)
void vrpn_SleepMsecs(double dMilliSecs)
#define vrpn_gettimeofday