rschaten/USB-Servo
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

First version

Browse Source
This commit is contained in:
Ronald Schaten
2006-10-28 12:40:42 +00:00
parent 62a932ecdd
commit 13e0ba9ae5
35 changed files with 23014 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

26
commandline/Makefile Normal file
View File

@ -0,0 +1,26 @@
# $Id: Makefile,v 1.1 2006/10/28 12:40:42 rschaten Exp $
CC = gcc
LIBUSB_CONFIG = libusb-config
# Make sure that libusb-config is in the search path or specify a full path. On
# Windows, there is no libusb-config and you must configure the options below
# manually. See examples.
CFLAGS = `$(LIBUSB_CONFIG) --cflags` -O -Wall -I../common
USBLIBS = `$(LIBUSB_CONFIG) --libs`
XSPLIBS = -lX11 -lm
all: usb-servo xservopointer
usb-servo: usb-servo.o usbdrv.o
$(CC) $(CFLAGS) -o usb-servo usb-servo.o usbdrv.o $(USBLIBS)
xservopointer: xservopointer.o usbdrv.o
$(CC) $(CFLAGS) -o xservopointer xservopointer.o usbdrv.o $(USBLIBS) $(XSPLIBS)
.c.o:
$(CC) $(CFLAGS) -c $*.c -o $*.o
clean:
rm -f usb-servo xservopointer *.o

75
commandline/usb-servo.c Normal file
View File

@ -0,0 +1,75 @@
/**
* \file usb-servo.c
* \brief Commandline-tool for the USB-Servo.
* \author Ronald Schaten
* \version $Id: usb-servo.c,v 1.1 2006/10/28 12:40:42 rschaten Exp $
*
* License: See documentation.
*/
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <usb.h> /* this is libusb, see
http://libusb.sourceforge.net/ */
#include "usbdrv.h"
/**
* Displays usage-information. This function is called if the parameters cannot
* be parsed.
* \param name The name of this application.
*/
void usage(char *name) {
fprintf(stderr, "usage:\n");
fprintf(stderr, " %s status\n", name);
fprintf(stderr, " %s set <angle>\n", name);
fprintf(stderr, " %s test\n\n", name);
fprintf(stderr, "parameters:\n");
fprintf(stderr, " angle: Required angle for the servo.\n");
}
/**
* Main function. Initializes the USB-device, parses commandline-parameters and
* calls the functions that communicate with the device.
* \param argc Number of arguments.
* \param argv Arguments.
* \return Error code.
*/
int main(int argc, char **argv) {
usb_dev_handle *handle = NULL;
if (argc < 2) {
usage(argv[0]);
exit(1);
}
usb_init();
if (usbOpenDevice(&handle, USBDEV_SHARED_VENDOR, "www.schatenseite.de",
USBDEV_SHARED_PRODUCT, "USB-Servo") != 0) {
fprintf(stderr,
"Could not find USB device \"USB-Servo\" with vid=0x%x pid=0x%x\n",
USBDEV_SHARED_VENDOR, USBDEV_SHARED_PRODUCT);
exit(1);
}
/* We have searched all devices on all busses for our USB device
above. Now try to open it and perform the vendor specific control
operations for the function requested by the user. */
if (strcmp(argv[1], "test") == 0) {
if (dev_test(handle, argc, argv) != 0) {
usage(argv[1]);
}
} else if (strcmp(argv[1], "set") == 0) {
if (dev_set(handle, argc, argv) != 0) {
usage(argv[1]);
}
} else if (strcmp(argv[1], "status") == 0) {
if (dev_status(handle, argc, argv) != 0) {
usage(argv[1]);
}
} else {
usage(argv[0]);
exit(1);
}
usb_close(handle);
return 0;
}

207
commandline/usbdrv.c Normal file
View File

