Rev 772 | 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> |
||
381 | giacomo | 10 | * Mauro Marinoni <mauro.marinoni@unipv.it> |
281 | giacomo | 11 | * (see the web pages for full authors list) |
12 | * |
||
13 | * ReTiS Lab (Scuola Superiore S.Anna - Pisa - Italy) |
||
14 | * |
||
15 | * http://www.sssup.it |
||
16 | * http://retis.sssup.it |
||
17 | * http://shark.sssup.it |
||
18 | */ |
||
19 | |||
20 | /* |
||
21 | * Copyright (C) 2002 Paolo Gai |
||
22 | * |
||
23 | * This program is free software; you can redistribute it and/or modify |
||
24 | * it under the terms of the GNU General Public License as published by |
||
25 | * the Free Software Foundation; either version 2 of the License, or |
||
26 | * (at your option) any later version. |
||
27 | * |
||
28 | * This program is distributed in the hope that it will be useful, |
||
29 | * but WITHOUT ANY WARRANTY; without even the implied warranty of |
||
30 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
||
31 | * GNU General Public License for more details. |
||
32 | * |
||
33 | * You should have received a copy of the GNU General Public License |
||
34 | * along with this program; if not, write to the Free Software |
||
35 | * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA |
||
36 | * |
||
37 | */ |
||
38 | |||
282 | giacomo | 39 | #include "kernel/kern.h" |
281 | giacomo | 40 | |
717 | giacomo | 41 | #include "drivers/scom.h" |
42 | #include "drivers/scomirq.h" |
||
281 | giacomo | 43 | |
717 | giacomo | 44 | #include "servo.h" |
286 | giacomo | 45 | |
750 | giacomo | 46 | //#define SERVO_DEBUG |
286 | giacomo | 47 | |
398 | giacomo | 48 | #define SERVO_SPEED 19200 |
735 | giacomo | 49 | #define SERVO_PARITY EVEN |
286 | giacomo | 50 | #define SERVO_LEN 8 |
51 | #define SERVO_STOP 1 |
||
282 | giacomo | 52 | |
358 | giacomo | 53 | #define SERVO_CLOCK 20000000 /* 20MHz */ |
54 | |||
315 | giacomo | 55 | #define TICK_LEN 1600 /* ns */ |
316 | giacomo | 56 | #define TICK_LEN_PERIOD 51200 /* ns */ |
290 | giacomo | 57 | |
58 | struct servo_data { |
||
59 | int min_angle_sec; |
||
60 | int max_angle_sec; |
||
61 | int delta_tick; |
||
62 | int zero_tick; |
||
63 | }; |
||
64 | |||
381 | giacomo | 65 | struct servo_data servo_table[4][16] = { |
66 | {{-324000, 324000, 1200, 1600}, |
||
316 | giacomo | 67 | {-324000, 324000, 1200, 1600}, |
68 | {-324000, 324000, 1200, 1600}, |
||
69 | {-324000, 324000, 1200, 1600}, |
||
70 | {-324000, 324000, 1200, 1600}, |
||
71 | {-324000, 324000, 1200, 1600}, |
||
72 | {-324000, 324000, 1200, 1600}, |
||
73 | {-324000, 324000, 1200, 1600}, |
||
335 | giacomo | 74 | {-324000, 324000, 1200, 1600}, |
75 | {-324000, 324000, 1200, 1600}, |
||
76 | {-324000, 324000, 1200, 1600}, |
||
77 | {-324000, 324000, 1200, 1600}, |
||
78 | {-324000, 324000, 1200, 1600}, |
||
79 | {-324000, 324000, 1200, 1600}, |
||
80 | {-324000, 324000, 1200, 1600}, |
||
381 | giacomo | 81 | {-324000, 324000, 1200, 1600}}, |
82 | {{-324000, 324000, 1200, 1600}, |
||
335 | giacomo | 83 | {-324000, 324000, 1200, 1600}, |
381 | giacomo | 84 | {-324000, 324000, 1200, 1600}, |
85 | {-324000, 324000, 1200, 1600}, |
||
86 | {-324000, 324000, 1200, 1600}, |
||
87 | {-324000, 324000, 1200, 1600}, |
||
88 | {-324000, 324000, 1200, 1600}, |
||
89 | {-324000, 324000, 1200, 1600}, |
||
90 | {-324000, 324000, 1200, 1600}, |
||
91 | {-324000, 324000, 1200, 1600}, |
||
92 | {-324000, 324000, 1200, 1600}, |
||
93 | {-324000, 324000, 1200, 1600}, |
||
94 | {-324000, 324000, 1200, 1600}, |
||
95 | {-324000, 324000, 1200, 1600}, |
||
96 | {-324000, 324000, 1200, 1600}, |
||
97 | {-324000, 324000, 1200, 1600}}, |
||
98 | {{-324000, 324000, 1200, 1600}, |
||
99 | {-324000, 324000, 1200, 1600}, |
||
100 | {-324000, 324000, 1200, 1600}, |
||
101 | {-324000, 324000, 1200, 1600}, |
||
102 | {-324000, 324000, 1200, 1600}, |
||
103 | {-324000, 324000, 1200, 1600}, |
||
104 | {-324000, 324000, 1200, 1600}, |
||
105 | {-324000, 324000, 1200, 1600}, |
||
106 | {-324000, 324000, 1200, 1600}, |
||
107 | {-324000, 324000, 1200, 1600}, |
||
108 | {-324000, 324000, 1200, 1600}, |
||
109 | {-324000, 324000, 1200, 1600}, |
||
110 | {-324000, 324000, 1200, 1600}, |
||
111 | {-324000, 324000, 1200, 1600}, |
||
112 | {-324000, 324000, 1200, 1600}, |
||
113 | {-324000, 324000, 1200, 1600}}, |
||
114 | {{-324000, 324000, 1200, 1600}, |
||
115 | {-324000, 324000, 1200, 1600}, |
||
116 | {-324000, 324000, 1200, 1600}, |
||
117 | {-324000, 324000, 1200, 1600}, |
||
118 | {-324000, 324000, 1200, 1600}, |
||
119 | {-324000, 324000, 1200, 1600}, |
||
120 | {-324000, 324000, 1200, 1600}, |
||
121 | {-324000, 324000, 1200, 1600}, |
||
122 | {-324000, 324000, 1200, 1600}, |
||
123 | {-324000, 324000, 1200, 1600}, |
||
124 | {-324000, 324000, 1200, 1600}, |
||
125 | {-324000, 324000, 1200, 1600}, |
||
126 | {-324000, 324000, 1200, 1600}, |
||
127 | {-324000, 324000, 1200, 1600}, |
||
128 | {-324000, 324000, 1200, 1600}, |
||
129 | {-324000, 324000, 1200, 1600}}}; |
||
290 | giacomo | 130 | |
772 | mauro | 131 | #define SERVO_TIMEOUT 100000 |
730 | giacomo | 132 | |
717 | giacomo | 133 | #define RXTX_BUFF_MAX 100 |
726 | giacomo | 134 | volatile static BYTE RXTX_buff[4][RXTX_BUFF_MAX]; |
135 | volatile static BYTE *RXTX_addr[4][RXTX_BUFF_MAX]; |
||
725 | giacomo | 136 | volatile unsigned int RX_position[4] = {0,0,0,0}; |
137 | volatile unsigned int TX_position[4] = {0,0,0,0}; |
||
730 | giacomo | 138 | volatile unsigned int RX_cycle[4] = {0,0,0,0}; |
139 | volatile unsigned int TX_cycle[4] = {0,0,0,0}; |
||
728 | giacomo | 140 | volatile unsigned int lock_write[4] = {0,0,0,0}; |
286 | giacomo | 141 | |
733 | giacomo | 142 | #define SERVO_LOCK 0 |
143 | #define SERVO_UNLOCK 1 |
||
144 | #define SERVO_SKIP (BYTE *)(0xFFFFFFFF) |
||
145 | |||
286 | giacomo | 146 | const int BaudTable[] = { |
147 | 1200, |
||
148 | 2400, |
||
149 | 4800, |
||
150 | 9600, |
||
151 | 14400, |
||
152 | 19200, |
||
153 | 38400, |
||
154 | 57600, |
||
155 | 115200, |
||
156 | -1}; |
||
157 | |||
735 | giacomo | 158 | void servo_rx_error(unsigned port, unsigned type, unsigned data) { |
292 | giacomo | 159 | |
734 | giacomo | 160 | #ifdef SERVO_DEBUG |
735 | giacomo | 161 | kern_printf("(SERVO: BUFFER ERROR PORT:%d ERR:%d DATA:%02x)",port,type,data); |
734 | giacomo | 162 | #endif |
717 | giacomo | 163 | |
281 | giacomo | 164 | } |
165 | |||
717 | giacomo | 166 | void servo_indication(unsigned port, BYTE data) { |
727 | giacomo | 167 | |
717 | giacomo | 168 | if (RXTX_addr[port][RX_position[port]] == NULL) { |
738 | giacomo | 169 | if (data != 0) { |
735 | giacomo | 170 | servo_rx_error(port,1,data); |
738 | giacomo | 171 | while(RXTX_addr[port][RX_position[port]] != SERVO_SKIP) { |
172 | RXTX_addr[port][RX_position[port]] = SERVO_SKIP; |
||
173 | RX_position[port]++; |
||
726 | giacomo | 174 | |
738 | giacomo | 175 | if (RX_position[port] >= RXTX_BUFF_MAX) { |
176 | RX_cycle[port]++; |
||
177 | RX_position[port] = 0; |
||
178 | } |
||
179 | if ((RX_cycle[port] == TX_cycle[port] && RX_position[port] > TX_position[port]) || |
||
180 | (RX_cycle[port] > TX_cycle[port])) |
||
181 | servo_rx_error(port,2,data); |
||
182 | |||
183 | } |
||
184 | } |
||
185 | |||
733 | giacomo | 186 | RXTX_addr[port][RX_position[port]] = SERVO_SKIP; |
187 | |||
717 | giacomo | 188 | RX_position[port]++; |
721 | giacomo | 189 | |
190 | if (RX_position[port] >= RXTX_BUFF_MAX) { |
||
730 | giacomo | 191 | RX_cycle[port]++; |
721 | giacomo | 192 | RX_position[port] = 0; |
193 | } |
||
730 | giacomo | 194 | if ((RX_cycle[port] == TX_cycle[port] && RX_position[port] > TX_position[port]) || |
195 | (RX_cycle[port] > TX_cycle[port])) |
||
735 | giacomo | 196 | servo_rx_error(port,2,data); |
721 | giacomo | 197 | |
717 | giacomo | 198 | } else { |
726 | giacomo | 199 | |
733 | giacomo | 200 | if (RXTX_addr[port][RX_position[port]] != SERVO_SKIP) { |
721 | giacomo | 201 | |
733 | giacomo | 202 | *RXTX_addr[port][RX_position[port]] = data; |
203 | RXTX_buff[port][RX_position[port]] = SERVO_UNLOCK; //Unlock the data |
||
204 | |||
205 | RXTX_addr[port][RX_position[port]] = SERVO_SKIP; |
||
206 | |||
207 | RX_position[port]++; |
||
208 | |||
209 | if (RX_position[port] >= RXTX_BUFF_MAX) { |
||
730 | giacomo | 210 | RX_cycle[port]++; |
721 | giacomo | 211 | RX_position[port] = 0; |
733 | giacomo | 212 | } |
213 | if ((RX_cycle[port] == TX_cycle[port] && RX_position[port] > TX_position[port]) || |
||
214 | (RX_cycle[port] > TX_cycle[port])) |
||
735 | giacomo | 215 | servo_rx_error(port,3,data); |
733 | giacomo | 216 | |
217 | } else { |
||
218 | |||
219 | RX_position[port]++; |
||
220 | |||
221 | if (RX_position[port] >= RXTX_BUFF_MAX) { |
||
222 | RX_cycle[port]++; |
||
223 | RX_position[port] = 0; |
||
224 | } |
||
225 | if ((RX_cycle[port] == TX_cycle[port] && RX_position[port] > TX_position[port]) || |
||
226 | (RX_cycle[port] > TX_cycle[port])) |
||
735 | giacomo | 227 | servo_rx_error(port,3,data); |
733 | giacomo | 228 | |
229 | servo_indication(port, data); |
||
230 | |||
721 | giacomo | 231 | } |
726 | giacomo | 232 | |
717 | giacomo | 233 | } |
295 | giacomo | 234 | |
286 | giacomo | 235 | } |
236 | |||
730 | giacomo | 237 | extern unsigned SCom_Error[4]; |
727 | giacomo | 238 | |
717 | giacomo | 239 | void servo_confirm(unsigned port, BYTE msg_status) { |
286 | giacomo | 240 | |
734 | giacomo | 241 | #ifdef SERVO_DEBUG |
242 | if (msg_status == COM_ERROR) { |
||
243 | kern_printf("(SCOM: PORT:%d ERROR:%d)",port,SCom_Error[port]); |
||
244 | } |
||
245 | #endif |
||
286 | giacomo | 246 | |
730 | giacomo | 247 | lock_write[port] = 0; |
727 | giacomo | 248 | |
717 | giacomo | 249 | } |
250 | |||
251 | int servo_send_msg(unsigned port, BYTE *msg, unsigned len_msg, BYTE *res, unsigned len_res) { |
||
252 | |||
726 | giacomo | 253 | int i = 0, old, oldcycle, RX = 0; |
733 | giacomo | 254 | volatile int start = kern_gettime(NULL), timeout = 0; |
721 | giacomo | 255 | SYS_FLAGS f; |
256 | |||
733 | giacomo | 257 | while(lock_write[port] == 1 && timeout == 0) |
258 | if ((start + SERVO_TIMEOUT) < kern_gettime(NULL)) timeout = 1; |
||
259 | |||
260 | if (timeout == 1) { |
||
734 | giacomo | 261 | |
262 | #ifdef SERVO_DEBUG |
||
263 | kern_printf("(SERVO: TIMEOUT SEND)"); |
||
264 | #endif |
||
265 | |||
733 | giacomo | 266 | return -1; |
267 | } |
||
268 | |||
728 | giacomo | 269 | lock_write[port] = 1; |
727 | giacomo | 270 | |
721 | giacomo | 271 | f = kern_fsave(); |
272 | |||
717 | giacomo | 273 | old = TX_position[port]; |
730 | giacomo | 274 | oldcycle = TX_cycle[port]; |
717 | giacomo | 275 | |
276 | while(i < len_msg) { |
||
277 | RXTX_buff[port][TX_position[port]] = msg[i]; |
||
278 | RXTX_addr[port][TX_position[port]] = NULL; |
||
279 | TX_position[port]++; |
||
721 | giacomo | 280 | if (TX_position[port] >= RXTX_BUFF_MAX) { |
730 | giacomo | 281 | TX_cycle[port]++; |
721 | giacomo | 282 | TX_position[port] = 0; |
283 | } |
||
717 | giacomo | 284 | if (TX_position[port] == RX_position[port]) { |
285 | TX_position[port] = old; |
||
730 | giacomo | 286 | TX_cycle[port] = oldcycle; |
287 | lock_write[port] = 0; |
||
733 | giacomo | 288 | |
734 | giacomo | 289 | #ifdef SERVO_DEBUG |
290 | kern_printf("(SERVO: BUFFER FULL)"); |
||
291 | #endif |
||
733 | giacomo | 292 | |
721 | giacomo | 293 | kern_frestore(f); |
717 | giacomo | 294 | return -1; |
295 | } |
||
296 | i++; |
||
297 | } |
||
298 | |||
299 | i = 0; |
||
300 | while(i < len_res) { |
||
733 | giacomo | 301 | RXTX_buff[port][TX_position[port]] = SERVO_LOCK; //Lock the data |
717 | giacomo | 302 | RXTX_addr[port][TX_position[port]] = res+i; |
726 | giacomo | 303 | RX = TX_position[port]; |
717 | giacomo | 304 | TX_position[port]++; |
721 | giacomo | 305 | if (TX_position[port] >= RXTX_BUFF_MAX) { |
730 | giacomo | 306 | TX_cycle[port]++; |
721 | giacomo | 307 | TX_position[port] = 0; |
308 | } |
||
717 | giacomo | 309 | if (TX_position[port] == RX_position[port]) { |
310 | TX_position[port] = old; |
||
730 | giacomo | 311 | TX_cycle[port] = oldcycle; |
312 | lock_write[port] = 0; |
||
733 | giacomo | 313 | |
734 | giacomo | 314 | #ifdef SERVO_DEBUG |
315 | kern_printf("(SERVO: BUFFER FULL)"); |
||
316 | #endif |
||
733 | giacomo | 317 | |
721 | giacomo | 318 | kern_frestore(f); |
717 | giacomo | 319 | return -1; |
320 | } |
||
321 | i++; |
||
322 | } |
||
323 | |||
738 | giacomo | 324 | /* SERVO_SKIP close the command sequence */ |
325 | RXTX_addr[port][TX_position[port]] = SERVO_SKIP; |
||
326 | TX_position[port]++; |
||
327 | if (TX_position[port] >= RXTX_BUFF_MAX) { |
||
328 | TX_cycle[port]++; |
||
329 | TX_position[port] = 0; |
||
330 | } |
||
331 | if (TX_position[port] == RX_position[port]) { |
||
332 | TX_position[port] = old; |
||
333 | TX_cycle[port] = oldcycle; |
||
334 | lock_write[port] = 0; |
||
335 | |||
336 | #ifdef SERVO_DEBUG |
||
337 | kern_printf("(SERVO: BUFFER FULL)"); |
||
338 | #endif |
||
339 | |||
340 | kern_frestore(f); |
||
341 | return -1; |
||
342 | } |
||
343 | |||
721 | giacomo | 344 | kern_frestore(f); |
345 | |||
717 | giacomo | 346 | com_irq_send(port, len_msg, msg); |
347 | |||
725 | giacomo | 348 | return RX; |
349 | |||
350 | } |
||
351 | |||
733 | giacomo | 352 | int servo_wait(unsigned port, int RX) { |
730 | giacomo | 353 | |
733 | giacomo | 354 | volatile int start = kern_gettime(NULL), timeout = 0, temp; |
730 | giacomo | 355 | |
733 | giacomo | 356 | /* Active wait until number bytes received */ |
738 | giacomo | 357 | while(RXTX_buff[port][RX] == SERVO_LOCK && timeout == 0) |
733 | giacomo | 358 | if ((start + SERVO_TIMEOUT) < kern_gettime(NULL)) timeout = 1; |
730 | giacomo | 359 | |
733 | giacomo | 360 | if (timeout == 1) { |
361 | SYS_FLAGS f; |
||
730 | giacomo | 362 | |
734 | giacomo | 363 | #ifdef SERVO_DEBUG |
364 | kern_printf("(SERVO: TIMEOUT GET)"); |
||
365 | #endif |
||
366 | |||
738 | giacomo | 367 | f = kern_fsave(); |
368 | |||
733 | giacomo | 369 | temp = RX; |
738 | giacomo | 370 | |
733 | giacomo | 371 | while(RXTX_addr[port][temp] != SERVO_SKIP) { |
372 | RXTX_addr[port][temp] = SERVO_SKIP; |
||
738 | giacomo | 373 | RXTX_buff[port][temp] = SERVO_UNLOCK; |
733 | giacomo | 374 | temp--; |
738 | giacomo | 375 | if (temp < 0) temp = RXTX_BUFF_MAX-1; |
733 | giacomo | 376 | } |
738 | giacomo | 377 | |
733 | giacomo | 378 | kern_frestore(f); |
725 | giacomo | 379 | |
733 | giacomo | 380 | } |
730 | giacomo | 381 | |
733 | giacomo | 382 | return timeout; |
286 | giacomo | 383 | |
384 | } |
||
385 | |||
358 | giacomo | 386 | int servo_open(int port,int speed) |
286 | giacomo | 387 | { |
388 | int err; |
||
389 | |||
358 | giacomo | 390 | err = com_open((unsigned)(port), (DWORD)speed, SERVO_PARITY, SERVO_LEN, SERVO_STOP); |
391 | |||
717 | giacomo | 392 | com_init_irq((unsigned)(port)); |
393 | |||
771 | giacomo | 394 | com_set_functions((unsigned)(port),servo_confirm,servo_indication); |
717 | giacomo | 395 | |
721 | giacomo | 396 | com_irq_enable((unsigned)(port),ALL_IRQ); |
397 | |||
286 | giacomo | 398 | return err; |
399 | |||
400 | } |
||
401 | |||
358 | giacomo | 402 | int servo_close(int port) |
286 | giacomo | 403 | { |
404 | int err; |
||
405 | |||
724 | giacomo | 406 | com_irq_disable((unsigned)(port),ALL_IRQ); |
407 | |||
717 | giacomo | 408 | com_close_irq((unsigned)(port)); |
409 | |||
358 | giacomo | 410 | err = com_close((unsigned)(port)); |
286 | giacomo | 411 | |
412 | return err; |
||
413 | |||
414 | } |
||
415 | |||
416 | /* 1000.011w:bbbb.bbbb */ |
||
358 | giacomo | 417 | int servo_set_RS232_baudrate(int port, int baud) |
281 | giacomo | 418 | { |
724 | giacomo | 419 | unsigned char b[2]; |
420 | int spbrg_temp, i; |
||
286 | giacomo | 421 | unsigned char spbrg, w; |
358 | giacomo | 422 | int servo_port = (unsigned)(port); |
286 | giacomo | 423 | |
424 | i = 0; |
||
425 | while(BaudTable[i] != baud && BaudTable[i] != -1) i++; |
||
426 | if (BaudTable[i] == -1) { |
||
427 | kern_printf("SERVO:Error wrong baud rate\n"); |
||
428 | return -1; |
||
429 | } |
||
430 | |||
358 | giacomo | 431 | w = 1; |
453 | giacomo | 432 | spbrg_temp = (SERVO_CLOCK*10 / (16*baud)) - 10; |
412 | giacomo | 433 | if (spbrg_temp>2550) { |
358 | giacomo | 434 | w = 0; |
453 | giacomo | 435 | spbrg_temp = (SERVO_CLOCK*10 / (64*baud)) - 10; |
286 | giacomo | 436 | } |
412 | giacomo | 437 | spbrg = spbrg_temp / 10; |
453 | giacomo | 438 | if (spbrg_temp % 10 > 5) spbrg++; |
412 | giacomo | 439 | |
286 | giacomo | 440 | #ifdef SERVO_DEBUG |
441 | kern_printf("(SERVO:SBPRG %d W %d)",spbrg,w); |
||
358 | giacomo | 442 | #endif |
443 | |||
724 | giacomo | 444 | b[0] = 0x86 | (w & 0x01); |
445 | b[1] = spbrg; |
||
446 | servo_send_msg(servo_port, b, 2, NULL, 0); |
||
358 | giacomo | 447 | |
724 | giacomo | 448 | return 0; |
381 | giacomo | 449 | |
281 | giacomo | 450 | } |
451 | |||
285 | giacomo | 452 | /* 1000.0101 */ |
358 | giacomo | 453 | int servo_get_RS232_baudrate(int port) |
281 | giacomo | 454 | { |
724 | giacomo | 455 | unsigned char b[1],r[2]; |
733 | giacomo | 456 | int res, res_w, res_b, t; |
358 | giacomo | 457 | int servo_port = (unsigned)(port); |
725 | giacomo | 458 | int RX; |
358 | giacomo | 459 | |
724 | giacomo | 460 | b[0] = 0x85; |
725 | giacomo | 461 | RX = servo_send_msg(servo_port, b, 1, r, 2); |
358 | giacomo | 462 | |
732 | giacomo | 463 | if (RX != -1) { |
464 | |||
733 | giacomo | 465 | t = servo_wait(servo_port, RX); |
732 | giacomo | 466 | |
733 | giacomo | 467 | if (t == 0) { |
725 | giacomo | 468 | |
732 | giacomo | 469 | res_w = r[0]; /* bit W */ |
470 | res_b = r[1]; /* byte SPBRG */ |
||
465 | giacomo | 471 | |
732 | giacomo | 472 | if (res_w != -1 && res_b != -1) { |
473 | if (res_w) |
||
474 | res = SERVO_CLOCK / ( 16 * (res_b + 1) ); |
||
475 | else |
||
476 | res = SERVO_CLOCK / ( 64 * (res_b + 1) ); |
||
477 | } else { |
||
478 | return -1; |
||
479 | } |
||
358 | giacomo | 480 | |
732 | giacomo | 481 | return res; |
358 | giacomo | 482 | |
732 | giacomo | 483 | } else return -1; |
484 | |||
485 | } else return -1; |
||
486 | |||
281 | giacomo | 487 | } |
488 | |||
286 | giacomo | 489 | /* 1000.0100 */ |
358 | giacomo | 490 | int servo_store_RS232_baudrate(int port) |
281 | giacomo | 491 | { |
724 | giacomo | 492 | unsigned char b[1]; |
358 | giacomo | 493 | int servo_port = (unsigned)(port); |
494 | |||
724 | giacomo | 495 | b[0] = 0x84; |
496 | servo_send_msg(servo_port, b, 1, NULL, 0); |
||
497 | |||
498 | return 0; |
||
358 | giacomo | 499 | |
281 | giacomo | 500 | } |
501 | |||
289 | giacomo | 502 | /* 1000.1010:llll.llll */ |
358 | giacomo | 503 | int servo_set_period(int port, int period) |
281 | giacomo | 504 | { |
724 | giacomo | 505 | unsigned char b[2]; |
358 | giacomo | 506 | int servo_port = (unsigned)(port); |
507 | |||
724 | giacomo | 508 | b[0] = 0x8A; |
509 | b[1] = (period*1000)/TICK_LEN_PERIOD/8 & 0xFF; |
||
510 | servo_send_msg(servo_port, b, 2, NULL, 0); |
||
381 | giacomo | 511 | |
724 | giacomo | 512 | return 0; |
358 | giacomo | 513 | |
281 | giacomo | 514 | } |
515 | |||
285 | giacomo | 516 | /* 1000.1001 */ |
358 | giacomo | 517 | int servo_get_period(int port) |
281 | giacomo | 518 | { |
724 | giacomo | 519 | unsigned char b[1],r[1]; |
733 | giacomo | 520 | int res, RX, t; |
358 | giacomo | 521 | int servo_port = (unsigned)(port); |
522 | |||
724 | giacomo | 523 | b[0] = 0x89; |
725 | giacomo | 524 | RX = servo_send_msg(servo_port, b, 1, r, 1); |
358 | giacomo | 525 | |
732 | giacomo | 526 | if (RX != -1) { |
527 | |||
733 | giacomo | 528 | t = servo_wait(servo_port, RX); |
732 | giacomo | 529 | |
733 | giacomo | 530 | if (t == 0) { |
725 | giacomo | 531 | |
732 | giacomo | 532 | res = r[0]; |
381 | giacomo | 533 | |
732 | giacomo | 534 | return (((unsigned char)(res))*TICK_LEN_PERIOD/1000*8); |
358 | giacomo | 535 | |
732 | giacomo | 536 | } else return -1; |
537 | |||
538 | } else return -1; |
||
539 | |||
281 | giacomo | 540 | } |
541 | |||
289 | giacomo | 542 | /* 1000.1000 */ |
358 | giacomo | 543 | int servo_store_period(int port) |
281 | giacomo | 544 | { |
724 | giacomo | 545 | unsigned char b[1]; |
358 | giacomo | 546 | int servo_port = (unsigned)(port); |
547 | |||
724 | giacomo | 548 | b[0] = 0x88; |
549 | servo_send_msg(servo_port, b, 1, NULL, 0); |
||
358 | giacomo | 550 | |
724 | giacomo | 551 | return 0; |
381 | giacomo | 552 | |
281 | giacomo | 553 | } |
554 | |||
283 | giacomo | 555 | /* 1000.1100 */ |
358 | giacomo | 556 | int servo_get_setup_switch(int port) |
281 | giacomo | 557 | { |
724 | giacomo | 558 | unsigned char b[1],r[1]; |
733 | giacomo | 559 | int res, RX, t; |
358 | giacomo | 560 | int servo_port = (unsigned)(port); |
561 | |||
724 | giacomo | 562 | b[0] = 0x8C; |
725 | giacomo | 563 | RX = servo_send_msg(servo_port, b, 1, r, 1); |
281 | giacomo | 564 | |
732 | giacomo | 565 | if (RX != -1) { |
566 | |||
733 | giacomo | 567 | t = servo_wait(servo_port, RX); |
732 | giacomo | 568 | |
733 | giacomo | 569 | if (t == 0) { |
725 | giacomo | 570 | |
732 | giacomo | 571 | res = r[0]; |
281 | giacomo | 572 | |
732 | giacomo | 573 | return res; |
283 | giacomo | 574 | |
732 | giacomo | 575 | } else return -1; |
576 | |||
577 | } else return -1; |
||
578 | |||
281 | giacomo | 579 | } |
580 | |||
283 | giacomo | 581 | /* 1000.111s */ |
358 | giacomo | 582 | int servo_set_RC5_switch(int port, int data) |
281 | giacomo | 583 | { |
724 | giacomo | 584 | unsigned char b[1]; |
358 | giacomo | 585 | int servo_port = (unsigned)(port); |
586 | |||
724 | giacomo | 587 | b[0] = 0x8E | (data & 0x01); |
588 | servo_send_msg(servo_port, b, 1, NULL, 0); |
||
281 | giacomo | 589 | |
724 | giacomo | 590 | return 0; |
381 | giacomo | 591 | |
281 | giacomo | 592 | } |
593 | |||
323 | giacomo | 594 | /* 1000.0000:0000.Mmmm */ |
358 | giacomo | 595 | int servo_turn_off(int port, int servo) |
285 | giacomo | 596 | { |
724 | giacomo | 597 | unsigned char b[2]; |
358 | giacomo | 598 | int servo_port = (unsigned)(port); |
599 | |||
600 | if (servo > 15) return -1; |
||
724 | giacomo | 601 | |
602 | b[0] = 0x80; |
||
603 | b[1] = 0x00 | (servo & 0x0F); |
||
604 | servo_send_msg(servo_port, b, 2, NULL, 0); |
||
381 | giacomo | 605 | |
724 | giacomo | 606 | return 0; |
285 | giacomo | 607 | |
608 | } |
||
609 | |||
325 | giacomo | 610 | /* 1000.0000:0001.Mmmm */ |
358 | giacomo | 611 | int servo_turn_on(int port, int servo) |
285 | giacomo | 612 | { |
721 | giacomo | 613 | unsigned char b[2]; |
358 | giacomo | 614 | int servo_port = (unsigned)(port); |
615 | |||
323 | giacomo | 616 | if (servo > 15) return -1; |
358 | giacomo | 617 | |
721 | giacomo | 618 | b[0] = 0x80; |
619 | b[1] = 0x10 | (servo & 0x0F); |
||
620 | servo_send_msg(servo_port, b, 2, NULL, 0); |
||
358 | giacomo | 621 | |
721 | giacomo | 622 | return 0; |
358 | giacomo | 623 | |
285 | giacomo | 624 | } |
625 | |||
291 | giacomo | 626 | /* 1000.0000:0010.0000 */ |
358 | giacomo | 627 | int servo_turn_off_all(int port) |
323 | giacomo | 628 | { |
724 | giacomo | 629 | unsigned char b[2]; |
358 | giacomo | 630 | int servo_port = (unsigned)(port); |
631 | |||
724 | giacomo | 632 | b[0] = 0x80; |
633 | b[1] = 0x20; |
||
634 | servo_send_msg(servo_port, b, 2, NULL, 0); |
||
358 | giacomo | 635 | |
724 | giacomo | 636 | return 0; |
358 | giacomo | 637 | |
285 | giacomo | 638 | } |
639 | |||
291 | giacomo | 640 | /* 1000.0000:0010.0001 */ |
358 | giacomo | 641 | int servo_turn_on_all(int port) |
285 | giacomo | 642 | { |
724 | giacomo | 643 | unsigned char b[2]; |
358 | giacomo | 644 | int servo_port = (unsigned)(port); |
645 | |||
724 | giacomo | 646 | b[0] = 0x80; |
647 | b[1] = 0x21; |
||
648 | servo_send_msg(servo_port, b, 2, NULL, 0); |
||
649 | |||
650 | return 0; |
||
358 | giacomo | 651 | |
285 | giacomo | 652 | } |
653 | |||
323 | giacomo | 654 | /* 1000.0000:0101.000M:mmmm.mmmm */ |
724 | giacomo | 655 | int servo_set_levels(int port, int bank, int mask) |
315 | giacomo | 656 | { |
724 | giacomo | 657 | unsigned char b[3]; |
358 | giacomo | 658 | int servo_port = (unsigned)(port); |
659 | |||
724 | giacomo | 660 | b[0] = 0x80; |
661 | b[1] = 0x50 | (0x01 & bank); |
||
662 | b[2] = (unsigned char)(mask & 0xFF); |
||
663 | servo_send_msg(servo_port, b, 3, NULL, 0); |
||
358 | giacomo | 664 | |
724 | giacomo | 665 | return 0; |
358 | giacomo | 666 | |
315 | giacomo | 667 | } |
668 | |||
323 | giacomo | 669 | /* 1000.0000:0100.000M */ |
358 | giacomo | 670 | int servo_get_levels(int port, int bank) |
297 | giacomo | 671 | { |
724 | giacomo | 672 | unsigned char b[2],r[1]; |
733 | giacomo | 673 | int res, RX, t; |
358 | giacomo | 674 | int servo_port = (unsigned)(port); |
675 | |||
724 | giacomo | 676 | b[0] = 0x80; |
677 | b[1] = 0x40 | (0x01 & bank); |
||
725 | giacomo | 678 | RX = servo_send_msg(servo_port, b, 2, r, 1); |
679 | |||
732 | giacomo | 680 | if (RX != -1) { |
681 | |||
733 | giacomo | 682 | t = servo_wait(servo_port, RX); |
732 | giacomo | 683 | |
733 | giacomo | 684 | if (t == 0) { |
358 | giacomo | 685 | |
732 | giacomo | 686 | res = r[0]; |
358 | giacomo | 687 | |
732 | giacomo | 688 | return res; |
689 | |||
690 | } else return -1; |
||
691 | |||
692 | } else return -1; |
||
693 | |||
297 | giacomo | 694 | } |
695 | |||
696 | /* 1000.0000:1000.0000 */ |
||
358 | giacomo | 697 | int servo_store_levels(int port) |
297 | giacomo | 698 | { |
724 | giacomo | 699 | unsigned char b[2]; |
358 | giacomo | 700 | int servo_port = (unsigned)(port); |
701 | |||
724 | giacomo | 702 | b[0] = 0x80; |
703 | b[1] = 0x80; |
||
704 | servo_send_msg(servo_port, b, 2, NULL, 0); |
||
381 | giacomo | 705 | |
724 | giacomo | 706 | return 0; |
358 | giacomo | 707 | |
297 | giacomo | 708 | } |
709 | |||
493 | giacomo | 710 | int servo_set_max_angle_sec(int port, int servo, int angle_sec) |
290 | giacomo | 711 | { |
712 | |||
381 | giacomo | 713 | servo_table[port][servo].max_angle_sec = angle_sec; |
290 | giacomo | 714 | return 0; |
715 | |||
716 | } |
||
717 | |||
493 | giacomo | 718 | int servo_set_min_angle_sec(int port, int servo, int angle_sec) |
290 | giacomo | 719 | { |
720 | |||
381 | giacomo | 721 | servo_table[port][servo].min_angle_sec = angle_sec; |
290 | giacomo | 722 | return 0; |
723 | |||
724 | } |
||
725 | |||
381 | giacomo | 726 | int servo_set_servo_tick(int port, int servo, int zero_tick, int delta_tick) |
727 | { |
||
728 | |||
729 | if (zero_tick != -1) servo_table[port][servo].zero_tick = zero_tick; |
||
730 | if (delta_tick != -1) servo_table[port][servo].delta_tick = delta_tick; |
||
731 | return 0; |
||
732 | |||
733 | } |
||
734 | |||
323 | giacomo | 735 | /* 0000.Pppp:0000.vvvv:vvvv.vvvv */ |
358 | giacomo | 736 | int servo_set_angle_sec(int port, int servo, int angle_sec) |
281 | giacomo | 737 | { |
717 | giacomo | 738 | unsigned char b[3]; |
730 | giacomo | 739 | int angle_tick, res = 0; |
358 | giacomo | 740 | int servo_port = (unsigned)(port); |
282 | giacomo | 741 | |
323 | giacomo | 742 | if (servo > 15) return -1; |
282 | giacomo | 743 | |
381 | giacomo | 744 | angle_tick = (servo_table[port][servo].zero_tick + angle_sec * |
745 | servo_table[port][servo].delta_tick / |
||
542 | giacomo | 746 | (servo_table[port][servo].max_angle_sec - servo_table[port][servo].min_angle_sec)) * 1000 / TICK_LEN; |
290 | giacomo | 747 | |
717 | giacomo | 748 | b[0] = 0x00 | (servo & 0x0F); |
749 | b[1] = 0x00 | ((angle_tick >> 8) & 0x0F); |
||
750 | b[2] = 0x00 | (angle_tick & 0xFF); |
||
730 | giacomo | 751 | res = servo_send_msg(servo_port, b, 3, NULL, 0); |
358 | giacomo | 752 | |
730 | giacomo | 753 | return res; |
282 | giacomo | 754 | |
281 | giacomo | 755 | } |
756 | |||
323 | giacomo | 757 | /* 0010.Pppp */ |
358 | giacomo | 758 | int servo_store_default_position(int port, int servo) |
290 | giacomo | 759 | { |
724 | giacomo | 760 | unsigned char b[1]; |
730 | giacomo | 761 | int res = 0; |
358 | giacomo | 762 | int servo_port = (unsigned)(port); |
290 | giacomo | 763 | |
323 | giacomo | 764 | if (servo > 15) return -1; |
358 | giacomo | 765 | |
724 | giacomo | 766 | b[0] = 0x20 | (servo & 0x0F); |
730 | giacomo | 767 | res = servo_send_msg(servo_port, b, 1, NULL, 0); |
358 | giacomo | 768 | |
730 | giacomo | 769 | return res; |
358 | giacomo | 770 | |
290 | giacomo | 771 | } |
772 | |||
323 | giacomo | 773 | /* 0001.Pppp */ |
358 | giacomo | 774 | int servo_get_angle_sec(int port, int servo) |
281 | giacomo | 775 | { |
724 | giacomo | 776 | unsigned char b[1],r[2]; |
733 | giacomo | 777 | int res,data,RX,t; |
358 | giacomo | 778 | int servo_port = (unsigned)(port); |
779 | |||
323 | giacomo | 780 | if (servo > 15) return -1; |
358 | giacomo | 781 | |
724 | giacomo | 782 | b[0] = 0x10 | (servo & 0x0F); |
725 | giacomo | 783 | RX = servo_send_msg(servo_port, b, 1, r, 2); |
281 | giacomo | 784 | |
730 | giacomo | 785 | if (RX != -1) { |
725 | giacomo | 786 | |
733 | giacomo | 787 | t = servo_wait(servo_port, RX); |
282 | giacomo | 788 | |
733 | giacomo | 789 | if (t == 0) { |
290 | giacomo | 790 | |
730 | giacomo | 791 | res = (int)(r[0]) << 8; |
792 | res |= r[1]; |
||
282 | giacomo | 793 | |
730 | giacomo | 794 | data = ((res*TICK_LEN/1000) - servo_table[port][servo].zero_tick) * |
795 | (servo_table[port][servo].max_angle_sec - servo_table[port][servo].min_angle_sec) / |
||
796 | servo_table[port][servo].delta_tick; |
||
797 | |||
798 | return data; |
||
799 | |||
800 | } else return -1; |
||
801 | |||
802 | } else return -1; |
||
803 | |||
281 | giacomo | 804 | } |
805 | |||
772 | mauro | 806 | /* 0100:baaa */ |
807 | int servo_get_analog(int port, int adport, int nbit) |
||
281 | giacomo | 808 | { |
724 | giacomo | 809 | unsigned char b[1],r[2]; |
772 | mauro | 810 | int res, RX, t, nb; |
358 | giacomo | 811 | int servo_port = (unsigned)(port); |
812 | |||
381 | giacomo | 813 | if (adport > 7) return -1; |
772 | mauro | 814 | nb = (nbit==8) ? 0 : 1; |
358 | giacomo | 815 | |
724 | giacomo | 816 | b[0] = 0x40 | (adport & 0x07); |
772 | mauro | 817 | if (nb) { |
818 | RX = servo_send_msg(servo_port, b, 1, r, 2); |
||
819 | } else { |
||
820 | b[0] |= 0x08; |
||
821 | RX = servo_send_msg(servo_port, b, 1, r, 1); |
||
822 | } |
||
281 | giacomo | 823 | |
732 | giacomo | 824 | if (RX != -1) { |
725 | giacomo | 825 | |
733 | giacomo | 826 | t = servo_wait(servo_port, RX); |
282 | giacomo | 827 | |
733 | giacomo | 828 | if (t == 0) { |
282 | giacomo | 829 | |
772 | mauro | 830 | if (nb) { |
831 | res = (int)r[0] << 8; |
||
832 | res |= r[1]; |
||
833 | } else { |
||
834 | res = r[0]; |
||
835 | } |
||
732 | giacomo | 836 | |
837 | return res; |
||
838 | |||
839 | } else return -1; |
||
840 | |||
841 | } else return -1; |
||
842 | |||
281 | giacomo | 843 | } |
844 |