Rev 315 | Rev 323 | 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 | |
315 | giacomo | 69 | #define TICK_LEN 1600 /* ns */ |
316 | giacomo | 70 | #define TICK_LEN_PERIOD 51200 /* ns */ |
290 | giacomo | 71 | |
72 | struct servo_data { |
||
73 | int min_angle_sec; |
||
74 | int max_angle_sec; |
||
75 | int delta_tick; |
||
76 | int zero_tick; |
||
77 | }; |
||
78 | |||
79 | struct servo_data servo_table[] = { |
||
316 | giacomo | 80 | {-324000, 324000, 1200, 1600}, |
81 | {-324000, 324000, 1200, 1600}, |
||
82 | {-324000, 324000, 1200, 1600}, |
||
83 | {-324000, 324000, 1200, 1600}, |
||
84 | {-324000, 324000, 1200, 1600}, |
||
85 | {-324000, 324000, 1200, 1600}, |
||
86 | {-324000, 324000, 1200, 1600}, |
||
87 | {-324000, 324000, 1200, 1600}}; |
||
290 | giacomo | 88 | |
281 | giacomo | 89 | int timer_expired = 0; |
283 | giacomo | 90 | int timeout_event; |
289 | giacomo | 91 | int servo_port; |
281 | giacomo | 92 | |
286 | giacomo | 93 | const unsigned com_base[] = {0x03F8,0x02F8,0x03E8,0x02E8}; |
94 | |||
95 | const int BaudTable[] = { |
||
96 | 1200, |
||
97 | 2400, |
||
98 | 4800, |
||
99 | 9600, |
||
100 | 14400, |
||
101 | 19200, |
||
102 | 38400, |
||
103 | 57600, |
||
104 | 115200, |
||
105 | -1}; |
||
106 | |||
107 | /* 20MHz: SPBRG, BAUD BRGH=0, BAUD BRGH=1 */ |
||
108 | const int Baud20MhzData[] = { |
||
109 | 255, 1221, 4882, |
||
110 | 254, 1225, 4902, |
||
111 | 253, 1230, 4921, |
||
112 | 252, 1235, 4941, |
||
113 | 133, 2332, 9328, |
||
114 | 132, 2350, 9398, |
||
115 | 131, 2367, 9470, |
||
116 | 130, 2385, 9542, |
||
117 | 129, 2404, 9615, |
||
118 | 128, 2422, 9690, |
||
119 | 127, 2441, 9766, |
||
120 | 126, 2461, 9842, |
||
121 | 88, 0, 14045, |
||
122 | 87, 0, 14204, |
||
123 | 86, 0, 14368, |
||
124 | 85, 0, 14535, |
||
125 | 84, 0, 14706, |
||
126 | 66, 4664, 18657, |
||
127 | 65, 4735, 18939, |
||
128 | 64, 4808, 19231, |
||
129 | 63, 4883, 19531, |
||
130 | 32, 9470, 37879, |
||
131 | 31, 9766, 39062, |
||
132 | 21, 14205, 56818, |
||
133 | 15, 19531, 0, |
||
134 | 10, 0, 113636, |
||
135 | 7, 39062, 0, |
||
136 | -1, -1, -1}; |
||
137 | |||
282 | giacomo | 138 | void set_timer_expired(void *arg) |
281 | giacomo | 139 | { |
283 | giacomo | 140 | timeout_event = NIL; |
281 | giacomo | 141 | timer_expired = 1; |
142 | } |
||
143 | |||
144 | unsigned com_read(unsigned port,unsigned reg) |
||
145 | { |
||
146 | unsigned b; |
||
147 | if (port > 3 || reg > 7) return(0); |
||
148 | b = ll_in(com_base[port]+reg); |
||
149 | return(b); |
||
150 | } |
||
151 | |||
152 | void com_write(unsigned port,unsigned reg,unsigned value) |
||
153 | { |
||
154 | if (port > 3 || reg > 7) return; |
||
155 | ll_out(com_base[port]+reg,value); |
||
156 | } |
||
157 | |||
158 | int com_send(unsigned port,BYTE b) |
||
159 | { |
||
160 | while ((com_read(port,LSR) & 32) == 0 && !timer_expired) |
||
161 | barrier(); |
||
162 | if (!timer_expired) { |
||
292 | giacomo | 163 | #ifdef SERVO_DEBUG |
295 | giacomo | 164 | kern_printf("(SERVO WRITE p = %d b = %02x)",port,b); |
292 | giacomo | 165 | #endif |
281 | giacomo | 166 | com_write(port,THR,b); |
167 | return 0; |
||
168 | } else { |
||
292 | giacomo | 169 | #ifdef SERVO_DEBUG |
295 | giacomo | 170 | kern_printf("(WRITE TIMEOUT)"); |
292 | giacomo | 171 | #endif |
281 | giacomo | 172 | return -1; |
173 | } |
||
174 | } |
||
175 | |||
176 | int com_receive(unsigned port) |
||
177 | { |
||
292 | giacomo | 178 | int b; |
179 | |||
281 | giacomo | 180 | while ((com_read(port,LSR) & 1) == 0 && !timer_expired) |
181 | barrier(); |
||
182 | if (!timer_expired) { |
||
292 | giacomo | 183 | b = (int)(com_read(port,RBR)); |
184 | #ifdef SERVO_DEBUG |
||
295 | giacomo | 185 | kern_printf("(SERVO READ p = %d b = %02x)",port,b); |
292 | giacomo | 186 | #endif |
187 | return b; |
||
281 | giacomo | 188 | } else { |
292 | giacomo | 189 | #ifdef SERVO_DEBUG |
295 | giacomo | 190 | kern_printf("(READ TIMEOUT)"); |
292 | giacomo | 191 | #endif |
281 | giacomo | 192 | return -1; |
193 | } |
||
194 | } |
||
195 | |||
286 | giacomo | 196 | int com_open(unsigned port,DWORD speed,BYTE parity,BYTE len,BYTE stop) |
197 | { |
||
198 | unsigned long div,b_mask; |
||
199 | |||
200 | /* Now set up the serial link */ |
||
201 | b_mask = (parity & 3) * 8 + (stop & 1) * 4 + ((len - 5) & 3); |
||
202 | div = 115200L / speed; |
||
203 | /* Clear serial interrupt enable register */ |
||
204 | com_write(port,IER,0); |
||
205 | /* Empty input buffer */ |
||
206 | com_read(port,RBR); |
||
207 | /* Activate DLAB bit for speed setting */ |
||
208 | com_write(port,LCR,0x80); |
||
209 | /* Load baud divisor */ |
||
210 | com_write(port,0,div & 0x00FF); |
||
211 | div >>= 8; |
||
212 | com_write(port,1,div & 0x00FF); |
||
213 | /* Load control word (parity,stop bit,bit len) */ |
||
214 | com_write(port,LCR,b_mask); |
||
215 | /* Attiva OUT1 & OUT2 */ |
||
216 | com_write(port,MCR,0x0C); |
||
217 | |||
218 | return 0; |
||
295 | giacomo | 219 | |
286 | giacomo | 220 | } |
221 | |||
222 | int com_close(unsigned port) |
||
223 | { |
||
224 | |||
225 | com_write(port,IER,0); |
||
226 | com_read(port,IIR); |
||
227 | com_read(port,RBR); |
||
228 | |||
229 | return 0; |
||
230 | |||
231 | } |
||
232 | |||
289 | giacomo | 233 | int servo_open(int port) |
286 | giacomo | 234 | { |
235 | int err; |
||
236 | |||
289 | giacomo | 237 | servo_port = (unsigned)(port); |
238 | err = com_open(servo_port, SERVO_SPEED, SERVO_PARITY, SERVO_LEN, SERVO_STOP); |
||
286 | giacomo | 239 | |
240 | return err; |
||
241 | |||
242 | } |
||
243 | |||
244 | int servo_close(void) |
||
245 | { |
||
246 | int err; |
||
247 | |||
289 | giacomo | 248 | err = com_close(servo_port); |
286 | giacomo | 249 | |
250 | return err; |
||
251 | |||
252 | } |
||
253 | |||
254 | /* 1000.011w:bbbb.bbbb */ |
||
281 | giacomo | 255 | int servo_set_RS232_baudrate(int baud) |
256 | { |
||
285 | giacomo | 257 | struct timespec current_time; |
258 | unsigned char b; |
||
286 | giacomo | 259 | int err,diff,mindiff,i; |
260 | unsigned char spbrg, w; |
||
261 | |||
262 | i = 0; |
||
263 | while(BaudTable[i] != baud && BaudTable[i] != -1) i++; |
||
264 | if (BaudTable[i] == -1) { |
||
265 | kern_printf("SERVO:Error wrong baud rate\n"); |
||
266 | return -1; |
||
267 | } |
||
268 | |||
269 | i = 0; |
||
270 | mindiff = 0xFFFFFF; |
||
271 | spbrg = 0; |
||
272 | w = 0; |
||
273 | while(Baud20MhzData[i] != -1) { |
||
274 | diff = abs(Baud20MhzData[i+1] - baud); |
||
275 | if (diff < mindiff) { |
||
276 | mindiff = diff; |
||
315 | giacomo | 277 | spbrg = (unsigned char)(Baud20MhzData[i]); |
286 | giacomo | 278 | w = 0; |
279 | } |
||
280 | diff = abs(Baud20MhzData[i+2] - baud); |
||
281 | if (diff < mindiff) { |
||
282 | mindiff = diff; |
||
315 | giacomo | 283 | spbrg = (unsigned char)(Baud20MhzData[i]); |
286 | giacomo | 284 | w = 1; |
285 | } |
||
286 | i += 3; |
||
287 | } |
||
288 | |||
289 | #ifdef SERVO_DEBUG |
||
290 | kern_printf("(SERVO:SBPRG %d W %d)",spbrg,w); |
||
291 | #endif |
||
292 | |||
285 | giacomo | 293 | timer_expired = 0; |
294 | kern_gettime(¤t_time); |
||
295 | ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
||
296 | timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
||
297 | |||
286 | giacomo | 298 | b = 0x86 | (w & 0x01); |
289 | giacomo | 299 | err = com_send(servo_port, b); |
300 | err = com_receive(servo_port); |
||
285 | giacomo | 301 | if (err != (int)(b)) timer_expired = 1; |
281 | giacomo | 302 | |
286 | giacomo | 303 | b = spbrg; |
289 | giacomo | 304 | err = com_send(servo_port, b); |
305 | err = com_receive(servo_port); |
||
285 | giacomo | 306 | if (err != (int)(b)) timer_expired = 1; |
281 | giacomo | 307 | |
285 | giacomo | 308 | if (timeout_event != NIL) kern_event_delete(timeout_event); |
286 | giacomo | 309 | |
289 | giacomo | 310 | com_close(servo_port); |
311 | com_open(servo_port, baud, SERVO_PARITY, SERVO_LEN, SERVO_STOP); |
||
286 | giacomo | 312 | |
285 | giacomo | 313 | if (!timer_expired) |
314 | return 0; |
||
315 | else |
||
316 | return -1; |
||
317 | |||
281 | giacomo | 318 | } |
319 | |||
285 | giacomo | 320 | /* 1000.0101 */ |
281 | giacomo | 321 | int servo_get_RS232_baudrate(void) |
322 | { |
||
285 | giacomo | 323 | struct timespec current_time; |
324 | unsigned char b; |
||
325 | int err,res; |
||
326 | |||
327 | timer_expired = 0; |
||
328 | kern_gettime(¤t_time); |
||
329 | ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
||
330 | timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
||
331 | |||
332 | b = 0x85; |
||
289 | giacomo | 333 | err = com_send(servo_port, b); |
334 | err = com_receive(servo_port); |
||
285 | giacomo | 335 | if (err != (int)(b)) timer_expired = 1; |
289 | giacomo | 336 | res = com_receive(servo_port); |
285 | giacomo | 337 | |
338 | if (timeout_event != NIL) kern_event_delete(timeout_event); |
||
339 | |||
340 | if (!timer_expired) |
||
341 | return res; |
||
342 | else |
||
343 | return -1; |
||
281 | giacomo | 344 | |
345 | } |
||
346 | |||
286 | giacomo | 347 | /* 1000.0100 */ |
281 | giacomo | 348 | int servo_store_RS232_buadrate(void) |
349 | { |
||
285 | giacomo | 350 | struct timespec current_time; |
351 | unsigned char b; |
||
352 | int err; |
||
353 | |||
354 | timer_expired = 0; |
||
355 | kern_gettime(¤t_time); |
||
356 | ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
||
357 | timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
||
358 | |||
286 | giacomo | 359 | b = 0x84; |
289 | giacomo | 360 | err = com_send(servo_port, b); |
361 | err = com_receive(servo_port); |
||
285 | giacomo | 362 | if (err != (int)(b)) timer_expired = 1; |
363 | |||
364 | if (timeout_event != NIL) kern_event_delete(timeout_event); |
||
365 | |||
366 | if (!timer_expired) |
||
367 | return 0; |
||
368 | else |
||
369 | return -1; |
||
281 | giacomo | 370 | |
371 | } |
||
372 | |||
289 | giacomo | 373 | /* 1000.1010:llll.llll */ |
281 | giacomo | 374 | int servo_set_period(int period) |
375 | { |
||
285 | giacomo | 376 | struct timespec current_time; |
377 | unsigned char b; |
||
378 | int err; |
||
379 | |||
380 | timer_expired = 0; |
||
381 | kern_gettime(¤t_time); |
||
382 | ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
||
383 | timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
||
384 | |||
289 | giacomo | 385 | b = 0x8A; |
386 | err = com_send(servo_port, b); |
||
387 | err = com_receive(servo_port); |
||
285 | giacomo | 388 | if (err != (int)(b)) timer_expired = 1; |
389 | |||
316 | giacomo | 390 | b = 0xFF - ((period*1000)/TICK_LEN_PERIOD/8 & 0xFF); |
289 | giacomo | 391 | err = com_send(servo_port, b); |
392 | err = com_receive(servo_port); |
||
285 | giacomo | 393 | if (err != (int)(b)) timer_expired = 1; |
394 | |||
395 | if (timeout_event != NIL) kern_event_delete(timeout_event); |
||
396 | |||
397 | if (!timer_expired) |
||
398 | return 0; |
||
399 | else |
||
400 | return -1; |
||
281 | giacomo | 401 | |
402 | } |
||
403 | |||
285 | giacomo | 404 | /* 1000.1001 */ |
281 | giacomo | 405 | int servo_get_period(void) |
406 | { |
||
285 | giacomo | 407 | struct timespec current_time; |
408 | unsigned char b; |
||
409 | int err,res; |
||
410 | |||
411 | timer_expired = 0; |
||
412 | kern_gettime(¤t_time); |
||
413 | ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
||
414 | timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
||
415 | |||
416 | b = 0x89; |
||
289 | giacomo | 417 | err = com_send(servo_port, b); |
418 | err = com_receive(servo_port); |
||
285 | giacomo | 419 | if (err != (int)(b)) timer_expired = 1; |
289 | giacomo | 420 | res = com_receive(servo_port); |
285 | giacomo | 421 | |
422 | if (timeout_event != NIL) kern_event_delete(timeout_event); |
||
423 | |||
424 | if (!timer_expired) |
||
316 | giacomo | 425 | return ((0xFF - (unsigned char)(res))*TICK_LEN_PERIOD/1000*8); |
285 | giacomo | 426 | else |
427 | return -1; |
||
281 | giacomo | 428 | |
429 | } |
||
430 | |||
289 | giacomo | 431 | /* 1000.1000 */ |
281 | giacomo | 432 | int servo_store_period(void) |
433 | { |
||
285 | giacomo | 434 | struct timespec current_time; |
435 | unsigned char b; |
||
436 | int err; |
||
437 | |||
438 | timer_expired = 0; |
||
439 | kern_gettime(¤t_time); |
||
440 | ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
||
441 | timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
||
442 | |||
289 | giacomo | 443 | b = 0x88; |
444 | err = com_send(servo_port, b); |
||
445 | err = com_receive(servo_port); |
||
285 | giacomo | 446 | if (err != (int)(b)) timer_expired = 1; |
447 | |||
448 | if (timeout_event != NIL) kern_event_delete(timeout_event); |
||
449 | |||
450 | if (!timer_expired) |
||
451 | return 0; |
||
452 | else |
||
453 | return -1; |
||
281 | giacomo | 454 | |
455 | } |
||
456 | |||
283 | giacomo | 457 | /* 1000.1100 */ |
281 | giacomo | 458 | int servo_get_setup_switch(void) |
459 | { |
||
283 | giacomo | 460 | struct timespec current_time; |
461 | unsigned char b; |
||
462 | int err,res; |
||
463 | timer_expired = 0; |
||
464 | kern_gettime(¤t_time); |
||
465 | ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
||
466 | timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
||
281 | giacomo | 467 | |
283 | giacomo | 468 | b = 0x8C; |
289 | giacomo | 469 | err = com_send(servo_port, b); |
470 | err = com_receive(servo_port); |
||
283 | giacomo | 471 | if (err != (int)(b)) timer_expired = 1; |
289 | giacomo | 472 | res = com_receive(servo_port); |
281 | giacomo | 473 | |
283 | giacomo | 474 | if (timeout_event != NIL) kern_event_delete(timeout_event); |
475 | |||
476 | if (!timer_expired) |
||
477 | return res; |
||
478 | else |
||
479 | return -1; |
||
480 | |||
281 | giacomo | 481 | } |
482 | |||
283 | giacomo | 483 | /* 1000.111s */ |
281 | giacomo | 484 | int servo_set_RC5_switch(int data) |
485 | { |
||
283 | giacomo | 486 | struct timespec current_time; |
487 | unsigned char b; |
||
488 | int err; |
||
489 | |||
490 | timer_expired = 0; |
||
491 | kern_gettime(¤t_time); |
||
492 | ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
||
493 | timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
||
281 | giacomo | 494 | |
283 | giacomo | 495 | b = 0x8E | (data & 0x01); |
289 | giacomo | 496 | err = com_send(servo_port, b); |
497 | err = com_receive(servo_port); |
||
283 | giacomo | 498 | if (err != (int)(b)) timer_expired = 1; |
499 | |||
500 | if (timeout_event != NIL) kern_event_delete(timeout_event); |
||
501 | |||
502 | if (!timer_expired) |
||
503 | return 0; |
||
504 | else |
||
505 | return -1; |
||
506 | |||
281 | giacomo | 507 | } |
508 | |||
285 | giacomo | 509 | /* 1000.0000:0000.0mmm */ |
510 | int servo_turn_off(int servo) |
||
511 | { |
||
512 | |||
513 | struct timespec current_time; |
||
514 | unsigned char b; |
||
515 | int err; |
||
516 | |||
517 | if (servo > 7) return -1; |
||
518 | |||
519 | timer_expired = 0; |
||
520 | kern_gettime(¤t_time); |
||
521 | ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
||
522 | timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
||
523 | |||
524 | b = 0x80; |
||
289 | giacomo | 525 | err = com_send(servo_port, b); |
526 | err = com_receive(servo_port); |
||
285 | giacomo | 527 | if (err != (int)(b)) timer_expired = 1; |
528 | |||
529 | b = 0x00 | (servo & 0x07); |
||
289 | giacomo | 530 | err = com_send(servo_port, b); |
531 | err = com_receive(servo_port); |
||
285 | giacomo | 532 | if (err != (int)(b)) timer_expired = 1; |
533 | |||
534 | if (timeout_event != NIL) kern_event_delete(timeout_event); |
||
535 | |||
536 | if (!timer_expired) |
||
537 | return 0; |
||
538 | else |
||
539 | return -1; |
||
540 | |||
541 | } |
||
542 | |||
291 | giacomo | 543 | /* 1000.0000:0001.0mmm */ |
285 | giacomo | 544 | int servo_turn_on(int servo) |
545 | { |
||
546 | |||
547 | struct timespec current_time; |
||
548 | unsigned char b; |
||
549 | int err; |
||
550 | |||
551 | if (servo > 7) return -1; |
||
552 | |||
553 | timer_expired = 0; |
||
554 | kern_gettime(¤t_time); |
||
555 | ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
||
556 | timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
||
557 | |||
558 | b = 0x80; |
||
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 | |||
291 | giacomo | 563 | b = 0x10 | (servo & 0x07); |
289 | giacomo | 564 | err = com_send(servo_port, b); |
565 | err = com_receive(servo_port); |
||
285 | giacomo | 566 | if (err != (int)(b)) timer_expired = 1; |
567 | |||
568 | if (timeout_event != NIL) kern_event_delete(timeout_event); |
||
569 | |||
570 | if (!timer_expired) |
||
571 | return 0; |
||
572 | else |
||
573 | return -1; |
||
574 | |||
575 | } |
||
576 | |||
291 | giacomo | 577 | /* 1000.0000:0010.0000 */ |
285 | giacomo | 578 | int servo_turn_off_all(void) |
579 | { struct timespec current_time; |
||
580 | unsigned char b; |
||
581 | int err; |
||
582 | |||
583 | timer_expired = 0; |
||
584 | kern_gettime(¤t_time); |
||
585 | ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
||
586 | timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
||
587 | |||
588 | b = 0x80; |
||
289 | giacomo | 589 | err = com_send(servo_port, b); |
590 | err = com_receive(servo_port); |
||
285 | giacomo | 591 | if (err != (int)(b)) timer_expired = 1; |
592 | |||
291 | giacomo | 593 | b = 0x20; |
289 | giacomo | 594 | err = com_send(servo_port, b); |
595 | err = com_receive(servo_port); |
||
285 | giacomo | 596 | if (err != (int)(b)) timer_expired = 1; |
597 | |||
598 | if (timeout_event != NIL) kern_event_delete(timeout_event); |
||
599 | |||
600 | if (!timer_expired) |
||
601 | return 0; |
||
602 | else |
||
603 | return -1; |
||
604 | |||
605 | } |
||
606 | |||
291 | giacomo | 607 | /* 1000.0000:0010.0001 */ |
285 | giacomo | 608 | int servo_turn_on_all(void) |
609 | { |
||
610 | struct timespec current_time; |
||
611 | unsigned char b; |
||
612 | int err; |
||
613 | |||
614 | timer_expired = 0; |
||
615 | kern_gettime(¤t_time); |
||
616 | ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
||
617 | timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
||
618 | |||
619 | b = 0x80; |
||
289 | giacomo | 620 | err = com_send(servo_port, b); |
621 | err = com_receive(servo_port); |
||
285 | giacomo | 622 | if (err != (int)(b)) timer_expired = 1; |
623 | |||
291 | giacomo | 624 | b = 0x21; |
289 | giacomo | 625 | err = com_send(servo_port, b); |
626 | err = com_receive(servo_port); |
||
285 | giacomo | 627 | if (err != (int)(b)) timer_expired = 1; |
628 | |||
629 | if (timeout_event != NIL) kern_event_delete(timeout_event); |
||
630 | |||
631 | if (!timer_expired) |
||
632 | return 0; |
||
633 | else |
||
634 | return -1; |
||
635 | |||
636 | } |
||
637 | |||
315 | giacomo | 638 | /* 1000.0000:0011.0000:mmmm.mmmm */ |
639 | int servo_set_levels(int mask) |
||
640 | { |
||
641 | struct timespec current_time; |
||
642 | unsigned char b; |
||
643 | int err; |
||
644 | |||
645 | timer_expired = 0; |
||
646 | kern_gettime(¤t_time); |
||
647 | ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
||
648 | timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
||
649 | |||
650 | b = 0x80; |
||
651 | err = com_send(servo_port, b); |
||
652 | err = com_receive(servo_port); |
||
653 | if (err != (int)(b)) timer_expired = 1; |
||
654 | |||
655 | b = 0x30; |
||
656 | err = com_send(servo_port, b); |
||
657 | err = com_receive(servo_port); |
||
658 | if (err != (int)(b)) timer_expired = 1; |
||
659 | |||
660 | b = (unsigned char)(mask & 0xFF); |
||
661 | err = com_send(servo_port, b); |
||
662 | err = com_receive(servo_port); |
||
663 | if (err != (int)(b)) timer_expired = 1; |
||
664 | |||
665 | if (timeout_event != NIL) kern_event_delete(timeout_event); |
||
666 | |||
667 | if (!timer_expired) |
||
668 | return 0; |
||
669 | else |
||
670 | return -1; |
||
671 | |||
672 | } |
||
673 | |||
297 | giacomo | 674 | /* 1000.0000:0100.0000 */ |
315 | giacomo | 675 | int servo_get_levels(void) |
297 | giacomo | 676 | { |
677 | struct timespec current_time; |
||
678 | unsigned char b; |
||
679 | int err; |
||
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); |
||
685 | |||
686 | b = 0x80; |
||
687 | err = com_send(servo_port, b); |
||
688 | err = com_receive(servo_port); |
||
689 | if (err != (int)(b)) timer_expired = 1; |
||
690 | |||
691 | b = 0x40; |
||
692 | err = com_send(servo_port, b); |
||
693 | err = com_receive(servo_port); |
||
694 | if (err != (int)(b)) timer_expired = 1; |
||
695 | err = com_receive(servo_port); |
||
696 | |||
697 | if (timeout_event != NIL) kern_event_delete(timeout_event); |
||
698 | |||
699 | if (!timer_expired) |
||
316 | giacomo | 700 | return err; |
297 | giacomo | 701 | else |
702 | return -1; |
||
703 | |||
704 | } |
||
705 | |||
706 | /* 1000.0000:1000.0000 */ |
||
707 | int servo_store_levels(void) |
||
708 | { |
||
709 | struct timespec current_time; |
||
710 | unsigned char b; |
||
711 | int err; |
||
712 | |||
713 | timer_expired = 0; |
||
714 | kern_gettime(¤t_time); |
||
715 | ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
||
716 | timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
||
717 | |||
718 | b = 0x80; |
||
719 | err = com_send(servo_port, b); |
||
720 | err = com_receive(servo_port); |
||
721 | if (err != (int)(b)) timer_expired = 1; |
||
722 | |||
723 | b = 0x80; |
||
724 | err = com_send(servo_port, b); |
||
725 | err = com_receive(servo_port); |
||
726 | if (err != (int)(b)) timer_expired = 1; |
||
727 | |||
728 | if (timeout_event != NIL) kern_event_delete(timeout_event); |
||
729 | |||
730 | if (!timer_expired) |
||
731 | return 0; |
||
732 | else |
||
733 | return -1; |
||
734 | |||
735 | } |
||
736 | |||
290 | giacomo | 737 | int servo_set_max_angle(int servo, int angle_sec) |
738 | { |
||
739 | |||
740 | servo_table[servo].max_angle_sec = angle_sec; |
||
741 | return 0; |
||
742 | |||
743 | } |
||
744 | |||
745 | int servo_set_min_angle(int servo, int angle_sec) |
||
746 | { |
||
747 | |||
748 | servo_table[servo].min_angle_sec = angle_sec; |
||
749 | return 0; |
||
750 | |||
751 | } |
||
752 | |||
282 | giacomo | 753 | /* 0000.0ppp:0000.vvvv:vvvv.vvvv */ |
290 | giacomo | 754 | int servo_set_angle_sec(int servo, int angle_sec) |
281 | giacomo | 755 | { |
282 | giacomo | 756 | struct timespec current_time; |
757 | unsigned char b; |
||
290 | giacomo | 758 | int err, angle_tick; |
282 | giacomo | 759 | |
760 | if (servo > 7) return -1; |
||
761 | |||
290 | giacomo | 762 | if (angle_sec > servo_table[servo].max_angle_sec || |
763 | angle_sec < servo_table[servo].min_angle_sec) return -1; |
||
764 | |||
316 | giacomo | 765 | angle_tick = (servo_table[servo].zero_tick + angle_sec * servo_table[servo].delta_tick / |
766 | (servo_table[servo].max_angle_sec - servo_table[servo].min_angle_sec)) * 1000 / TICK_LEN; |
||
290 | giacomo | 767 | |
282 | giacomo | 768 | timer_expired = 0; |
769 | kern_gettime(¤t_time); |
||
770 | ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
||
771 | timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
||
772 | |||
773 | b = 0x00 | (servo & 0x07); |
||
289 | giacomo | 774 | err = com_send(servo_port, b); |
775 | err = com_receive(servo_port); |
||
283 | giacomo | 776 | if (err != (int)(b)) timer_expired = 1; |
282 | giacomo | 777 | |
290 | giacomo | 778 | b = 0x00 | ((angle_tick >> 8) & 0x0F); |
289 | giacomo | 779 | err = com_send(servo_port, b); |
780 | err = com_receive(servo_port); |
||
283 | giacomo | 781 | if (err != (int)(b)) timer_expired = 1; |
282 | giacomo | 782 | |
290 | giacomo | 783 | b = 0x00 | (angle_tick & 0xFF); |
289 | giacomo | 784 | err = com_send(servo_port, b); |
785 | err = com_receive(servo_port); |
||
283 | giacomo | 786 | if (err != (int)(b)) timer_expired = 1; |
282 | giacomo | 787 | |
283 | giacomo | 788 | if (timeout_event != NIL) kern_event_delete(timeout_event); |
282 | giacomo | 789 | |
283 | giacomo | 790 | if (!timer_expired) |
791 | return 0; |
||
792 | else |
||
793 | return -1; |
||
281 | giacomo | 794 | |
795 | } |
||
796 | |||
291 | giacomo | 797 | /* 0010.0ppp */ |
290 | giacomo | 798 | int servo_store_default_position(int servo) |
799 | { |
||
800 | struct timespec current_time; |
||
801 | unsigned char b; |
||
802 | int err; |
||
803 | |||
804 | if (servo > 7) return -1; |
||
805 | |||
806 | timer_expired = 0; |
||
807 | kern_gettime(¤t_time); |
||
808 | ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
||
809 | timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
||
810 | |||
291 | giacomo | 811 | b = 0x20 | (servo & 0x07); |
290 | giacomo | 812 | err = com_send(servo_port, b); |
813 | err = com_receive(servo_port); |
||
814 | if (err != (int)(b)) timer_expired = 1; |
||
815 | |||
816 | if (timeout_event != NIL) kern_event_delete(timeout_event); |
||
817 | |||
818 | if (!timer_expired) |
||
819 | return 0; |
||
820 | else |
||
821 | return -1; |
||
822 | |||
823 | } |
||
824 | |||
291 | giacomo | 825 | /* 0001.0ppp */ |
290 | giacomo | 826 | int servo_get_angle_sec(int servo) |
281 | giacomo | 827 | { |
282 | giacomo | 828 | struct timespec current_time; |
829 | unsigned char b; |
||
290 | giacomo | 830 | int err,res,data; |
282 | giacomo | 831 | |
832 | if (servo > 7) return -1; |
||
833 | |||
834 | timer_expired = 0; |
||
835 | kern_gettime(¤t_time); |
||
836 | ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
||
837 | timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
||
281 | giacomo | 838 | |
291 | giacomo | 839 | b = 0x10 | (servo & 0x07); |
289 | giacomo | 840 | err = com_send(servo_port, b); |
841 | err = com_receive(servo_port); |
||
283 | giacomo | 842 | if (err != (int)(b)) timer_expired = 1; |
289 | giacomo | 843 | res = com_receive(servo_port) << 8; |
844 | res |= com_receive(servo_port); |
||
282 | giacomo | 845 | |
283 | giacomo | 846 | if (timeout_event != NIL) kern_event_delete(timeout_event); |
282 | giacomo | 847 | |
316 | giacomo | 848 | data = ((res*TICK_LEN/1000) - servo_table[servo].zero_tick) * |
290 | giacomo | 849 | (servo_table[servo].max_angle_sec - servo_table[servo].min_angle_sec) / |
850 | servo_table[servo].delta_tick; |
||
851 | |||
282 | giacomo | 852 | if (!timer_expired) |
290 | giacomo | 853 | return data; |
282 | giacomo | 854 | else |
855 | return -1; |
||
856 | |||
281 | giacomo | 857 | } |
858 | |||
282 | giacomo | 859 | /* 0100:0aaa */ |
281 | giacomo | 860 | int servo_get_analog(int port) |
861 | { |
||
862 | |||
282 | giacomo | 863 | struct timespec current_time; |
864 | unsigned char b; |
||
283 | giacomo | 865 | int err,res; |
282 | giacomo | 866 | |
867 | if (port > 4) return -1; |
||
868 | |||
869 | timer_expired = 0; |
||
870 | kern_gettime(¤t_time); |
||
871 | ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
||
872 | timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
||
281 | giacomo | 873 | |
282 | giacomo | 874 | b = 0x40 | (port & 7); |
289 | giacomo | 875 | err = com_send(servo_port, b); |
876 | err = com_receive(servo_port); |
||
283 | giacomo | 877 | if (err != (int)(b)) timer_expired = 1; |
289 | giacomo | 878 | res = com_receive(servo_port) << 8; |
879 | res |= com_receive(servo_port); |
||
282 | giacomo | 880 | |
283 | giacomo | 881 | if (timeout_event != NIL) kern_event_delete(timeout_event); |
282 | giacomo | 882 | |
883 | if (!timer_expired) |
||
884 | return res; |
||
885 | else |
||
886 | return -1; |
||
887 | |||
281 | giacomo | 888 | } |
889 |