@ -0,0 +1,207 @@
/**
* \file usbdrv.c
* \brief USB-driver-parts for implementing a client.
* \author Ronald Schaten
* \version $Id: usbdrv.c,v 1.1 2006/10/28 12:40:42 rschaten Exp $
*
* License: See documentation.
*/
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <usb.h> /* this is libusb, see
http://libusb.sourceforge.net/ */
#include "usbdrv.h"
#include "usbservo.h"
int usbGetStringAscii(usb_dev_handle * dev, int index, int langid,
char *buf, int buflen) {
char buffer[256];
int rval, i;
if ((rval = usb_control_msg(dev, USB_ENDPOINT_IN, USB_REQ_GET_DESCRIPTOR,
(USB_DT_STRING << 8) + index, langid, buffer,
sizeof(buffer), 1000)) < 0) {
return rval;
}
if (buffer[1] != USB_DT_STRING) {
return 0;
}
if ((unsigned char)buffer[0] < rval) {
rval = (unsigned char)buffer[0];
}
rval /= 2;
/* lossy conversion to ISO Latin1 */
for (i = 1; i < rval; i++) {
if (i > buflen) {
/* destination buffer overflow */
break;
}
buf[i - 1] = buffer[2 * i];
if (buffer[2 * i + 1] != 0) {
/* outside of ISO Latin1 range */
buf[i - 1] = '?';
}
}
buf[i - 1] = 0;
return i - 1;
}
int usbOpenDevice(usb_dev_handle ** device, int vendor, char *vendorName,
int product, char *productName) {
struct usb_bus *bus;
struct usb_device *dev;
usb_dev_handle *handle = NULL;
int errorCode = USB_ERROR_NOTFOUND;
static int didUsbInit = 0;
if (!didUsbInit) {
didUsbInit = 1;
usb_init();
}
usb_find_busses();
usb_find_devices();
for (bus = usb_get_busses(); bus; bus = bus->next) {
for (dev = bus->devices; dev; dev = dev->next) {
if (dev->descriptor.idVendor == vendor
&& dev->descriptor.idProduct == product) {
char string[256];
int len;
handle = usb_open(dev); /* we need to open the device in
order to query strings */
if (!handle) {
errorCode = USB_ERROR_ACCESS;
fprintf(stderr,
"Warning: cannot open USB device: %s\n",
usb_strerror());
continue;
}
if (vendorName == NULL && productName == NULL) {
/* name does not matter */
break;
}
/* now check whether the names match: */
len = usbGetStringAscii(handle,
dev->descriptor.iManufacturer,
0x0409, string, sizeof(string));
if (len < 0) {
errorCode = USB_ERROR_IO;
fprintf(stderr,
"Warning: cannot query manufacturer for device: %s\n",
usb_strerror());
} else {
errorCode = USB_ERROR_NOTFOUND;
if (strcmp(string, vendorName) == 0) {
len = usbGetStringAscii(handle,
dev->descriptor.iProduct,
0x0409, string,
sizeof(string));
if (len < 0) {
errorCode = USB_ERROR_IO;
fprintf(stderr,
"Warning: cannot query product for device: %s\n",
usb_strerror());
} else {
errorCode = USB_ERROR_NOTFOUND;
if (strcmp(string, productName) == 0) {
break;
}
}
}
}
usb_close(handle);
handle = NULL;
}
}
if (handle) {
break;
}
}
if (handle != NULL) {
errorCode = 0;
*device = handle;
}
return errorCode;
}
int dev_test(usb_dev_handle * handle, int argc, char **argv) {
unsigned char buffer[8];
int nBytes;
int i, v, r;
if (argc != 2) {
return 1;
}
for (i = 0; i < 1000; i++) {
v = rand() & 0xffff;
nBytes = usb_control_msg(handle,
USB_TYPE_VENDOR | USB_RECIP_DEVICE |
USB_ENDPOINT_IN, CMD_ECHO, v, 0,
(char *)buffer, sizeof(buffer), 5000);
if (nBytes < 2) {
if (nBytes < 0) {
fprintf(stderr, "USB error: %s\n", usb_strerror());
}
fprintf(stderr, "only %d bytes received in iteration %d\n",
nBytes, i);
exit(1);
}
r = buffer[0] | (buffer[1] << 8);
if (r != v) {
fprintf(stderr,
"data error: received 0x%x instead of 0x%x in iteration %d\n",
r, v, i);
exit(1);
}
}
printf("test succeeded\n");
return 0;
}
int dev_set(usb_dev_handle * handle, int argc, char **argv) {
unsigned char buffer[8];
int nBytes;
if (argc != 3) {
return 1;
}
int angle = atoi(argv[2]);
if ((angle < 0) || (angle > 255)) {
fprintf(stderr, "invalid angle: %d\n", angle);
exit(1);
}
nBytes = usb_control_msg(handle,
USB_TYPE_VENDOR | USB_RECIP_DEVICE |
USB_ENDPOINT_OUT, CMD_SET, angle, 0,
(char *)buffer, sizeof(buffer), 5000);
if (nBytes < 0) {
fprintf(stderr, "USB error: %s\n", usb_strerror());
exit(1);
}
return 0;
}
int dev_status(usb_dev_handle * handle, int argc, char **argv) {
int nBytes;
unsigned char buffer[8];
if (argc != 2) {
return 1;
}
nBytes = usb_control_msg(handle,
USB_TYPE_VENDOR | USB_RECIP_DEVICE |
USB_ENDPOINT_IN, CMD_GET, 0, 0, (char *)buffer,
sizeof(buffer), 5000);
if (nBytes < 0) {
fprintf(stderr, "USB error: %s\n", usb_strerror());
exit(1);
}
if (nBytes < 1) {
fprintf(stderr, "only %d bytes status received\n", nBytes);
exit(1);
}
printf("Current servo angle: %d\n", buffer[0]);
return 0;
}

