00001
00010 #include <math.h>
00011 #include <time.h>
00012
00013 #include <X11/Xlib.h>
00014
00015 #include <usb.h>
00016
00017 #include "usbdrv.h"
00018
00019 Display *dpy;
00020 Window root;
00022 int rootwidth, rootheight;
00023 int servoposx, servoposy;
00025 usb_dev_handle *handle = NULL;
00030 void update() {
00031 Window wroot, wchild;
00032 int absx, absy, relx, rely, angle;
00033 static int oldabsx = -1000;
00034 static int oldabsy = -1000;
00035 unsigned int modmask;
00036 unsigned char buffer[8];
00037
00038
00039 XQueryPointer(dpy, root, &wroot, &wchild, &absx, &absy, &relx, &rely, &modmask);
00040
00041 if (oldabsx == absx && oldabsy == absy) {
00042
00043 } else {
00044
00045 angle = (int)(atan((float)(absx - servoposx) / (absy - servoposy)) * (255 / M_PI));
00046 angle = 128 - angle;
00047 printf("absx/absy %d/%d -> angle %d\n", absx, absy, angle);
00048 int nBytes = usb_control_msg(handle,
00049 USB_TYPE_VENDOR | USB_RECIP_DEVICE | USB_ENDPOINT_OUT, CMD_SET, angle, 0,
00050 (char *)buffer, sizeof(buffer), 5000);
00051
00052 if (nBytes < 0) {
00053 fprintf(stderr, "USB error: %s\n", usb_strerror());
00054 exit(1);
00055 }
00056 }
00057 oldabsx = absx;
00058 oldabsy = absy;
00059 }
00060
00068 int main(int argc, char *argv[]) {
00069
00070 struct timespec ts;
00071 ts.tv_sec = 0;
00072 ts.tv_nsec = 100 * 1000 * 1000;
00073
00074
00075 dpy = XOpenDisplay(NULL);
00076 if (dpy == NULL) {
00077 fprintf(stderr, "xservopointer: Unable to open display\n");
00078 exit(1);
00079 }
00080
00081 root = DefaultRootWindow(dpy);
00082
00083
00084 XWindowAttributes Attributes;
00085 XGetWindowAttributes(dpy, root, &Attributes);
00086 rootwidth = Attributes.width;
00087 rootheight = Attributes.height;
00088
00089
00090 servoposx = rootwidth / 2;
00091 servoposy = -150;
00092
00093
00094 usb_init();
00095 if (usbOpenDevice(&handle, USBDEV_SHARED_VENDOR, "www.schatenseite.de", USBDEV_SHARED_PRODUCT, "USB-Servo") != 0) {
00096 fprintf(stderr, "Could not find USB device \"USB-Servo\" with vid=0x%x pid=0x%x\n", USBDEV_SHARED_VENDOR, USBDEV_SHARED_PRODUCT);
00097 exit(1);
00098 }
00099
00100
00101 while (1) {
00102 update();
00103 nanosleep(&ts, NULL);
00104 }
00105 usb_close(handle);
00106 return 0;
00107 }