]>
Commit | Line | Data |
---|---|---|
1da177e4 LT |
1 | /* |
2 | * USB PhidgetServo driver 1.0 | |
3 | * | |
4 | * Copyright (C) 2004 Sean Young <sean@mess.org> | |
5 | * | |
6 | * This program is free software; you can redistribute it and/or modify | |
7 | * it under the terms of the GNU General Public License as published by | |
8 | * the Free Software Foundation; either version 2 of the License, or | |
9 | * (at your option) any later version. | |
10 | * | |
11 | * This is a driver for the USB PhidgetServo version 2.0 and 3.0 servo | |
12 | * controllers available at: http://www.phidgets.com/ | |
13 | * | |
14 | * Note that the driver takes input as: degrees.minutes | |
15 | * | |
16 | * CAUTION: Generally you should use 0 < degrees < 180 as anything else | |
17 | * is probably beyond the range of your servo and may damage it. | |
18 | * | |
19 | * Jun 16, 2004: Sean Young <sean@mess.org> | |
20 | * - cleanups | |
21 | * - was using memory after kfree() | |
22 | * Aug 8, 2004: Sean Young <sean@mess.org> | |
23 | * - set the highest angle as high as the hardware allows, there are | |
24 | * some odd servos out there | |
25 | * | |
26 | */ | |
27 | ||
28 | #include <linux/config.h> | |
29 | #ifdef CONFIG_USB_DEBUG | |
30 | #define DEBUG 1 | |
31 | #endif | |
32 | #include <linux/kernel.h> | |
33 | #include <linux/errno.h> | |
34 | #include <linux/init.h> | |
35 | #include <linux/slab.h> | |
36 | #include <linux/module.h> | |
37 | #include <linux/usb.h> | |
38 | ||
39 | #define DRIVER_AUTHOR "Sean Young <sean@mess.org>" | |
40 | #define DRIVER_DESC "USB PhidgetServo Driver" | |
41 | ||
42 | #define VENDOR_ID_GLAB 0x06c2 | |
43 | #define DEVICE_ID_GLAB_PHIDGETSERVO_QUAD 0x0038 | |
44 | #define DEVICE_ID_GLAB_PHIDGETSERVO_UNI 0x0039 | |
45 | ||
46 | #define VENDOR_ID_WISEGROUP 0x0925 | |
47 | #define VENDOR_ID_WISEGROUP_PHIDGETSERVO_QUAD 0x8101 | |
48 | #define VENDOR_ID_WISEGROUP_PHIDGETSERVO_UNI 0x8104 | |
49 | ||
50 | #define SERVO_VERSION_30 0x01 | |
51 | #define SERVO_COUNT_QUAD 0x02 | |
52 | ||
53 | static struct usb_device_id id_table[] = { | |
54 | { | |
55 | USB_DEVICE(VENDOR_ID_GLAB, DEVICE_ID_GLAB_PHIDGETSERVO_QUAD), | |
56 | .driver_info = SERVO_VERSION_30 | SERVO_COUNT_QUAD | |
57 | }, | |
58 | { | |
59 | USB_DEVICE(VENDOR_ID_GLAB, DEVICE_ID_GLAB_PHIDGETSERVO_UNI), | |
60 | .driver_info = SERVO_VERSION_30 | |
61 | }, | |
62 | { | |
63 | USB_DEVICE(VENDOR_ID_WISEGROUP, | |
64 | VENDOR_ID_WISEGROUP_PHIDGETSERVO_QUAD), | |
65 | .driver_info = SERVO_COUNT_QUAD | |
66 | }, | |
67 | { | |
68 | USB_DEVICE(VENDOR_ID_WISEGROUP, | |
69 | VENDOR_ID_WISEGROUP_PHIDGETSERVO_UNI), | |
70 | .driver_info = 0 | |
71 | }, | |
72 | {} | |
73 | }; | |
74 | ||
75 | MODULE_DEVICE_TABLE(usb, id_table); | |
76 | ||
77 | struct phidget_servo { | |
78 | struct usb_device *udev; | |
79 | ulong type; | |
80 | int pulse[4]; | |
81 | int degrees[4]; | |
82 | int minutes[4]; | |
83 | }; | |
84 | ||
85 | static int | |
86 | change_position_v30(struct phidget_servo *servo, int servo_no, int degrees, | |
87 | int minutes) | |
88 | { | |
89 | int retval; | |
90 | unsigned char *buffer; | |
91 | ||
92 | if (degrees < -23 || degrees > 362) | |
93 | return -EINVAL; | |
94 | ||
95 | buffer = kmalloc(6, GFP_KERNEL); | |
96 | if (!buffer) { | |
97 | dev_err(&servo->udev->dev, "%s - out of memory\n", | |
98 | __FUNCTION__); | |
99 | return -ENOMEM; | |
100 | } | |
101 | ||
102 | /* | |
103 | * pulse = 0 - 4095 | |
104 | * angle = 0 - 180 degrees | |
105 | * | |
106 | * pulse = angle * 10.6 + 243.8 | |
107 | */ | |
108 | servo->pulse[servo_no] = ((degrees*60 + minutes)*106 + 2438*60)/600; | |
109 | servo->degrees[servo_no]= degrees; | |
110 | servo->minutes[servo_no]= minutes; | |
111 | ||
112 | /* | |
113 | * The PhidgetServo v3.0 is controlled by sending 6 bytes, | |
114 | * 4 * 12 bits for each servo. | |
115 | * | |
116 | * low = lower 8 bits pulse | |
117 | * high = higher 4 bits pulse | |
118 | * | |
119 | * offset bits | |
120 | * +---+-----------------+ | |
121 | * | 0 | low 0 | | |
122 | * +---+--------+--------+ | |
123 | * | 1 | high 1 | high 0 | | |
124 | * +---+--------+--------+ | |
125 | * | 2 | low 1 | | |
126 | * +---+-----------------+ | |
127 | * | 3 | low 2 | | |
128 | * +---+--------+--------+ | |
129 | * | 4 | high 3 | high 2 | | |
130 | * +---+--------+--------+ | |
131 | * | 5 | low 3 | | |
132 | * +---+-----------------+ | |
133 | */ | |
134 | ||
135 | buffer[0] = servo->pulse[0] & 0xff; | |
136 | buffer[1] = (servo->pulse[0] >> 8 & 0x0f) | |
137 | | (servo->pulse[1] >> 4 & 0xf0); | |
138 | buffer[2] = servo->pulse[1] & 0xff; | |
139 | buffer[3] = servo->pulse[2] & 0xff; | |
140 | buffer[4] = (servo->pulse[2] >> 8 & 0x0f) | |
141 | | (servo->pulse[3] >> 4 & 0xf0); | |
142 | buffer[5] = servo->pulse[3] & 0xff; | |
143 | ||
144 | dev_dbg(&servo->udev->dev, | |
145 | "data: %02x %02x %02x %02x %02x %02x\n", | |
146 | buffer[0], buffer[1], buffer[2], | |
147 | buffer[3], buffer[4], buffer[5]); | |
148 | ||
149 | retval = usb_control_msg(servo->udev, | |
150 | usb_sndctrlpipe(servo->udev, 0), | |
151 | 0x09, 0x21, 0x0200, 0x0000, buffer, 6, 2000); | |
152 | ||
153 | kfree(buffer); | |
154 | ||
155 | return retval; | |
156 | } | |
157 | ||
158 | static int | |
159 | change_position_v20(struct phidget_servo *servo, int servo_no, int degrees, | |
160 | int minutes) | |
161 | { | |
162 | int retval; | |
163 | unsigned char *buffer; | |
164 | ||
165 | if (degrees < -23 || degrees > 278) | |
166 | return -EINVAL; | |
167 | ||
168 | buffer = kmalloc(2, GFP_KERNEL); | |
169 | if (!buffer) { | |
170 | dev_err(&servo->udev->dev, "%s - out of memory\n", | |
171 | __FUNCTION__); | |
172 | return -ENOMEM; | |
173 | } | |
174 | ||
175 | /* | |
176 | * angle = 0 - 180 degrees | |
177 | * pulse = angle + 23 | |
178 | */ | |
179 | servo->pulse[servo_no]= degrees + 23; | |
180 | servo->degrees[servo_no]= degrees; | |
181 | servo->minutes[servo_no]= 0; | |
182 | ||
183 | /* | |
184 | * The PhidgetServo v2.0 is controlled by sending two bytes. The | |
185 | * first byte is the servo number xor'ed with 2: | |
186 | * | |
187 | * servo 0 = 2 | |
188 | * servo 1 = 3 | |
189 | * servo 2 = 0 | |
190 | * servo 3 = 1 | |
191 | * | |
192 | * The second byte is the position. | |
193 | */ | |
194 | ||
195 | buffer[0] = servo_no ^ 2; | |
196 | buffer[1] = servo->pulse[servo_no]; | |
197 | ||
198 | dev_dbg(&servo->udev->dev, "data: %02x %02x\n", buffer[0], buffer[1]); | |
199 | ||
200 | retval = usb_control_msg(servo->udev, | |
201 | usb_sndctrlpipe(servo->udev, 0), | |
202 | 0x09, 0x21, 0x0200, 0x0000, buffer, 2, 2000); | |
203 | ||
204 | kfree(buffer); | |
205 | ||
206 | return retval; | |
207 | } | |
208 | ||
209 | #define show_set(value) \ | |
060b8845 | 210 | static ssize_t set_servo##value (struct device *dev, struct device_attribute *attr, \ |
1da177e4 LT |
211 | const char *buf, size_t count) \ |
212 | { \ | |
213 | int degrees, minutes, retval; \ | |
214 | struct usb_interface *intf = to_usb_interface (dev); \ | |
215 | struct phidget_servo *servo = usb_get_intfdata (intf); \ | |
216 | \ | |
217 | minutes = 0; \ | |
218 | /* must at least convert degrees */ \ | |
219 | if (sscanf (buf, "%d.%d", °rees, &minutes) < 1) { \ | |
220 | return -EINVAL; \ | |
221 | } \ | |
222 | \ | |
223 | if (minutes < 0 || minutes > 59) \ | |
224 | return -EINVAL; \ | |
225 | \ | |
226 | if (servo->type & SERVO_VERSION_30) \ | |
227 | retval = change_position_v30 (servo, value, degrees, \ | |
228 | minutes); \ | |
229 | else \ | |
230 | retval = change_position_v20 (servo, value, degrees, \ | |
231 | minutes); \ | |
232 | \ | |
233 | return retval < 0 ? retval : count; \ | |
234 | } \ | |
235 | \ | |
060b8845 | 236 | static ssize_t show_servo##value (struct device *dev, struct device_attribute *attr, char *buf) \ |
1da177e4 LT |
237 | { \ |
238 | struct usb_interface *intf = to_usb_interface (dev); \ | |
239 | struct phidget_servo *servo = usb_get_intfdata (intf); \ | |
240 | \ | |
241 | return sprintf (buf, "%d.%02d\n", servo->degrees[value], \ | |
242 | servo->minutes[value]); \ | |
243 | } \ | |
244 | static DEVICE_ATTR(servo##value, S_IWUGO | S_IRUGO, \ | |
245 | show_servo##value, set_servo##value); | |
246 | ||
247 | show_set(0); | |
248 | show_set(1); | |
249 | show_set(2); | |
250 | show_set(3); | |
251 | ||
252 | static int | |
253 | servo_probe(struct usb_interface *interface, const struct usb_device_id *id) | |
254 | { | |
255 | struct usb_device *udev = interface_to_usbdev(interface); | |
256 | struct phidget_servo *dev; | |
257 | ||
258 | dev = kmalloc(sizeof (struct phidget_servo), GFP_KERNEL); | |
259 | if (dev == NULL) { | |
260 | dev_err(&interface->dev, "%s - out of memory\n", __FUNCTION__); | |
261 | return -ENOMEM; | |
262 | } | |
263 | memset(dev, 0x00, sizeof (*dev)); | |
264 | ||
265 | dev->udev = usb_get_dev(udev); | |
266 | dev->type = id->driver_info; | |
267 | usb_set_intfdata(interface, dev); | |
268 | ||
269 | device_create_file(&interface->dev, &dev_attr_servo0); | |
270 | if (dev->type & SERVO_COUNT_QUAD) { | |
271 | device_create_file(&interface->dev, &dev_attr_servo1); | |
272 | device_create_file(&interface->dev, &dev_attr_servo2); | |
273 | device_create_file(&interface->dev, &dev_attr_servo3); | |
274 | } | |
275 | ||
276 | dev_info(&interface->dev, "USB %d-Motor PhidgetServo v%d.0 attached\n", | |
277 | dev->type & SERVO_COUNT_QUAD ? 4 : 1, | |
278 | dev->type & SERVO_VERSION_30 ? 3 : 2); | |
279 | ||
280 | if(!(dev->type & SERVO_VERSION_30)) | |
281 | dev_info(&interface->dev, | |
282 | "WARNING: v2.0 not tested! Please report if it works.\n"); | |
283 | ||
284 | return 0; | |
285 | } | |
286 | ||
287 | static void | |
288 | servo_disconnect(struct usb_interface *interface) | |
289 | { | |
290 | struct phidget_servo *dev; | |
291 | ||
292 | dev = usb_get_intfdata(interface); | |
293 | usb_set_intfdata(interface, NULL); | |
294 | ||
295 | device_remove_file(&interface->dev, &dev_attr_servo0); | |
296 | if (dev->type & SERVO_COUNT_QUAD) { | |
297 | device_remove_file(&interface->dev, &dev_attr_servo1); | |
298 | device_remove_file(&interface->dev, &dev_attr_servo2); | |
299 | device_remove_file(&interface->dev, &dev_attr_servo3); | |
300 | } | |
301 | ||
302 | usb_put_dev(dev->udev); | |
303 | ||
304 | dev_info(&interface->dev, "USB %d-Motor PhidgetServo v%d.0 detached\n", | |
305 | dev->type & SERVO_COUNT_QUAD ? 4 : 1, | |
306 | dev->type & SERVO_VERSION_30 ? 3 : 2); | |
307 | ||
308 | kfree(dev); | |
309 | } | |
310 | ||
311 | static struct usb_driver servo_driver = { | |
312 | .owner = THIS_MODULE, | |
313 | .name = "phidgetservo", | |
314 | .probe = servo_probe, | |
315 | .disconnect = servo_disconnect, | |
316 | .id_table = id_table | |
317 | }; | |
318 | ||
319 | static int __init | |
320 | phidget_servo_init(void) | |
321 | { | |
322 | int retval; | |
323 | ||
324 | retval = usb_register(&servo_driver); | |
325 | if (retval) | |
326 | err("usb_register failed. Error number %d", retval); | |
327 | ||
328 | return retval; | |
329 | } | |
330 | ||
331 | static void __exit | |
332 | phidget_servo_exit(void) | |
333 | { | |
334 | usb_deregister(&servo_driver); | |
335 | } | |
336 | ||
337 | module_init(phidget_servo_init); | |
338 | module_exit(phidget_servo_exit); | |
339 | ||
340 | MODULE_AUTHOR(DRIVER_AUTHOR); | |
341 | MODULE_DESCRIPTION(DRIVER_DESC); | |
342 | MODULE_LICENSE("GPL"); |