74
commandline/usbdrv.h Normal file
View File

@ -0,0 +1,74 @@
/**
* \file usbdrv.h
* \brief USB-driver-parts for implementing a client.
* \author Ronald Schaten
* \version $Id: usbdrv.h,v 1.1 2006/10/28 12:40:42 rschaten Exp $
*
* License: See documentation.
*/
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <usb.h> /* this is libusb, see http://libusb.sourceforge.net/ */
#include "usbservo.h"
#define USBDEV_SHARED_VENDOR 0x16C0 /**< VOTI */
#define USBDEV_SHARED_PRODUCT 0x05DC /**< Obdev's free shared PID. Use obdev's generic shared VID/PID pair and follow the rules outlined in firmware/usbdrv/USBID-License.txt. */
/* These are error codes for the communication via USB. */
#define USB_ERROR_NOTFOUND 1 /**< Error code if the device isn't found. */
#define USB_ERROR_ACCESS 2 /**< Error code if the device isn't accessible. */
#define USB_ERROR_IO 3 /**< Error code if errors in the communication with the device occur. */
/**
* Reads and converts a string from USB. The conversion to ASCII is 'lossy' (unknown characters become '?').
* \param dev Handle of the USB-Device.
* \param index Index of the required data.
* \param langid Index of the expected language.
* \param buf Buffer to contain the return-string.
* \param buflen Length of buf.
* \return Length of the string.
*/
int usbGetStringAscii(usb_dev_handle * dev, int index, int langid, char *buf, int buflen);
/**
* Connect to the USB-device. Loops through all connected USB-Devices and
* searches our counterpart.
* \param device Handle to address the device.
* \param vendor USBDEV_SHARED_VENDOR as defined.
* \param vendorName In our case "www.schatenseite.de".
* \param product USBDEV_SHARED_PRODUCT as defined.
* \param productName In our case "USB-Servo".
* \return Error code.
*/
int usbOpenDevice(usb_dev_handle ** device, int vendor, char *vendorName, int product, char *productName);
/**
* Test connection to the device. The test consists of writing 1000 random
* numbers to the device and checking the echo. This should discover systematic
* bit errors (e.g. in bit stuffing).
* \param handle Handle to talk to the device.
* \param argc Number of arguments.
* \param argv Arguments.
*/
int dev_test(usb_dev_handle * handle, int argc, char **argv);
/**
* Set the angle of the Servo.
* \param handle Handle to talk to the device.
* \param argc Number of arguments.
* \param argv Arguments.
*/
int dev_set(usb_dev_handle * handle, int argc, char **argv);
/**
* Get the status of the device. Status information is printed in detail (we
* dont't have too many details with only one servo).
* \param handle Handle to talk to the device.
* \param argc Number of arguments.
* \param argv Arguments.
*/
int dev_status(usb_dev_handle * handle, int argc, char **argv);

