00001
00010 #include <stdio.h>
00011 #include <stdlib.h>
00012 #include <string.h>
00013 #include <usb.h>
00014
00015
00016 #include "usbdrv.h"
00017 #include "usbservo.h"
00018
00019 int usbGetStringAscii(usb_dev_handle * dev, int index, int langid,
00020 char *buf, int buflen) {
00021 char buffer[256];
00022 int rval, i;
00023
00024 if ((rval = usb_control_msg(dev, USB_ENDPOINT_IN, USB_REQ_GET_DESCRIPTOR,
00025 (USB_DT_STRING << 8) + index, langid, buffer,
00026 sizeof(buffer), 1000)) < 0) {
00027 return rval;
00028 }
00029 if (buffer[1] != USB_DT_STRING) {
00030 return 0;
00031 }
00032 if ((unsigned char)buffer[0] < rval) {
00033 rval = (unsigned char)buffer[0];
00034 }
00035 rval /= 2;
00036
00037 for (i = 1; i < rval; i++) {
00038 if (i > buflen) {
00039
00040 break;
00041 }
00042 buf[i - 1] = buffer[2 * i];
00043 if (buffer[2 * i + 1] != 0) {
00044
00045 buf[i - 1] = '?';
00046 }
00047 }
00048 buf[i - 1] = 0;
00049 return i - 1;
00050 }
00051
00052 int usbOpenDevice(usb_dev_handle ** device, int vendor, char *vendorName,
00053 int product, char *productName) {
00054 struct usb_bus *bus;
00055 struct usb_device *dev;
00056 usb_dev_handle *handle = NULL;
00057 int errorCode = USB_ERROR_NOTFOUND;
00058 static int didUsbInit = 0;
00059
00060 if (!didUsbInit) {
00061 didUsbInit = 1;
00062 usb_init();
00063 }
00064 usb_find_busses();
00065 usb_find_devices();
00066 for (bus = usb_get_busses(); bus; bus = bus->next) {
00067 for (dev = bus->devices; dev; dev = dev->next) {
00068 if (dev->descriptor.idVendor == vendor
00069 && dev->descriptor.idProduct == product) {
00070 char string[256];
00071 int len;
00072 handle = usb_open(dev);
00073
00074 if (!handle) {
00075 errorCode = USB_ERROR_ACCESS;
00076 fprintf(stderr,
00077 "Warning: cannot open USB device: %s\n",
00078 usb_strerror());
00079 continue;
00080 }
00081 if (vendorName == NULL && productName == NULL) {
00082
00083 break;
00084 }
00085
00086 len = usbGetStringAscii(handle,
00087 dev->descriptor.iManufacturer,
00088 0x0409, string, sizeof(string));
00089 if (len < 0) {
00090 errorCode = USB_ERROR_IO;
00091 fprintf(stderr,
00092 "Warning: cannot query manufacturer for device: %s\n",
00093 usb_strerror());
00094 } else {
00095 errorCode = USB_ERROR_NOTFOUND;
00096 if (strcmp(string, vendorName) == 0) {
00097 len = usbGetStringAscii(handle,
00098 dev->descriptor.iProduct,
00099 0x0409, string,
00100 sizeof(string));
00101 if (len < 0) {
00102 errorCode = USB_ERROR_IO;
00103 fprintf(stderr,
00104 "Warning: cannot query product for device: %s\n",
00105 usb_strerror());
00106 } else {
00107 errorCode = USB_ERROR_NOTFOUND;
00108 if (strcmp(string, productName) == 0) {
00109 break;
00110 }
00111 }
00112 }
00113 }
00114 usb_close(handle);
00115 handle = NULL;
00116 }
00117 }
00118 if (handle) {
00119 break;
00120 }
00121 }
00122 if (handle != NULL) {
00123 errorCode = 0;
00124 *device = handle;
00125 }
00126 return errorCode;
00127 }
00128
00129 int dev_test(usb_dev_handle * handle, int argc, char **argv) {
00130 unsigned char buffer[8];
00131 int nBytes;
00132 int i, v, r;
00133 if (argc != 2) {
00134 return 1;
00135 }
00136 for (i = 0; i < 1000; i++) {
00137 v = rand() & 0xffff;
00138 nBytes = usb_control_msg(handle,
00139 USB_TYPE_VENDOR | USB_RECIP_DEVICE |
00140 USB_ENDPOINT_IN, CMD_ECHO, v, 0,
00141 (char *)buffer, sizeof(buffer), 5000);
00142 if (nBytes < 2) {
00143 if (nBytes < 0) {
00144 fprintf(stderr, "USB error: %s\n", usb_strerror());
00145 }
00146 fprintf(stderr, "only %d bytes received in iteration %d\n",
00147 nBytes, i);
00148 exit(1);
00149 }
00150 r = buffer[0] | (buffer[1] << 8);
00151 if (r != v) {
00152 fprintf(stderr,
00153 "data error: received 0x%x instead of 0x%x in iteration %d\n",
00154 r, v, i);
00155 exit(1);
00156 }
00157 }
00158 printf("test succeeded\n");
00159 return 0;
00160 }
00161
00162 int dev_set(usb_dev_handle * handle, int argc, char **argv) {
00163 unsigned char buffer[8];
00164 int nBytes;
00165 if (argc != 3) {
00166 return 1;
00167 }
00168 int angle = atoi(argv[2]);
00169 if ((angle < 0) || (angle > 255)) {
00170 fprintf(stderr, "invalid angle: %d\n", angle);
00171 exit(1);
00172 }
00173
00174 nBytes = usb_control_msg(handle,
00175 USB_TYPE_VENDOR | USB_RECIP_DEVICE |
00176 USB_ENDPOINT_OUT, CMD_SET, angle, 0,
00177 (char *)buffer, sizeof(buffer), 5000);
00178
00179 if (nBytes < 0) {
00180 fprintf(stderr, "USB error: %s\n", usb_strerror());
00181 exit(1);
00182 }
00183 return 0;
00184 }
00185
00186 int dev_status(usb_dev_handle * handle, int argc, char **argv) {
00187 int nBytes;
00188 unsigned char buffer[8];
00189 if (argc != 2) {
00190 return 1;
00191 }
00192 nBytes = usb_control_msg(handle,
00193 USB_TYPE_VENDOR | USB_RECIP_DEVICE |
00194 USB_ENDPOINT_IN, CMD_GET, 0, 0, (char *)buffer,
00195 sizeof(buffer), 5000);
00196 if (nBytes < 0) {
00197 fprintf(stderr, "USB error: %s\n", usb_strerror());
00198 exit(1);
00199 }
00200 if (nBytes < 1) {
00201 fprintf(stderr, "only %d bytes status received\n", nBytes);
00202 exit(1);
00203 }
00204 printf("Current servo angle: %d\n", buffer[0]);
00205 return 0;
00206 }
00207