Rev 286 | Rev 290 | 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 | |||
286 | giacomo | 53 | /* Parity value */ |
54 | #define NONE 0 |
||
55 | #define ODD 1 |
||
56 | #define EVEN 3 |
||
57 | |||
281 | giacomo | 58 | #define barrier() __asm__ __volatile__("" ::: "memory"); |
59 | |||
286 | giacomo | 60 | #define SERVO_DEBUG |
61 | |||
282 | giacomo | 62 | #define SERVO_TIMEOUT 100000 /* us */ |
281 | giacomo | 63 | |
286 | giacomo | 64 | #define SERVO_SPEED 38400 |
65 | #define SERVO_PARITY NONE |
||
66 | #define SERVO_LEN 8 |
||
67 | #define SERVO_STOP 1 |
||
282 | giacomo | 68 | |
281 | giacomo | 69 | int timer_expired = 0; |
283 | giacomo | 70 | int timeout_event; |
289 | giacomo | 71 | int servo_port; |
281 | giacomo | 72 | |
286 | giacomo | 73 | const unsigned com_base[] = {0x03F8,0x02F8,0x03E8,0x02E8}; |
74 | |||
75 | const int BaudTable[] = { |
||
76 | 1200, |
||
77 | 2400, |
||
78 | 4800, |
||
79 | 9600, |
||
80 | 14400, |
||
81 | 19200, |
||
82 | 38400, |
||
83 | 57600, |
||
84 | 115200, |
||
85 | 230400, |
||
86 | -1}; |
||
87 | |||
88 | /* 20MHz: SPBRG, BAUD BRGH=0, BAUD BRGH=1 */ |
||
89 | const int Baud20MhzData[] = { |
||
90 | 255, 1221, 4882, |
||
91 | 254, 1225, 4902, |
||
92 | 253, 1230, 4921, |
||
93 | 252, 1235, 4941, |
||
94 | 133, 2332, 9328, |
||
95 | 132, 2350, 9398, |
||
96 | 131, 2367, 9470, |
||
97 | 130, 2385, 9542, |
||
98 | 129, 2404, 9615, |
||
99 | 128, 2422, 9690, |
||
100 | 127, 2441, 9766, |
||
101 | 126, 2461, 9842, |
||
102 | 88, 0, 14045, |
||
103 | 87, 0, 14204, |
||
104 | 86, 0, 14368, |
||
105 | 85, 0, 14535, |
||
106 | 84, 0, 14706, |
||
107 | 66, 4664, 18657, |
||
108 | 65, 4735, 18939, |
||
109 | 64, 4808, 19231, |
||
110 | 63, 4883, 19531, |
||
111 | 32, 9470, 37879, |
||
112 | 31, 9766, 39062, |
||
113 | 21, 14205, 56818, |
||
114 | 15, 19531, 0, |
||
115 | 10, 0, 113636, |
||
116 | 7, 39062, 0, |
||
117 | -1, -1, -1}; |
||
118 | |||
282 | giacomo | 119 | void set_timer_expired(void *arg) |
281 | giacomo | 120 | { |
283 | giacomo | 121 | timeout_event = NIL; |
281 | giacomo | 122 | timer_expired = 1; |
123 | } |
||
124 | |||
125 | unsigned com_read(unsigned port,unsigned reg) |
||
126 | { |
||
127 | unsigned b; |
||
128 | if (port > 3 || reg > 7) return(0); |
||
129 | b = ll_in(com_base[port]+reg); |
||
130 | return(b); |
||
131 | } |
||
132 | |||
133 | void com_write(unsigned port,unsigned reg,unsigned value) |
||
134 | { |
||
135 | if (port > 3 || reg > 7) return; |
||
136 | ll_out(com_base[port]+reg,value); |
||
137 | } |
||
138 | |||
139 | int com_send(unsigned port,BYTE b) |
||
140 | { |
||
141 | while ((com_read(port,LSR) & 32) == 0 && !timer_expired) |
||
142 | barrier(); |
||
143 | if (!timer_expired) { |
||
144 | com_write(port,THR,b); |
||
145 | return 0; |
||
146 | } else { |
||
147 | return -1; |
||
148 | } |
||
149 | } |
||
150 | |||
151 | int com_receive(unsigned port) |
||
152 | { |
||
153 | while ((com_read(port,LSR) & 1) == 0 && !timer_expired) |
||
154 | barrier(); |
||
155 | if (!timer_expired) { |
||
156 | return((int)(com_read(port,RBR))); |
||
157 | } else { |
||
158 | return -1; |
||
159 | } |
||
160 | } |
||
161 | |||
286 | giacomo | 162 | int com_open(unsigned port,DWORD speed,BYTE parity,BYTE len,BYTE stop) |
163 | { |
||
164 | unsigned long div,b_mask; |
||
165 | |||
166 | /* Now set up the serial link */ |
||
167 | b_mask = (parity & 3) * 8 + (stop & 1) * 4 + ((len - 5) & 3); |
||
168 | div = 115200L / speed; |
||
169 | /* Clear serial interrupt enable register */ |
||
170 | com_write(port,IER,0); |
||
171 | /* Empty input buffer */ |
||
172 | com_read(port,RBR); |
||
173 | /* Activate DLAB bit for speed setting */ |
||
174 | com_write(port,LCR,0x80); |
||
175 | /* Load baud divisor */ |
||
176 | com_write(port,0,div & 0x00FF); |
||
177 | div >>= 8; |
||
178 | com_write(port,1,div & 0x00FF); |
||
179 | /* Load control word (parity,stop bit,bit len) */ |
||
180 | com_write(port,LCR,b_mask); |
||
181 | /* Attiva OUT1 & OUT2 */ |
||
182 | com_write(port,MCR,0x0C); |
||
183 | |||
184 | return 0; |
||
185 | } |
||
186 | |||
187 | int com_close(unsigned port) |
||
188 | { |
||
189 | |||
190 | com_write(port,IER,0); |
||
191 | com_read(port,IIR); |
||
192 | com_read(port,RBR); |
||
193 | |||
194 | return 0; |
||
195 | |||
196 | } |
||
197 | |||
289 | giacomo | 198 | int servo_open(int port) |
286 | giacomo | 199 | { |
200 | int err; |
||
201 | |||
289 | giacomo | 202 | servo_port = (unsigned)(port); |
203 | err = com_open(servo_port, SERVO_SPEED, SERVO_PARITY, SERVO_LEN, SERVO_STOP); |
||
286 | giacomo | 204 | |
205 | return err; |
||
206 | |||
207 | } |
||
208 | |||
209 | int servo_close(void) |
||
210 | { |
||
211 | int err; |
||
212 | |||
289 | giacomo | 213 | err = com_close(servo_port); |
286 | giacomo | 214 | |
215 | return err; |
||
216 | |||
217 | } |
||
218 | |||
219 | /* 1000.011w:bbbb.bbbb */ |
||
281 | giacomo | 220 | int servo_set_RS232_baudrate(int baud) |
221 | { |
||
285 | giacomo | 222 | struct timespec current_time; |
223 | unsigned char b; |
||
286 | giacomo | 224 | int err,diff,mindiff,i; |
225 | unsigned char spbrg, w; |
||
226 | |||
227 | i = 0; |
||
228 | while(BaudTable[i] != baud && BaudTable[i] != -1) i++; |
||
229 | if (BaudTable[i] == -1) { |
||
230 | kern_printf("SERVO:Error wrong baud rate\n"); |
||
231 | return -1; |
||
232 | } |
||
233 | |||
234 | i = 0; |
||
235 | mindiff = 0xFFFFFF; |
||
236 | spbrg = 0; |
||
237 | w = 0; |
||
238 | while(Baud20MhzData[i] != -1) { |
||
239 | diff = abs(Baud20MhzData[i+1] - baud); |
||
240 | if (diff < mindiff) { |
||
241 | mindiff = diff; |
||
242 | spbrg = (unsigned char)(Baud20MhzData[i+1]); |
||
243 | w = 0; |
||
244 | } |
||
245 | diff = abs(Baud20MhzData[i+2] - baud); |
||
246 | if (diff < mindiff) { |
||
247 | mindiff = diff; |
||
248 | spbrg = (unsigned char)(Baud20MhzData[i+2]); |
||
249 | w = 1; |
||
250 | } |
||
251 | i += 3; |
||
252 | } |
||
253 | |||
254 | #ifdef SERVO_DEBUG |
||
255 | kern_printf("(SERVO:SBPRG %d W %d)",spbrg,w); |
||
256 | #endif |
||
257 | |||
285 | giacomo | 258 | timer_expired = 0; |
259 | kern_gettime(¤t_time); |
||
260 | ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
||
261 | timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
||
262 | |||
286 | giacomo | 263 | b = 0x86 | (w & 0x01); |
289 | giacomo | 264 | err = com_send(servo_port, b); |
265 | err = com_receive(servo_port); |
||
285 | giacomo | 266 | if (err != (int)(b)) timer_expired = 1; |
281 | giacomo | 267 | |
286 | giacomo | 268 | b = spbrg; |
289 | giacomo | 269 | err = com_send(servo_port, b); |
270 | err = com_receive(servo_port); |
||
285 | giacomo | 271 | if (err != (int)(b)) timer_expired = 1; |
281 | giacomo | 272 | |
285 | giacomo | 273 | if (timeout_event != NIL) kern_event_delete(timeout_event); |
286 | giacomo | 274 | |
289 | giacomo | 275 | com_close(servo_port); |
276 | com_open(servo_port, baud, SERVO_PARITY, SERVO_LEN, SERVO_STOP); |
||
286 | giacomo | 277 | |
285 | giacomo | 278 | if (!timer_expired) |
279 | return 0; |
||
280 | else |
||
281 | return -1; |
||
282 | |||
281 | giacomo | 283 | } |
284 | |||
285 | giacomo | 285 | /* 1000.0101 */ |
281 | giacomo | 286 | int servo_get_RS232_baudrate(void) |
287 | { |
||
285 | giacomo | 288 | struct timespec current_time; |
289 | unsigned char b; |
||
290 | int err,res; |
||
291 | |||
292 | timer_expired = 0; |
||
293 | kern_gettime(¤t_time); |
||
294 | ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
||
295 | timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
||
296 | |||
297 | b = 0x85; |
||
289 | giacomo | 298 | err = com_send(servo_port, b); |
299 | err = com_receive(servo_port); |
||
285 | giacomo | 300 | if (err != (int)(b)) timer_expired = 1; |
289 | giacomo | 301 | res = com_receive(servo_port); |
285 | giacomo | 302 | |
303 | if (timeout_event != NIL) kern_event_delete(timeout_event); |
||
304 | |||
305 | if (!timer_expired) |
||
306 | return res; |
||
307 | else |
||
308 | return -1; |
||
281 | giacomo | 309 | |
310 | } |
||
311 | |||
286 | giacomo | 312 | /* 1000.0100 */ |
281 | giacomo | 313 | int servo_store_RS232_buadrate(void) |
314 | { |
||
285 | giacomo | 315 | struct timespec current_time; |
316 | unsigned char b; |
||
317 | int err; |
||
318 | |||
319 | timer_expired = 0; |
||
320 | kern_gettime(¤t_time); |
||
321 | ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
||
322 | timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
||
323 | |||
286 | giacomo | 324 | b = 0x84; |
289 | giacomo | 325 | err = com_send(servo_port, b); |
326 | err = com_receive(servo_port); |
||
285 | giacomo | 327 | if (err != (int)(b)) timer_expired = 1; |
328 | |||
329 | if (timeout_event != NIL) kern_event_delete(timeout_event); |
||
330 | |||
331 | if (!timer_expired) |
||
332 | return 0; |
||
333 | else |
||
334 | return -1; |
||
281 | giacomo | 335 | |
336 | } |
||
337 | |||
289 | giacomo | 338 | /* 1000.1010:llll.llll */ |
281 | giacomo | 339 | int servo_set_period(int period) |
340 | { |
||
285 | giacomo | 341 | struct timespec current_time; |
342 | unsigned char b; |
||
343 | int err; |
||
344 | |||
345 | timer_expired = 0; |
||
346 | kern_gettime(¤t_time); |
||
347 | ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
||
348 | timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
||
349 | |||
289 | giacomo | 350 | b = 0x8A; |
351 | err = com_send(servo_port, b); |
||
352 | err = com_receive(servo_port); |
||
285 | giacomo | 353 | if (err != (int)(b)) timer_expired = 1; |
354 | |||
355 | b = period/8 & 0xFF; |
||
289 | giacomo | 356 | err = com_send(servo_port, b); |
357 | err = com_receive(servo_port); |
||
285 | giacomo | 358 | if (err != (int)(b)) timer_expired = 1; |
359 | |||
360 | if (timeout_event != NIL) kern_event_delete(timeout_event); |
||
361 | |||
362 | if (!timer_expired) |
||
363 | return 0; |
||
364 | else |
||
365 | return -1; |
||
281 | giacomo | 366 | |
367 | } |
||
368 | |||
285 | giacomo | 369 | /* 1000.1001 */ |
281 | giacomo | 370 | int servo_get_period(void) |
371 | { |
||
285 | giacomo | 372 | struct timespec current_time; |
373 | unsigned char b; |
||
374 | int err,res; |
||
375 | |||
376 | timer_expired = 0; |
||
377 | kern_gettime(¤t_time); |
||
378 | ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
||
379 | timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
||
380 | |||
381 | b = 0x89; |
||
289 | giacomo | 382 | err = com_send(servo_port, b); |
383 | err = com_receive(servo_port); |
||
285 | giacomo | 384 | if (err != (int)(b)) timer_expired = 1; |
289 | giacomo | 385 | res = com_receive(servo_port); |
285 | giacomo | 386 | |
387 | if (timeout_event != NIL) kern_event_delete(timeout_event); |
||
388 | |||
389 | if (!timer_expired) |
||
390 | return res*8; |
||
391 | else |
||
392 | return -1; |
||
281 | giacomo | 393 | |
394 | } |
||
395 | |||
289 | giacomo | 396 | /* 1000.1000 */ |
281 | giacomo | 397 | int servo_store_period(void) |
398 | { |
||
285 | giacomo | 399 | struct timespec current_time; |
400 | unsigned char b; |
||
401 | int err; |
||
402 | |||
403 | timer_expired = 0; |
||
404 | kern_gettime(¤t_time); |
||
405 | ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
||
406 | timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
||
407 | |||
289 | giacomo | 408 | b = 0x88; |
409 | err = com_send(servo_port, b); |
||
410 | err = com_receive(servo_port); |
||
285 | giacomo | 411 | if (err != (int)(b)) timer_expired = 1; |
412 | |||
413 | if (timeout_event != NIL) kern_event_delete(timeout_event); |
||
414 | |||
415 | if (!timer_expired) |
||
416 | return 0; |
||
417 | else |
||
418 | return -1; |
||
281 | giacomo | 419 | |
420 | } |
||
421 | |||
283 | giacomo | 422 | /* 1000.1100 */ |
281 | giacomo | 423 | int servo_get_setup_switch(void) |
424 | { |
||
283 | giacomo | 425 | struct timespec current_time; |
426 | unsigned char b; |
||
427 | int err,res; |
||
428 | timer_expired = 0; |
||
429 | kern_gettime(¤t_time); |
||
430 | ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
||
431 | timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
||
281 | giacomo | 432 | |
283 | giacomo | 433 | b = 0x8C; |
289 | giacomo | 434 | err = com_send(servo_port, b); |
435 | err = com_receive(servo_port); |
||
283 | giacomo | 436 | if (err != (int)(b)) timer_expired = 1; |
289 | giacomo | 437 | res = com_receive(servo_port); |
281 | giacomo | 438 | |
283 | giacomo | 439 | if (timeout_event != NIL) kern_event_delete(timeout_event); |
440 | |||
441 | if (!timer_expired) |
||
442 | return res; |
||
443 | else |
||
444 | return -1; |
||
445 | |||
281 | giacomo | 446 | } |
447 | |||
283 | giacomo | 448 | /* 1000.111s */ |
281 | giacomo | 449 | int servo_set_RC5_switch(int data) |
450 | { |
||
283 | giacomo | 451 | struct timespec current_time; |
452 | unsigned char b; |
||
453 | int err; |
||
454 | |||
455 | timer_expired = 0; |
||
456 | kern_gettime(¤t_time); |
||
457 | ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
||
458 | timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
||
281 | giacomo | 459 | |
283 | giacomo | 460 | b = 0x8E | (data & 0x01); |
289 | giacomo | 461 | err = com_send(servo_port, b); |
462 | err = com_receive(servo_port); |
||
283 | giacomo | 463 | if (err != (int)(b)) timer_expired = 1; |
464 | |||
465 | if (timeout_event != NIL) kern_event_delete(timeout_event); |
||
466 | |||
467 | if (!timer_expired) |
||
468 | return 0; |
||
469 | else |
||
470 | return -1; |
||
471 | |||
281 | giacomo | 472 | } |
473 | |||
285 | giacomo | 474 | /* 1000.0000:0000.0mmm */ |
475 | int servo_turn_off(int servo) |
||
476 | { |
||
477 | |||
478 | struct timespec current_time; |
||
479 | unsigned char b; |
||
480 | int err; |
||
481 | |||
482 | if (servo > 7) return -1; |
||
483 | |||
484 | timer_expired = 0; |
||
485 | kern_gettime(¤t_time); |
||
486 | ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
||
487 | timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
||
488 | |||
489 | b = 0x80; |
||
289 | giacomo | 490 | err = com_send(servo_port, b); |
491 | err = com_receive(servo_port); |
||
285 | giacomo | 492 | if (err != (int)(b)) timer_expired = 1; |
493 | |||
494 | b = 0x00 | (servo & 0x07); |
||
289 | giacomo | 495 | err = com_send(servo_port, b); |
496 | err = com_receive(servo_port); |
||
285 | giacomo | 497 | if (err != (int)(b)) timer_expired = 1; |
498 | |||
499 | if (timeout_event != NIL) kern_event_delete(timeout_event); |
||
500 | |||
501 | if (!timer_expired) |
||
502 | return 0; |
||
503 | else |
||
504 | return -1; |
||
505 | |||
506 | } |
||
507 | |||
508 | /* 1000.0000:1000.0mmm */ |
||
509 | int servo_turn_on(int servo) |
||
510 | { |
||
511 | |||
512 | struct timespec current_time; |
||
513 | unsigned char b; |
||
514 | int err; |
||
515 | |||
516 | if (servo > 7) return -1; |
||
517 | |||
518 | timer_expired = 0; |
||
519 | kern_gettime(¤t_time); |
||
520 | ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
||
521 | timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
||
522 | |||
523 | b = 0x80; |
||
289 | giacomo | 524 | err = com_send(servo_port, b); |
525 | err = com_receive(servo_port); |
||
285 | giacomo | 526 | if (err != (int)(b)) timer_expired = 1; |
527 | |||
528 | b = 0x08 | (servo & 0x07); |
||
289 | giacomo | 529 | err = com_send(servo_port, b); |
530 | err = com_receive(servo_port); |
||
285 | giacomo | 531 | if (err != (int)(b)) timer_expired = 1; |
532 | |||
533 | if (timeout_event != NIL) kern_event_delete(timeout_event); |
||
534 | |||
535 | if (!timer_expired) |
||
536 | return 0; |
||
537 | else |
||
538 | return -1; |
||
539 | |||
540 | } |
||
541 | |||
542 | /* 1000.0000:0001.0000 */ |
||
543 | int servo_turn_off_all(void) |
||
544 | { struct timespec current_time; |
||
545 | unsigned char b; |
||
546 | int err; |
||
547 | |||
548 | timer_expired = 0; |
||
549 | kern_gettime(¤t_time); |
||
550 | ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
||
551 | timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
||
552 | |||
553 | b = 0x80; |
||
289 | giacomo | 554 | err = com_send(servo_port, b); |
555 | err = com_receive(servo_port); |
||
285 | giacomo | 556 | if (err != (int)(b)) timer_expired = 1; |
557 | |||
558 | b = 0x10; |
||
289 | giacomo | 559 | err = com_send(servo_port, b); |
560 | err = com_receive(servo_port); |
||
285 | giacomo | 561 | if (err != (int)(b)) timer_expired = 1; |
562 | |||
563 | if (timeout_event != NIL) kern_event_delete(timeout_event); |
||
564 | |||
565 | if (!timer_expired) |
||
566 | return 0; |
||
567 | else |
||
568 | return -1; |
||
569 | |||
570 | } |
||
571 | |||
572 | /* 1000.0000:0001.0001 */ |
||
573 | int servo_turn_on_all(void) |
||
574 | { |
||
575 | struct timespec current_time; |
||
576 | unsigned char b; |
||
577 | int err; |
||
578 | |||
579 | timer_expired = 0; |
||
580 | kern_gettime(¤t_time); |
||
581 | ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
||
582 | timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
||
583 | |||
584 | b = 0x80; |
||
289 | giacomo | 585 | err = com_send(servo_port, b); |
586 | err = com_receive(servo_port); |
||
285 | giacomo | 587 | if (err != (int)(b)) timer_expired = 1; |
588 | |||
589 | b = 0x11; |
||
289 | giacomo | 590 | err = com_send(servo_port, b); |
591 | err = com_receive(servo_port); |
||
285 | giacomo | 592 | if (err != (int)(b)) timer_expired = 1; |
593 | |||
594 | if (timeout_event != NIL) kern_event_delete(timeout_event); |
||
595 | |||
596 | if (!timer_expired) |
||
597 | return 0; |
||
598 | else |
||
599 | return -1; |
||
600 | |||
601 | } |
||
602 | |||
282 | giacomo | 603 | /* 0000.0ppp:0000.vvvv:vvvv.vvvv */ |
281 | giacomo | 604 | int servo_set_angle(int servo, int angle) |
605 | { |
||
282 | giacomo | 606 | struct timespec current_time; |
607 | unsigned char b; |
||
283 | giacomo | 608 | int err; |
282 | giacomo | 609 | |
610 | if (servo > 7) return -1; |
||
611 | |||
612 | timer_expired = 0; |
||
613 | kern_gettime(¤t_time); |
||
614 | ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
||
615 | timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
||
616 | |||
617 | b = 0x00 | (servo & 0x07); |
||
289 | giacomo | 618 | err = com_send(servo_port, b); |
619 | err = com_receive(servo_port); |
||
283 | giacomo | 620 | if (err != (int)(b)) timer_expired = 1; |
282 | giacomo | 621 | |
622 | b = 0x00 | ((angle >> 8) & 0x0F); |
||
289 | giacomo | 623 | err = com_send(servo_port, b); |
624 | err = com_receive(servo_port); |
||
283 | giacomo | 625 | if (err != (int)(b)) timer_expired = 1; |
282 | giacomo | 626 | |
627 | b = 0x00 | (angle & 0xFF); |
||
289 | giacomo | 628 | err = com_send(servo_port, b); |
629 | err = com_receive(servo_port); |
||
283 | giacomo | 630 | if (err != (int)(b)) timer_expired = 1; |
282 | giacomo | 631 | |
283 | giacomo | 632 | if (timeout_event != NIL) kern_event_delete(timeout_event); |
282 | giacomo | 633 | |
283 | giacomo | 634 | if (!timer_expired) |
635 | return 0; |
||
636 | else |
||
637 | return -1; |
||
281 | giacomo | 638 | |
639 | } |
||
640 | |||
282 | giacomo | 641 | /* 0000.1ppp */ |
281 | giacomo | 642 | int servo_get_angle(int servo) |
643 | { |
||
282 | giacomo | 644 | struct timespec current_time; |
645 | unsigned char b; |
||
283 | giacomo | 646 | int err,res; |
282 | giacomo | 647 | |
648 | if (servo > 7) return -1; |
||
649 | |||
650 | timer_expired = 0; |
||
651 | kern_gettime(¤t_time); |
||
652 | ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
||
653 | timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
||
281 | giacomo | 654 | |
282 | giacomo | 655 | b = 0x08 | (servo & 0x07); |
289 | giacomo | 656 | err = com_send(servo_port, b); |
657 | err = com_receive(servo_port); |
||
283 | giacomo | 658 | if (err != (int)(b)) timer_expired = 1; |
289 | giacomo | 659 | res = com_receive(servo_port) << 8; |
660 | res |= com_receive(servo_port); |
||
282 | giacomo | 661 | |
283 | giacomo | 662 | if (timeout_event != NIL) kern_event_delete(timeout_event); |
282 | giacomo | 663 | |
664 | if (!timer_expired) |
||
665 | return res; |
||
666 | else |
||
667 | return -1; |
||
668 | |||
281 | giacomo | 669 | } |
670 | |||
282 | giacomo | 671 | /* 0100:0aaa */ |
281 | giacomo | 672 | int servo_get_analog(int port) |
673 | { |
||
674 | |||
282 | giacomo | 675 | struct timespec current_time; |
676 | unsigned char b; |
||
283 | giacomo | 677 | int err,res; |
282 | giacomo | 678 | |
679 | if (port > 4) return -1; |
||
680 | |||
681 | timer_expired = 0; |
||
682 | kern_gettime(¤t_time); |
||
683 | ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
||
684 | timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
||
281 | giacomo | 685 | |
282 | giacomo | 686 | b = 0x40 | (port & 7); |
289 | giacomo | 687 | err = com_send(servo_port, b); |
688 | err = com_receive(servo_port); |
||
283 | giacomo | 689 | if (err != (int)(b)) timer_expired = 1; |
289 | giacomo | 690 | res = com_receive(servo_port) << 8; |
691 | res |= com_receive(servo_port); |
||
282 | giacomo | 692 | |
283 | giacomo | 693 | if (timeout_event != NIL) kern_event_delete(timeout_event); |
282 | giacomo | 694 | |
695 | if (!timer_expired) |
||
696 | return res; |
||
697 | else |
||
698 | return -1; |
||
699 | |||
281 | giacomo | 700 | } |
701 |