107
commandline/xservopointer.c Normal file
View File

@ -0,0 +1,107 @@
/**
* \file xservopointer.c
* \brief Tool that uses a servo to point to the mouse cursor under X.
* \author Ronald Schaten
* \version $Id: xservopointer.c,v 1.1 2006/10/28 12:40:42 rschaten Exp $
*
* License: See documentation.
*/
#include <math.h>
#include <time.h>
#include <X11/Xlib.h>
#include <usb.h> /* this is libusb, see http://libusb.sourceforge.net/ */
#include "usbdrv.h"
Display *dpy; /** The display to use */
Window root; /** The root-window of the display */
int rootwidth, rootheight; /** Measurements of the desktop */
int servoposx, servoposy; /** Position of the mouse cursor */
usb_dev_handle *handle = NULL; /** Handle to the device */
/**
* Determines the current cursor position and sets the servo angle.
*/
void update() {
Window wroot, wchild;
int absx, absy, relx, rely, angle;
static int oldabsx = -1000;
static int oldabsy = -1000;
unsigned int modmask;
unsigned char buffer[8];
// get cursor position
XQueryPointer(dpy, root, &wroot, &wchild, &absx, &absy, &relx, &rely, &modmask);
if (oldabsx == absx && oldabsy == absy) {
// position unchanged
} else {
// calculate new angle
angle = (int)(atan((float)(absx - servoposx) / (absy - servoposy)) * (255 / M_PI));
angle = 128 - angle;
printf("absx/absy %d/%d -> angle %d\n", absx, absy, angle);
int nBytes = usb_control_msg(handle,
USB_TYPE_VENDOR | USB_RECIP_DEVICE | USB_ENDPOINT_OUT, CMD_SET, angle, 0,
(char *)buffer, sizeof(buffer), 5000);
if (nBytes < 0) {
fprintf(stderr, "USB error: %s\n", usb_strerror());
exit(1);
}
}
oldabsx = absx;
oldabsy = absy;
}
/**
* Main function. Initializes the X-settings and the USB-device, starts the
* timer and calls the update-function.
* \param argc Number of arguments.
* \param argv Arguments.
* \return Error code.
*/
int main(int argc, char *argv[]) {
// values for timer
struct timespec ts;
ts.tv_sec = 0;
ts.tv_nsec = 100 * 1000 * 1000; // every 100ms
// open display
dpy = XOpenDisplay(NULL);
if (dpy == NULL) {
fprintf(stderr, "xservopointer: Unable to open display\n");
exit(1);
}
root = DefaultRootWindow(dpy);
// determine screen width and height
XWindowAttributes Attributes;
XGetWindowAttributes(dpy, root, &Attributes);
rootwidth = Attributes.width;
rootheight = Attributes.height;
// set servo position
servoposx = rootwidth / 2; // middle of the screen
servoposy = -150; // above the screen
// initialize USB-device
usb_init();
if (usbOpenDevice(&handle, USBDEV_SHARED_VENDOR, "www.schatenseite.de", USBDEV_SHARED_PRODUCT, "USB-Servo") != 0) {
fprintf(stderr, "Could not find USB device \"USB-Servo\" with vid=0x%x pid=0x%x\n", USBDEV_SHARED_VENDOR, USBDEV_SHARED_PRODUCT);
exit(1);
}
// main loop
while (1) {
update();
nanosleep(&ts, NULL);
}
usb_close(handle);
return 0;
}