Rev 289 | Rev 291 | 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 | |
290 | giacomo | 69 | #define TICK_LEN 1600 /* us */ |
70 | |||
71 | struct servo_data { |
||
72 | int min_angle_sec; |
||
73 | int max_angle_sec; |
||
74 | int delta_tick; |
||
75 | int zero_tick; |
||
76 | }; |
||
77 | |||
78 | struct servo_data servo_table[] = { |
||
79 | {-324000, 324000, 1800, 1600}, |
||
80 | {-324000, 324000, 1800, 1600}, |
||
81 | {-324000, 324000, 1800, 1600}, |
||
82 | {-324000, 324000, 1800, 1600}, |
||
83 | {-324000, 324000, 1800, 1600}, |
||
84 | {-324000, 324000, 1800, 1600}, |
||
85 | {-324000, 324000, 1800, 1600}, |
||
86 | {-324000, 324000, 1800, 1600}}; |
||
87 | |||
281 | giacomo | 88 | int timer_expired = 0; |
283 | giacomo | 89 | int timeout_event; |
289 | giacomo | 90 | int servo_port; |
281 | giacomo | 91 | |
286 | giacomo | 92 | const unsigned com_base[] = {0x03F8,0x02F8,0x03E8,0x02E8}; |
93 | |||
94 | const int BaudTable[] = { |
||
95 | 1200, |
||
96 | 2400, |
||
97 | 4800, |
||
98 | 9600, |
||
99 | 14400, |
||
100 | 19200, |
||
101 | 38400, |
||
102 | 57600, |
||
103 | 115200, |
||
104 | 230400, |
||
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) { |
||
163 | com_write(port,THR,b); |
||
164 | return 0; |
||
165 | } else { |
||
166 | return -1; |
||
167 | } |
||
168 | } |
||
169 | |||
170 | int com_receive(unsigned port) |
||
171 | { |
||
172 | while ((com_read(port,LSR) & 1) == 0 && !timer_expired) |
||
173 | barrier(); |
||
174 | if (!timer_expired) { |
||
175 | return((int)(com_read(port,RBR))); |
||
176 | } else { |
||
177 | return -1; |
||
178 | } |
||
179 | } |
||
180 | |||
286 | giacomo | 181 | int com_open(unsigned port,DWORD speed,BYTE parity,BYTE len,BYTE stop) |
182 | { |
||
183 | unsigned long div,b_mask; |
||
184 | |||
185 | /* Now set up the serial link */ |
||
186 | b_mask = (parity & 3) * 8 + (stop & 1) * 4 + ((len - 5) & 3); |
||
187 | div = 115200L / speed; |
||
188 | /* Clear serial interrupt enable register */ |
||
189 | com_write(port,IER,0); |
||
190 | /* Empty input buffer */ |
||
191 | com_read(port,RBR); |
||
192 | /* Activate DLAB bit for speed setting */ |
||
193 | com_write(port,LCR,0x80); |
||
194 | /* Load baud divisor */ |
||
195 | com_write(port,0,div & 0x00FF); |
||
196 | div >>= 8; |
||
197 | com_write(port,1,div & 0x00FF); |
||
198 | /* Load control word (parity,stop bit,bit len) */ |
||
199 | com_write(port,LCR,b_mask); |
||
200 | /* Attiva OUT1 & OUT2 */ |
||
201 | com_write(port,MCR,0x0C); |
||
202 | |||
203 | return 0; |
||
204 | } |
||
205 | |||
206 | int com_close(unsigned port) |
||
207 | { |
||
208 | |||
209 | com_write(port,IER,0); |
||
210 | com_read(port,IIR); |
||
211 | com_read(port,RBR); |
||
212 | |||
213 | return 0; |
||
214 | |||
215 | } |
||
216 | |||
289 | giacomo | 217 | int servo_open(int port) |
286 | giacomo | 218 | { |
219 | int err; |
||
220 | |||
289 | giacomo | 221 | servo_port = (unsigned)(port); |
222 | err = com_open(servo_port, SERVO_SPEED, SERVO_PARITY, SERVO_LEN, SERVO_STOP); |
||
286 | giacomo | 223 | |
224 | return err; |
||
225 | |||
226 | } |
||
227 | |||
228 | int servo_close(void) |
||
229 | { |
||
230 | int err; |
||
231 | |||
289 | giacomo | 232 | err = com_close(servo_port); |
286 | giacomo | 233 | |
234 | return err; |
||
235 | |||
236 | } |
||
237 | |||
238 | /* 1000.011w:bbbb.bbbb */ |
||
281 | giacomo | 239 | int servo_set_RS232_baudrate(int baud) |
240 | { |
||
285 | giacomo | 241 | struct timespec current_time; |
242 | unsigned char b; |
||
286 | giacomo | 243 | int err,diff,mindiff,i; |
244 | unsigned char spbrg, w; |
||
245 | |||
246 | i = 0; |
||
247 | while(BaudTable[i] != baud && BaudTable[i] != -1) i++; |
||
248 | if (BaudTable[i] == -1) { |
||
249 | kern_printf("SERVO:Error wrong baud rate\n"); |
||
250 | return -1; |
||
251 | } |
||
252 | |||
253 | i = 0; |
||
254 | mindiff = 0xFFFFFF; |
||
255 | spbrg = 0; |
||
256 | w = 0; |
||
257 | while(Baud20MhzData[i] != -1) { |
||
258 | diff = abs(Baud20MhzData[i+1] - baud); |
||
259 | if (diff < mindiff) { |
||
260 | mindiff = diff; |
||
261 | spbrg = (unsigned char)(Baud20MhzData[i+1]); |
||
262 | w = 0; |
||
263 | } |
||
264 | diff = abs(Baud20MhzData[i+2] - baud); |
||
265 | if (diff < mindiff) { |
||
266 | mindiff = diff; |
||
267 | spbrg = (unsigned char)(Baud20MhzData[i+2]); |
||
268 | w = 1; |
||
269 | } |
||
270 | i += 3; |
||
271 | } |
||
272 | |||
273 | #ifdef SERVO_DEBUG |
||
274 | kern_printf("(SERVO:SBPRG %d W %d)",spbrg,w); |
||
275 | #endif |
||
276 | |||
285 | giacomo | 277 | timer_expired = 0; |
278 | kern_gettime(¤t_time); |
||
279 | ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
||
280 | timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
||
281 | |||
286 | giacomo | 282 | b = 0x86 | (w & 0x01); |
289 | giacomo | 283 | err = com_send(servo_port, b); |
284 | err = com_receive(servo_port); |
||
285 | giacomo | 285 | if (err != (int)(b)) timer_expired = 1; |
281 | giacomo | 286 | |
286 | giacomo | 287 | b = spbrg; |
289 | giacomo | 288 | err = com_send(servo_port, b); |
289 | err = com_receive(servo_port); |
||
285 | giacomo | 290 | if (err != (int)(b)) timer_expired = 1; |
281 | giacomo | 291 | |
285 | giacomo | 292 | if (timeout_event != NIL) kern_event_delete(timeout_event); |
286 | giacomo | 293 | |
289 | giacomo | 294 | com_close(servo_port); |
295 | com_open(servo_port, baud, SERVO_PARITY, SERVO_LEN, SERVO_STOP); |
||
286 | giacomo | 296 | |
285 | giacomo | 297 | if (!timer_expired) |
298 | return 0; |
||
299 | else |
||
300 | return -1; |
||
301 | |||
281 | giacomo | 302 | } |
303 | |||
285 | giacomo | 304 | /* 1000.0101 */ |
281 | giacomo | 305 | int servo_get_RS232_baudrate(void) |
306 | { |
||
285 | giacomo | 307 | struct timespec current_time; |
308 | unsigned char b; |
||
309 | int err,res; |
||
310 | |||
311 | timer_expired = 0; |
||
312 | kern_gettime(¤t_time); |
||
313 | ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
||
314 | timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
||
315 | |||
316 | b = 0x85; |
||
289 | giacomo | 317 | err = com_send(servo_port, b); |
318 | err = com_receive(servo_port); |
||
285 | giacomo | 319 | if (err != (int)(b)) timer_expired = 1; |
289 | giacomo | 320 | res = com_receive(servo_port); |
285 | giacomo | 321 | |
322 | if (timeout_event != NIL) kern_event_delete(timeout_event); |
||
323 | |||
324 | if (!timer_expired) |
||
325 | return res; |
||
326 | else |
||
327 | return -1; |
||
281 | giacomo | 328 | |
329 | } |
||
330 | |||
286 | giacomo | 331 | /* 1000.0100 */ |
281 | giacomo | 332 | int servo_store_RS232_buadrate(void) |
333 | { |
||
285 | giacomo | 334 | struct timespec current_time; |
335 | unsigned char b; |
||
336 | int err; |
||
337 | |||
338 | timer_expired = 0; |
||
339 | kern_gettime(¤t_time); |
||
340 | ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
||
341 | timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
||
342 | |||
286 | giacomo | 343 | b = 0x84; |
289 | giacomo | 344 | err = com_send(servo_port, b); |
345 | err = com_receive(servo_port); |
||
285 | giacomo | 346 | if (err != (int)(b)) timer_expired = 1; |
347 | |||
348 | if (timeout_event != NIL) kern_event_delete(timeout_event); |
||
349 | |||
350 | if (!timer_expired) |
||
351 | return 0; |
||
352 | else |
||
353 | return -1; |
||
281 | giacomo | 354 | |
355 | } |
||
356 | |||
289 | giacomo | 357 | /* 1000.1010:llll.llll */ |
281 | giacomo | 358 | int servo_set_period(int period) |
359 | { |
||
285 | giacomo | 360 | struct timespec current_time; |
361 | unsigned char b; |
||
362 | int err; |
||
363 | |||
364 | timer_expired = 0; |
||
365 | kern_gettime(¤t_time); |
||
366 | ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
||
367 | timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
||
368 | |||
289 | giacomo | 369 | b = 0x8A; |
370 | err = com_send(servo_port, b); |
||
371 | err = com_receive(servo_port); |
||
285 | giacomo | 372 | if (err != (int)(b)) timer_expired = 1; |
373 | |||
290 | giacomo | 374 | b = period/TICK_LEN/8 & 0xFF; |
289 | giacomo | 375 | err = com_send(servo_port, b); |
376 | err = com_receive(servo_port); |
||
285 | giacomo | 377 | if (err != (int)(b)) timer_expired = 1; |
378 | |||
379 | if (timeout_event != NIL) kern_event_delete(timeout_event); |
||
380 | |||
381 | if (!timer_expired) |
||
382 | return 0; |
||
383 | else |
||
384 | return -1; |
||
281 | giacomo | 385 | |
386 | } |
||
387 | |||
285 | giacomo | 388 | /* 1000.1001 */ |
281 | giacomo | 389 | int servo_get_period(void) |
390 | { |
||
285 | giacomo | 391 | struct timespec current_time; |
392 | unsigned char b; |
||
393 | int err,res; |
||
394 | |||
395 | timer_expired = 0; |
||
396 | kern_gettime(¤t_time); |
||
397 | ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
||
398 | timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
||
399 | |||
400 | b = 0x89; |
||
289 | giacomo | 401 | err = com_send(servo_port, b); |
402 | err = com_receive(servo_port); |
||
285 | giacomo | 403 | if (err != (int)(b)) timer_expired = 1; |
289 | giacomo | 404 | res = com_receive(servo_port); |
285 | giacomo | 405 | |
406 | if (timeout_event != NIL) kern_event_delete(timeout_event); |
||
407 | |||
408 | if (!timer_expired) |
||
290 | giacomo | 409 | return (res*TICK_LEN*8); |
285 | giacomo | 410 | else |
411 | return -1; |
||
281 | giacomo | 412 | |
413 | } |
||
414 | |||
289 | giacomo | 415 | /* 1000.1000 */ |
281 | giacomo | 416 | int servo_store_period(void) |
417 | { |
||
285 | giacomo | 418 | struct timespec current_time; |
419 | unsigned char b; |
||
420 | int err; |
||
421 | |||
422 | timer_expired = 0; |
||
423 | kern_gettime(¤t_time); |
||
424 | ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
||
425 | timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
||
426 | |||
289 | giacomo | 427 | b = 0x88; |
428 | err = com_send(servo_port, b); |
||
429 | err = com_receive(servo_port); |
||
285 | giacomo | 430 | if (err != (int)(b)) timer_expired = 1; |
431 | |||
432 | if (timeout_event != NIL) kern_event_delete(timeout_event); |
||
433 | |||
434 | if (!timer_expired) |
||
435 | return 0; |
||
436 | else |
||
437 | return -1; |
||
281 | giacomo | 438 | |
439 | } |
||
440 | |||
283 | giacomo | 441 | /* 1000.1100 */ |
281 | giacomo | 442 | int servo_get_setup_switch(void) |
443 | { |
||
283 | giacomo | 444 | struct timespec current_time; |
445 | unsigned char b; |
||
446 | int err,res; |
||
447 | timer_expired = 0; |
||
448 | kern_gettime(¤t_time); |
||
449 | ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
||
450 | timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
||
281 | giacomo | 451 | |
283 | giacomo | 452 | b = 0x8C; |
289 | giacomo | 453 | err = com_send(servo_port, b); |
454 | err = com_receive(servo_port); |
||
283 | giacomo | 455 | if (err != (int)(b)) timer_expired = 1; |
289 | giacomo | 456 | res = com_receive(servo_port); |
281 | giacomo | 457 | |
283 | giacomo | 458 | if (timeout_event != NIL) kern_event_delete(timeout_event); |
459 | |||
460 | if (!timer_expired) |
||
461 | return res; |
||
462 | else |
||
463 | return -1; |
||
464 | |||
281 | giacomo | 465 | } |
466 | |||
283 | giacomo | 467 | /* 1000.111s */ |
281 | giacomo | 468 | int servo_set_RC5_switch(int data) |
469 | { |
||
283 | giacomo | 470 | struct timespec current_time; |
471 | unsigned char b; |
||
472 | int err; |
||
473 | |||
474 | timer_expired = 0; |
||
475 | kern_gettime(¤t_time); |
||
476 | ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
||
477 | timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
||
281 | giacomo | 478 | |
283 | giacomo | 479 | b = 0x8E | (data & 0x01); |
289 | giacomo | 480 | err = com_send(servo_port, b); |
481 | err = com_receive(servo_port); |
||
283 | giacomo | 482 | if (err != (int)(b)) timer_expired = 1; |
483 | |||
484 | if (timeout_event != NIL) kern_event_delete(timeout_event); |
||
485 | |||
486 | if (!timer_expired) |
||
487 | return 0; |
||
488 | else |
||
489 | return -1; |
||
490 | |||
281 | giacomo | 491 | } |
492 | |||
285 | giacomo | 493 | /* 1000.0000:0000.0mmm */ |
494 | int servo_turn_off(int servo) |
||
495 | { |
||
496 | |||
497 | struct timespec current_time; |
||
498 | unsigned char b; |
||
499 | int err; |
||
500 | |||
501 | if (servo > 7) return -1; |
||
502 | |||
503 | timer_expired = 0; |
||
504 | kern_gettime(¤t_time); |
||
505 | ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
||
506 | timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
||
507 | |||
508 | b = 0x80; |
||
289 | giacomo | 509 | err = com_send(servo_port, b); |
510 | err = com_receive(servo_port); |
||
285 | giacomo | 511 | if (err != (int)(b)) timer_expired = 1; |
512 | |||
513 | b = 0x00 | (servo & 0x07); |
||
289 | giacomo | 514 | err = com_send(servo_port, b); |
515 | err = com_receive(servo_port); |
||
285 | giacomo | 516 | if (err != (int)(b)) timer_expired = 1; |
517 | |||
518 | if (timeout_event != NIL) kern_event_delete(timeout_event); |
||
519 | |||
520 | if (!timer_expired) |
||
521 | return 0; |
||
522 | else |
||
523 | return -1; |
||
524 | |||
525 | } |
||
526 | |||
527 | /* 1000.0000:1000.0mmm */ |
||
528 | int servo_turn_on(int servo) |
||
529 | { |
||
530 | |||
531 | struct timespec current_time; |
||
532 | unsigned char b; |
||
533 | int err; |
||
534 | |||
535 | if (servo > 7) return -1; |
||
536 | |||
537 | timer_expired = 0; |
||
538 | kern_gettime(¤t_time); |
||
539 | ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
||
540 | timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
||
541 | |||
542 | b = 0x80; |
||
289 | giacomo | 543 | err = com_send(servo_port, b); |
544 | err = com_receive(servo_port); |
||
285 | giacomo | 545 | if (err != (int)(b)) timer_expired = 1; |
546 | |||
547 | b = 0x08 | (servo & 0x07); |
||
289 | giacomo | 548 | err = com_send(servo_port, b); |
549 | err = com_receive(servo_port); |
||
285 | giacomo | 550 | if (err != (int)(b)) timer_expired = 1; |
551 | |||
552 | if (timeout_event != NIL) kern_event_delete(timeout_event); |
||
553 | |||
554 | if (!timer_expired) |
||
555 | return 0; |
||
556 | else |
||
557 | return -1; |
||
558 | |||
559 | } |
||
560 | |||
561 | /* 1000.0000:0001.0000 */ |
||
562 | int servo_turn_off_all(void) |
||
563 | { struct timespec current_time; |
||
564 | unsigned char b; |
||
565 | int err; |
||
566 | |||
567 | timer_expired = 0; |
||
568 | kern_gettime(¤t_time); |
||
569 | ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
||
570 | timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
||
571 | |||
572 | b = 0x80; |
||
289 | giacomo | 573 | err = com_send(servo_port, b); |
574 | err = com_receive(servo_port); |
||
285 | giacomo | 575 | if (err != (int)(b)) timer_expired = 1; |
576 | |||
577 | b = 0x10; |
||
289 | giacomo | 578 | err = com_send(servo_port, b); |
579 | err = com_receive(servo_port); |
||
285 | giacomo | 580 | if (err != (int)(b)) timer_expired = 1; |
581 | |||
582 | if (timeout_event != NIL) kern_event_delete(timeout_event); |
||
583 | |||
584 | if (!timer_expired) |
||
585 | return 0; |
||
586 | else |
||
587 | return -1; |
||
588 | |||
589 | } |
||
590 | |||
591 | /* 1000.0000:0001.0001 */ |
||
592 | int servo_turn_on_all(void) |
||
593 | { |
||
594 | struct timespec current_time; |
||
595 | unsigned char b; |
||
596 | int err; |
||
597 | |||
598 | timer_expired = 0; |
||
599 | kern_gettime(¤t_time); |
||
600 | ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
||
601 | timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
||
602 | |||
603 | b = 0x80; |
||
289 | giacomo | 604 | err = com_send(servo_port, b); |
605 | err = com_receive(servo_port); |
||
285 | giacomo | 606 | if (err != (int)(b)) timer_expired = 1; |
607 | |||
608 | b = 0x11; |
||
289 | giacomo | 609 | err = com_send(servo_port, b); |
610 | err = com_receive(servo_port); |
||
285 | giacomo | 611 | if (err != (int)(b)) timer_expired = 1; |
612 | |||
613 | if (timeout_event != NIL) kern_event_delete(timeout_event); |
||
614 | |||
615 | if (!timer_expired) |
||
616 | return 0; |
||
617 | else |
||
618 | return -1; |
||
619 | |||
620 | } |
||
621 | |||
290 | giacomo | 622 | int servo_set_max_angle(int servo, int angle_sec) |
623 | { |
||
624 | |||
625 | servo_table[servo].max_angle_sec = angle_sec; |
||
626 | return 0; |
||
627 | |||
628 | } |
||
629 | |||
630 | int servo_set_min_angle(int servo, int angle_sec) |
||
631 | { |
||
632 | |||
633 | servo_table[servo].min_angle_sec = angle_sec; |
||
634 | return 0; |
||
635 | |||
636 | } |
||
637 | |||
282 | giacomo | 638 | /* 0000.0ppp:0000.vvvv:vvvv.vvvv */ |
290 | giacomo | 639 | int servo_set_angle_sec(int servo, int angle_sec) |
281 | giacomo | 640 | { |
282 | giacomo | 641 | struct timespec current_time; |
642 | unsigned char b; |
||
290 | giacomo | 643 | int err, angle_tick; |
282 | giacomo | 644 | |
645 | if (servo > 7) return -1; |
||
646 | |||
290 | giacomo | 647 | if (angle_sec > servo_table[servo].max_angle_sec || |
648 | angle_sec < servo_table[servo].min_angle_sec) return -1; |
||
649 | |||
650 | angle_tick = servo_table[servo].zero_tick + angle_sec * servo_table[servo].delta_tick / |
||
651 | (servo_table[servo].max_angle_sec - servo_table[servo].min_angle_sec); |
||
652 | |||
282 | giacomo | 653 | timer_expired = 0; |
654 | kern_gettime(¤t_time); |
||
655 | ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
||
656 | timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
||
657 | |||
658 | b = 0x00 | (servo & 0x07); |
||
289 | giacomo | 659 | err = com_send(servo_port, b); |
660 | err = com_receive(servo_port); |
||
283 | giacomo | 661 | if (err != (int)(b)) timer_expired = 1; |
282 | giacomo | 662 | |
290 | giacomo | 663 | b = 0x00 | ((angle_tick >> 8) & 0x0F); |
289 | giacomo | 664 | err = com_send(servo_port, b); |
665 | err = com_receive(servo_port); |
||
283 | giacomo | 666 | if (err != (int)(b)) timer_expired = 1; |
282 | giacomo | 667 | |
290 | giacomo | 668 | b = 0x00 | (angle_tick & 0xFF); |
289 | giacomo | 669 | err = com_send(servo_port, b); |
670 | err = com_receive(servo_port); |
||
283 | giacomo | 671 | if (err != (int)(b)) timer_expired = 1; |
282 | giacomo | 672 | |
283 | giacomo | 673 | if (timeout_event != NIL) kern_event_delete(timeout_event); |
282 | giacomo | 674 | |
283 | giacomo | 675 | if (!timer_expired) |
676 | return 0; |
||
677 | else |
||
678 | return -1; |
||
281 | giacomo | 679 | |
680 | } |
||
681 | |||
290 | giacomo | 682 | /* 0001.0ppp */ |
683 | int servo_store_default_position(int servo) |
||
684 | { |
||
685 | struct timespec current_time; |
||
686 | unsigned char b; |
||
687 | int err; |
||
688 | |||
689 | if (servo > 7) return -1; |
||
690 | |||
691 | timer_expired = 0; |
||
692 | kern_gettime(¤t_time); |
||
693 | ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
||
694 | timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
||
695 | |||
696 | b = 0x10 | (servo & 0x07); |
||
697 | err = com_send(servo_port, b); |
||
698 | err = com_receive(servo_port); |
||
699 | if (err != (int)(b)) timer_expired = 1; |
||
700 | |||
701 | if (timeout_event != NIL) kern_event_delete(timeout_event); |
||
702 | |||
703 | if (!timer_expired) |
||
704 | return 0; |
||
705 | else |
||
706 | return -1; |
||
707 | |||
708 | } |
||
709 | |||
282 | giacomo | 710 | /* 0000.1ppp */ |
290 | giacomo | 711 | int servo_get_angle_sec(int servo) |
281 | giacomo | 712 | { |
282 | giacomo | 713 | struct timespec current_time; |
714 | unsigned char b; |
||
290 | giacomo | 715 | int err,res,data; |
282 | giacomo | 716 | |
717 | if (servo > 7) return -1; |
||
718 | |||
719 | timer_expired = 0; |
||
720 | kern_gettime(¤t_time); |
||
721 | ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
||
722 | timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
||
281 | giacomo | 723 | |
282 | giacomo | 724 | b = 0x08 | (servo & 0x07); |
289 | giacomo | 725 | err = com_send(servo_port, b); |
726 | err = com_receive(servo_port); |
||
283 | giacomo | 727 | if (err != (int)(b)) timer_expired = 1; |
289 | giacomo | 728 | res = com_receive(servo_port) << 8; |
729 | res |= com_receive(servo_port); |
||
282 | giacomo | 730 | |
283 | giacomo | 731 | if (timeout_event != NIL) kern_event_delete(timeout_event); |
282 | giacomo | 732 | |
290 | giacomo | 733 | data = (res - servo_table[servo].zero_tick) * |
734 | (servo_table[servo].max_angle_sec - servo_table[servo].min_angle_sec) / |
||
735 | servo_table[servo].delta_tick; |
||
736 | |||
282 | giacomo | 737 | if (!timer_expired) |
290 | giacomo | 738 | return data; |
282 | giacomo | 739 | else |
740 | return -1; |
||
741 | |||
281 | giacomo | 742 | } |
743 | |||
282 | giacomo | 744 | /* 0100:0aaa */ |
281 | giacomo | 745 | int servo_get_analog(int port) |
746 | { |
||
747 | |||
282 | giacomo | 748 | struct timespec current_time; |
749 | unsigned char b; |
||
283 | giacomo | 750 | int err,res; |
282 | giacomo | 751 | |
752 | if (port > 4) return -1; |
||
753 | |||
754 | timer_expired = 0; |
||
755 | kern_gettime(¤t_time); |
||
756 | ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
||
757 | timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
||
281 | giacomo | 758 | |
282 | giacomo | 759 | b = 0x40 | (port & 7); |
289 | giacomo | 760 | err = com_send(servo_port, b); |
761 | err = com_receive(servo_port); |
||
283 | giacomo | 762 | if (err != (int)(b)) timer_expired = 1; |
289 | giacomo | 763 | res = com_receive(servo_port) << 8; |
764 | res |= com_receive(servo_port); |
||
282 | giacomo | 765 | |
283 | giacomo | 766 | if (timeout_event != NIL) kern_event_delete(timeout_event); |
282 | giacomo | 767 | |
768 | if (!timer_expired) |
||
769 | return res; |
||
770 | else |
||
771 | return -1; |
||
772 | |||
281 | giacomo | 773 | } |
774 |