Rev 283 | Rev 286 | Go to most recent revision | Details | Compare with Previous | Last modification | View Log | RSS feed
Rev | Author | Line No. | Line |
---|---|---|---|
281 | giacomo | 1 | /* |
2 | * Project: S.Ha.R.K. |
||
3 | * |
||
4 | * Coordinators: |
||
5 | * Giorgio Buttazzo <giorgio@sssup.it> |
||
6 | * Paolo Gai <pj@gandalf.sssup.it> |
||
7 | * |
||
8 | * Authors : |
||
9 | * Giacomo Guidi <giacomo@gandalf.sssup.it> |
||
10 | * (see the web pages for full authors list) |
||
11 | * |
||
12 | * ReTiS Lab (Scuola Superiore S.Anna - Pisa - Italy) |
||
13 | * |
||
14 | * http://www.sssup.it |
||
15 | * http://retis.sssup.it |
||
16 | * http://shark.sssup.it |
||
17 | */ |
||
18 | |||
19 | /* |
||
20 | * Copyright (C) 2002 Paolo Gai |
||
21 | * |
||
22 | * This program is free software; you can redistribute it and/or modify |
||
23 | * it under the terms of the GNU General Public License as published by |
||
24 | * the Free Software Foundation; either version 2 of the License, or |
||
25 | * (at your option) any later version. |
||
26 | * |
||
27 | * This program is distributed in the hope that it will be useful, |
||
28 | * but WITHOUT ANY WARRANTY; without even the implied warranty of |
||
29 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
||
30 | * GNU General Public License for more details. |
||
31 | * |
||
32 | * You should have received a copy of the GNU General Public License |
||
33 | * along with this program; if not, write to the Free Software |
||
34 | * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA |
||
35 | * |
||
36 | */ |
||
37 | |||
38 | #include <ll/sys/ll/ll-instr.h> |
||
282 | giacomo | 39 | #include "kernel/kern.h" |
281 | giacomo | 40 | #include "servo.h" |
41 | |||
42 | #define THR 0 |
||
43 | #define RBR 0 |
||
44 | #define IER 1 |
||
45 | #define FCR 2 |
||
46 | #define IIR 2 |
||
47 | #define LCR 3 |
||
48 | #define MCR 4 |
||
49 | #define LSR 5 |
||
50 | #define MSR 6 |
||
51 | #define SPad 7 |
||
52 | |||
53 | #define barrier() __asm__ __volatile__("" ::: "memory"); |
||
54 | |||
282 | giacomo | 55 | #define SERVO_TIMEOUT 100000 /* us */ |
281 | giacomo | 56 | |
282 | giacomo | 57 | #define SERVO_PORT 1 |
58 | |||
281 | giacomo | 59 | int timer_expired = 0; |
283 | giacomo | 60 | int timeout_event; |
281 | giacomo | 61 | unsigned com_base[] = {0x03F8,0x02F8,0x03E8,0x02E8}; |
62 | |||
282 | giacomo | 63 | void set_timer_expired(void *arg) |
281 | giacomo | 64 | { |
283 | giacomo | 65 | timeout_event = NIL; |
281 | giacomo | 66 | timer_expired = 1; |
67 | } |
||
68 | |||
69 | unsigned com_read(unsigned port,unsigned reg) |
||
70 | { |
||
71 | unsigned b; |
||
72 | if (port > 3 || reg > 7) return(0); |
||
73 | b = ll_in(com_base[port]+reg); |
||
74 | return(b); |
||
75 | } |
||
76 | |||
77 | void com_write(unsigned port,unsigned reg,unsigned value) |
||
78 | { |
||
79 | if (port > 3 || reg > 7) return; |
||
80 | ll_out(com_base[port]+reg,value); |
||
81 | } |
||
82 | |||
83 | int com_send(unsigned port,BYTE b) |
||
84 | { |
||
85 | while ((com_read(port,LSR) & 32) == 0 && !timer_expired) |
||
86 | barrier(); |
||
87 | if (!timer_expired) { |
||
88 | com_write(port,THR,b); |
||
89 | return 0; |
||
90 | } else { |
||
91 | return -1; |
||
92 | } |
||
93 | } |
||
94 | |||
95 | int com_receive(unsigned port) |
||
96 | { |
||
97 | while ((com_read(port,LSR) & 1) == 0 && !timer_expired) |
||
98 | barrier(); |
||
99 | if (!timer_expired) { |
||
100 | return((int)(com_read(port,RBR))); |
||
101 | } else { |
||
102 | return -1; |
||
103 | } |
||
104 | } |
||
105 | |||
285 | giacomo | 106 | /* 1000.0100:bbbb.bbbb */ |
281 | giacomo | 107 | int servo_set_RS232_baudrate(int baud) |
108 | { |
||
285 | giacomo | 109 | struct timespec current_time; |
110 | unsigned char b; |
||
111 | int err; |
||
112 | |||
113 | timer_expired = 0; |
||
114 | kern_gettime(¤t_time); |
||
115 | ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
||
116 | timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
||
117 | |||
118 | b = 0x84; |
||
119 | err = com_send(SERVO_PORT, b); |
||
120 | err = com_receive(SERVO_PORT); |
||
121 | if (err != (int)(b)) timer_expired = 1; |
||
281 | giacomo | 122 | |
285 | giacomo | 123 | b = baud & 0xFF; |
124 | err = com_send(SERVO_PORT, b); |
||
125 | err = com_receive(SERVO_PORT); |
||
126 | if (err != (int)(b)) timer_expired = 1; |
||
281 | giacomo | 127 | |
285 | giacomo | 128 | if (timeout_event != NIL) kern_event_delete(timeout_event); |
129 | |||
130 | if (!timer_expired) |
||
131 | return 0; |
||
132 | else |
||
133 | return -1; |
||
134 | |||
281 | giacomo | 135 | } |
136 | |||
285 | giacomo | 137 | /* 1000.0101 */ |
281 | giacomo | 138 | int servo_get_RS232_baudrate(void) |
139 | { |
||
285 | giacomo | 140 | struct timespec current_time; |
141 | unsigned char b; |
||
142 | int err,res; |
||
143 | |||
144 | timer_expired = 0; |
||
145 | kern_gettime(¤t_time); |
||
146 | ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
||
147 | timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
||
148 | |||
149 | b = 0x85; |
||
150 | err = com_send(SERVO_PORT, b); |
||
151 | err = com_receive(SERVO_PORT); |
||
152 | if (err != (int)(b)) timer_expired = 1; |
||
153 | res = com_receive(SERVO_PORT); |
||
154 | |||
155 | if (timeout_event != NIL) kern_event_delete(timeout_event); |
||
156 | |||
157 | if (!timer_expired) |
||
158 | return res; |
||
159 | else |
||
160 | return -1; |
||
281 | giacomo | 161 | |
162 | } |
||
163 | |||
285 | giacomo | 164 | /* 1000.0110 */ |
281 | giacomo | 165 | int servo_store_RS232_buadrate(void) |
166 | { |
||
285 | giacomo | 167 | struct timespec current_time; |
168 | unsigned char b; |
||
169 | int err; |
||
170 | |||
171 | timer_expired = 0; |
||
172 | kern_gettime(¤t_time); |
||
173 | ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
||
174 | timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
||
175 | |||
176 | b = 0x86; |
||
177 | err = com_send(SERVO_PORT, b); |
||
178 | err = com_receive(SERVO_PORT); |
||
179 | if (err != (int)(b)) timer_expired = 1; |
||
180 | |||
181 | if (timeout_event != NIL) kern_event_delete(timeout_event); |
||
182 | |||
183 | if (!timer_expired) |
||
184 | return 0; |
||
185 | else |
||
186 | return -1; |
||
281 | giacomo | 187 | |
188 | } |
||
189 | |||
285 | giacomo | 190 | /* 1000.1000:llll.llll */ |
281 | giacomo | 191 | int servo_set_period(int period) |
192 | { |
||
285 | giacomo | 193 | struct timespec current_time; |
194 | unsigned char b; |
||
195 | int err; |
||
196 | |||
197 | timer_expired = 0; |
||
198 | kern_gettime(¤t_time); |
||
199 | ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
||
200 | timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
||
201 | |||
202 | b = 0x88; |
||
203 | err = com_send(SERVO_PORT, b); |
||
204 | err = com_receive(SERVO_PORT); |
||
205 | if (err != (int)(b)) timer_expired = 1; |
||
206 | |||
207 | b = period/8 & 0xFF; |
||
208 | err = com_send(SERVO_PORT, b); |
||
209 | err = com_receive(SERVO_PORT); |
||
210 | if (err != (int)(b)) timer_expired = 1; |
||
211 | |||
212 | if (timeout_event != NIL) kern_event_delete(timeout_event); |
||
213 | |||
214 | if (!timer_expired) |
||
215 | return 0; |
||
216 | else |
||
217 | return -1; |
||
281 | giacomo | 218 | |
219 | } |
||
220 | |||
285 | giacomo | 221 | /* 1000.1001 */ |
281 | giacomo | 222 | int servo_get_period(void) |
223 | { |
||
285 | giacomo | 224 | struct timespec current_time; |
225 | unsigned char b; |
||
226 | int err,res; |
||
227 | |||
228 | timer_expired = 0; |
||
229 | kern_gettime(¤t_time); |
||
230 | ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
||
231 | timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
||
232 | |||
233 | b = 0x89; |
||
234 | err = com_send(SERVO_PORT, b); |
||
235 | err = com_receive(SERVO_PORT); |
||
236 | if (err != (int)(b)) timer_expired = 1; |
||
237 | res = com_receive(SERVO_PORT); |
||
238 | |||
239 | if (timeout_event != NIL) kern_event_delete(timeout_event); |
||
240 | |||
241 | if (!timer_expired) |
||
242 | return res*8; |
||
243 | else |
||
244 | return -1; |
||
281 | giacomo | 245 | |
246 | } |
||
247 | |||
285 | giacomo | 248 | /* 1000.1010 */ |
281 | giacomo | 249 | int servo_store_period(void) |
250 | { |
||
285 | giacomo | 251 | struct timespec current_time; |
252 | unsigned char b; |
||
253 | int err; |
||
254 | |||
255 | timer_expired = 0; |
||
256 | kern_gettime(¤t_time); |
||
257 | ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
||
258 | timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
||
259 | |||
260 | b = 0x8A; |
||
261 | err = com_send(SERVO_PORT, b); |
||
262 | err = com_receive(SERVO_PORT); |
||
263 | if (err != (int)(b)) timer_expired = 1; |
||
264 | |||
265 | if (timeout_event != NIL) kern_event_delete(timeout_event); |
||
266 | |||
267 | if (!timer_expired) |
||
268 | return 0; |
||
269 | else |
||
270 | return -1; |
||
281 | giacomo | 271 | |
272 | } |
||
273 | |||
283 | giacomo | 274 | /* 1000.1100 */ |
281 | giacomo | 275 | int servo_get_setup_switch(void) |
276 | { |
||
283 | giacomo | 277 | struct timespec current_time; |
278 | unsigned char b; |
||
279 | int err,res; |
||
280 | timer_expired = 0; |
||
281 | kern_gettime(¤t_time); |
||
282 | ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
||
283 | timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
||
281 | giacomo | 284 | |
283 | giacomo | 285 | b = 0x8C; |
286 | err = com_send(SERVO_PORT, b); |
||
287 | err = com_receive(SERVO_PORT); |
||
288 | if (err != (int)(b)) timer_expired = 1; |
||
289 | res = com_receive(SERVO_PORT); |
||
281 | giacomo | 290 | |
283 | giacomo | 291 | if (timeout_event != NIL) kern_event_delete(timeout_event); |
292 | |||
293 | if (!timer_expired) |
||
294 | return res; |
||
295 | else |
||
296 | return -1; |
||
297 | |||
281 | giacomo | 298 | } |
299 | |||
283 | giacomo | 300 | /* 1000.111s */ |
281 | giacomo | 301 | int servo_set_RC5_switch(int data) |
302 | { |
||
283 | giacomo | 303 | struct timespec current_time; |
304 | unsigned char b; |
||
305 | int err; |
||
306 | |||
307 | timer_expired = 0; |
||
308 | kern_gettime(¤t_time); |
||
309 | ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
||
310 | timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
||
281 | giacomo | 311 | |
283 | giacomo | 312 | b = 0x8E | (data & 0x01); |
313 | err = com_send(SERVO_PORT, b); |
||
314 | err = com_receive(SERVO_PORT); |
||
315 | if (err != (int)(b)) timer_expired = 1; |
||
316 | |||
317 | if (timeout_event != NIL) kern_event_delete(timeout_event); |
||
318 | |||
319 | if (!timer_expired) |
||
320 | return 0; |
||
321 | else |
||
322 | return -1; |
||
323 | |||
281 | giacomo | 324 | } |
325 | |||
285 | giacomo | 326 | /* 1000.0000:0000.0mmm */ |
327 | int servo_turn_off(int servo) |
||
328 | { |
||
329 | |||
330 | struct timespec current_time; |
||
331 | unsigned char b; |
||
332 | int err; |
||
333 | |||
334 | if (servo > 7) return -1; |
||
335 | |||
336 | timer_expired = 0; |
||
337 | kern_gettime(¤t_time); |
||
338 | ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
||
339 | timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
||
340 | |||
341 | b = 0x80; |
||
342 | err = com_send(SERVO_PORT, b); |
||
343 | err = com_receive(SERVO_PORT); |
||
344 | if (err != (int)(b)) timer_expired = 1; |
||
345 | |||
346 | b = 0x00 | (servo & 0x07); |
||
347 | err = com_send(SERVO_PORT, b); |
||
348 | err = com_receive(SERVO_PORT); |
||
349 | if (err != (int)(b)) timer_expired = 1; |
||
350 | |||
351 | if (timeout_event != NIL) kern_event_delete(timeout_event); |
||
352 | |||
353 | if (!timer_expired) |
||
354 | return 0; |
||
355 | else |
||
356 | return -1; |
||
357 | |||
358 | } |
||
359 | |||
360 | /* 1000.0000:1000.0mmm */ |
||
361 | int servo_turn_on(int servo) |
||
362 | { |
||
363 | |||
364 | struct timespec current_time; |
||
365 | unsigned char b; |
||
366 | int err; |
||
367 | |||
368 | if (servo > 7) return -1; |
||
369 | |||
370 | timer_expired = 0; |
||
371 | kern_gettime(¤t_time); |
||
372 | ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
||
373 | timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
||
374 | |||
375 | b = 0x80; |
||
376 | err = com_send(SERVO_PORT, b); |
||
377 | err = com_receive(SERVO_PORT); |
||
378 | if (err != (int)(b)) timer_expired = 1; |
||
379 | |||
380 | b = 0x08 | (servo & 0x07); |
||
381 | err = com_send(SERVO_PORT, b); |
||
382 | err = com_receive(SERVO_PORT); |
||
383 | if (err != (int)(b)) timer_expired = 1; |
||
384 | |||
385 | if (timeout_event != NIL) kern_event_delete(timeout_event); |
||
386 | |||
387 | if (!timer_expired) |
||
388 | return 0; |
||
389 | else |
||
390 | return -1; |
||
391 | |||
392 | } |
||
393 | |||
394 | /* 1000.0000:0001.0000 */ |
||
395 | int servo_turn_off_all(void) |
||
396 | { struct timespec current_time; |
||
397 | unsigned char b; |
||
398 | int err; |
||
399 | |||
400 | timer_expired = 0; |
||
401 | kern_gettime(¤t_time); |
||
402 | ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
||
403 | timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
||
404 | |||
405 | b = 0x80; |
||
406 | err = com_send(SERVO_PORT, b); |
||
407 | err = com_receive(SERVO_PORT); |
||
408 | if (err != (int)(b)) timer_expired = 1; |
||
409 | |||
410 | b = 0x10; |
||
411 | err = com_send(SERVO_PORT, b); |
||
412 | err = com_receive(SERVO_PORT); |
||
413 | if (err != (int)(b)) timer_expired = 1; |
||
414 | |||
415 | if (timeout_event != NIL) kern_event_delete(timeout_event); |
||
416 | |||
417 | if (!timer_expired) |
||
418 | return 0; |
||
419 | else |
||
420 | return -1; |
||
421 | |||
422 | } |
||
423 | |||
424 | /* 1000.0000:0001.0001 */ |
||
425 | int servo_turn_on_all(void) |
||
426 | { |
||
427 | struct timespec current_time; |
||
428 | unsigned char b; |
||
429 | int err; |
||
430 | |||
431 | timer_expired = 0; |
||
432 | kern_gettime(¤t_time); |
||
433 | ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
||
434 | timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
||
435 | |||
436 | b = 0x80; |
||
437 | err = com_send(SERVO_PORT, b); |
||
438 | err = com_receive(SERVO_PORT); |
||
439 | if (err != (int)(b)) timer_expired = 1; |
||
440 | |||
441 | b = 0x11; |
||
442 | err = com_send(SERVO_PORT, b); |
||
443 | err = com_receive(SERVO_PORT); |
||
444 | if (err != (int)(b)) timer_expired = 1; |
||
445 | |||
446 | if (timeout_event != NIL) kern_event_delete(timeout_event); |
||
447 | |||
448 | if (!timer_expired) |
||
449 | return 0; |
||
450 | else |
||
451 | return -1; |
||
452 | |||
453 | } |
||
454 | |||
282 | giacomo | 455 | /* 0000.0ppp:0000.vvvv:vvvv.vvvv */ |
281 | giacomo | 456 | int servo_set_angle(int servo, int angle) |
457 | { |
||
282 | giacomo | 458 | struct timespec current_time; |
459 | unsigned char b; |
||
283 | giacomo | 460 | int err; |
282 | giacomo | 461 | |
462 | if (servo > 7) return -1; |
||
463 | |||
464 | timer_expired = 0; |
||
465 | kern_gettime(¤t_time); |
||
466 | ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
||
467 | timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
||
468 | |||
469 | b = 0x00 | (servo & 0x07); |
||
470 | err = com_send(SERVO_PORT, b); |
||
471 | err = com_receive(SERVO_PORT); |
||
283 | giacomo | 472 | if (err != (int)(b)) timer_expired = 1; |
282 | giacomo | 473 | |
474 | b = 0x00 | ((angle >> 8) & 0x0F); |
||
475 | err = com_send(SERVO_PORT, b); |
||
476 | err = com_receive(SERVO_PORT); |
||
283 | giacomo | 477 | if (err != (int)(b)) timer_expired = 1; |
282 | giacomo | 478 | |
479 | b = 0x00 | (angle & 0xFF); |
||
480 | err = com_send(SERVO_PORT, b); |
||
481 | err = com_receive(SERVO_PORT); |
||
283 | giacomo | 482 | if (err != (int)(b)) timer_expired = 1; |
282 | giacomo | 483 | |
283 | giacomo | 484 | if (timeout_event != NIL) kern_event_delete(timeout_event); |
282 | giacomo | 485 | |
283 | giacomo | 486 | if (!timer_expired) |
487 | return 0; |
||
488 | else |
||
489 | return -1; |
||
281 | giacomo | 490 | |
491 | } |
||
492 | |||
282 | giacomo | 493 | /* 0000.1ppp */ |
281 | giacomo | 494 | int servo_get_angle(int servo) |
495 | { |
||
282 | giacomo | 496 | struct timespec current_time; |
497 | unsigned char b; |
||
283 | giacomo | 498 | int err,res; |
282 | giacomo | 499 | |
500 | if (servo > 7) return -1; |
||
501 | |||
502 | timer_expired = 0; |
||
503 | kern_gettime(¤t_time); |
||
504 | ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
||
505 | timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
||
281 | giacomo | 506 | |
282 | giacomo | 507 | b = 0x08 | (servo & 0x07); |
508 | err = com_send(SERVO_PORT, b); |
||
509 | err = com_receive(SERVO_PORT); |
||
283 | giacomo | 510 | if (err != (int)(b)) timer_expired = 1; |
511 | res = com_receive(SERVO_PORT) << 8; |
||
512 | res |= com_receive(SERVO_PORT); |
||
282 | giacomo | 513 | |
283 | giacomo | 514 | if (timeout_event != NIL) kern_event_delete(timeout_event); |
282 | giacomo | 515 | |
516 | if (!timer_expired) |
||
517 | return res; |
||
518 | else |
||
519 | return -1; |
||
520 | |||
281 | giacomo | 521 | } |
522 | |||
282 | giacomo | 523 | /* 0100:0aaa */ |
281 | giacomo | 524 | int servo_get_analog(int port) |
525 | { |
||
526 | |||
282 | giacomo | 527 | struct timespec current_time; |
528 | unsigned char b; |
||
283 | giacomo | 529 | int err,res; |
282 | giacomo | 530 | |
531 | if (port > 4) return -1; |
||
532 | |||
533 | timer_expired = 0; |
||
534 | kern_gettime(¤t_time); |
||
535 | ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
||
536 | timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
||
281 | giacomo | 537 | |
282 | giacomo | 538 | b = 0x40 | (port & 7); |
539 | err = com_send(SERVO_PORT, b); |
||
540 | err = com_receive(SERVO_PORT); |
||
283 | giacomo | 541 | if (err != (int)(b)) timer_expired = 1; |
282 | giacomo | 542 | res = com_receive(SERVO_PORT) << 8; |
543 | res |= com_receive(SERVO_PORT); |
||
544 | |||
283 | giacomo | 545 | if (timeout_event != NIL) kern_event_delete(timeout_event); |
282 | giacomo | 546 | |
547 | if (!timer_expired) |
||
548 | return res; |
||
549 | else |
||
550 | return -1; |
||
551 | |||
281 | giacomo | 552 | } |
553 |