Rev 733 | Rev 735 | 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> |
||
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 | |
358 | giacomo | 46 | //#define SERVO_DEBUG |
286 | giacomo | 47 | |
398 | giacomo | 48 | #define SERVO_SPEED 19200 |
286 | giacomo | 49 | #define SERVO_PARITY NONE |
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 | |
734 | giacomo | 131 | #define SERVO_TIMEOUT 20000 |
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 | |||
721 | giacomo | 158 | void servo_rx_error(unsigned port, unsigned type) { |
292 | giacomo | 159 | |
734 | giacomo | 160 | #ifdef SERVO_DEBUG |
161 | kern_printf("(SERVO: BUFFER ERROR PORT:%d ERR:%d)",port,type); |
||
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) { |
169 | if (data != RXTX_buff[port][RX_position[port]]) |
||
721 | giacomo | 170 | servo_rx_error(port,1); |
726 | giacomo | 171 | |
733 | giacomo | 172 | RXTX_addr[port][RX_position[port]] = SERVO_SKIP; |
173 | |||
717 | giacomo | 174 | RX_position[port]++; |
721 | giacomo | 175 | |
176 | if (RX_position[port] >= RXTX_BUFF_MAX) { |
||
730 | giacomo | 177 | RX_cycle[port]++; |
721 | giacomo | 178 | RX_position[port] = 0; |
179 | } |
||
730 | giacomo | 180 | if ((RX_cycle[port] == TX_cycle[port] && RX_position[port] > TX_position[port]) || |
181 | (RX_cycle[port] > TX_cycle[port])) |
||
721 | giacomo | 182 | servo_rx_error(port,2); |
183 | |||
717 | giacomo | 184 | } else { |
726 | giacomo | 185 | |
733 | giacomo | 186 | if (RXTX_addr[port][RX_position[port]] != SERVO_SKIP) { |
721 | giacomo | 187 | |
733 | giacomo | 188 | *RXTX_addr[port][RX_position[port]] = data; |
189 | RXTX_buff[port][RX_position[port]] = SERVO_UNLOCK; //Unlock the data |
||
190 | |||
191 | RXTX_addr[port][RX_position[port]] = SERVO_SKIP; |
||
192 | |||
193 | RX_position[port]++; |
||
194 | |||
195 | if (RX_position[port] >= RXTX_BUFF_MAX) { |
||
730 | giacomo | 196 | RX_cycle[port]++; |
721 | giacomo | 197 | RX_position[port] = 0; |
733 | giacomo | 198 | } |
199 | if ((RX_cycle[port] == TX_cycle[port] && RX_position[port] > TX_position[port]) || |
||
200 | (RX_cycle[port] > TX_cycle[port])) |
||
201 | servo_rx_error(port,3); |
||
202 | |||
203 | } else { |
||
204 | |||
205 | RX_position[port]++; |
||
206 | |||
207 | if (RX_position[port] >= RXTX_BUFF_MAX) { |
||
208 | RX_cycle[port]++; |
||
209 | RX_position[port] = 0; |
||
210 | } |
||
211 | if ((RX_cycle[port] == TX_cycle[port] && RX_position[port] > TX_position[port]) || |
||
212 | (RX_cycle[port] > TX_cycle[port])) |
||
213 | servo_rx_error(port,3); |
||
214 | |||
215 | servo_indication(port, data); |
||
216 | |||
721 | giacomo | 217 | } |
726 | giacomo | 218 | |
717 | giacomo | 219 | } |
295 | giacomo | 220 | |
286 | giacomo | 221 | } |
222 | |||
730 | giacomo | 223 | extern unsigned SCom_Error[4]; |
727 | giacomo | 224 | |
717 | giacomo | 225 | void servo_confirm(unsigned port, BYTE msg_status) { |
286 | giacomo | 226 | |
734 | giacomo | 227 | #ifdef SERVO_DEBUG |
228 | if (msg_status == COM_ERROR) { |
||
229 | kern_printf("(SCOM: PORT:%d ERROR:%d)",port,SCom_Error[port]); |
||
230 | } |
||
231 | #endif |
||
286 | giacomo | 232 | |
730 | giacomo | 233 | lock_write[port] = 0; |
727 | giacomo | 234 | |
717 | giacomo | 235 | } |
236 | |||
237 | int servo_send_msg(unsigned port, BYTE *msg, unsigned len_msg, BYTE *res, unsigned len_res) { |
||
238 | |||
726 | giacomo | 239 | int i = 0, old, oldcycle, RX = 0; |
733 | giacomo | 240 | volatile int start = kern_gettime(NULL), timeout = 0; |
721 | giacomo | 241 | SYS_FLAGS f; |
242 | |||
733 | giacomo | 243 | while(lock_write[port] == 1 && timeout == 0) |
244 | if ((start + SERVO_TIMEOUT) < kern_gettime(NULL)) timeout = 1; |
||
245 | |||
246 | if (timeout == 1) { |
||
734 | giacomo | 247 | |
248 | #ifdef SERVO_DEBUG |
||
249 | kern_printf("(SERVO: TIMEOUT SEND)"); |
||
250 | #endif |
||
251 | |||
733 | giacomo | 252 | lock_write[port] = 0; |
253 | return -1; |
||
254 | } |
||
255 | |||
728 | giacomo | 256 | lock_write[port] = 1; |
727 | giacomo | 257 | |
721 | giacomo | 258 | f = kern_fsave(); |
259 | |||
717 | giacomo | 260 | old = TX_position[port]; |
730 | giacomo | 261 | oldcycle = TX_cycle[port]; |
717 | giacomo | 262 | |
263 | while(i < len_msg) { |
||
264 | RXTX_buff[port][TX_position[port]] = msg[i]; |
||
265 | RXTX_addr[port][TX_position[port]] = NULL; |
||
266 | TX_position[port]++; |
||
721 | giacomo | 267 | if (TX_position[port] >= RXTX_BUFF_MAX) { |
730 | giacomo | 268 | TX_cycle[port]++; |
721 | giacomo | 269 | TX_position[port] = 0; |
270 | } |
||
717 | giacomo | 271 | if (TX_position[port] == RX_position[port]) { |
272 | TX_position[port] = old; |
||
730 | giacomo | 273 | TX_cycle[port] = oldcycle; |
274 | lock_write[port] = 0; |
||
733 | giacomo | 275 | |
734 | giacomo | 276 | #ifdef SERVO_DEBUG |
277 | kern_printf("(SERVO: BUFFER FULL)"); |
||
278 | #endif |
||
733 | giacomo | 279 | |
721 | giacomo | 280 | kern_frestore(f); |
717 | giacomo | 281 | return -1; |
282 | } |
||
283 | i++; |
||
284 | } |
||
285 | |||
286 | i = 0; |
||
287 | while(i < len_res) { |
||
733 | giacomo | 288 | RXTX_buff[port][TX_position[port]] = SERVO_LOCK; //Lock the data |
717 | giacomo | 289 | RXTX_addr[port][TX_position[port]] = res+i; |
726 | giacomo | 290 | RX = TX_position[port]; |
717 | giacomo | 291 | TX_position[port]++; |
721 | giacomo | 292 | if (TX_position[port] >= RXTX_BUFF_MAX) { |
730 | giacomo | 293 | TX_cycle[port]++; |
721 | giacomo | 294 | TX_position[port] = 0; |
295 | } |
||
717 | giacomo | 296 | if (TX_position[port] == RX_position[port]) { |
297 | TX_position[port] = old; |
||
730 | giacomo | 298 | TX_cycle[port] = oldcycle; |
299 | lock_write[port] = 0; |
||
733 | giacomo | 300 | |
734 | giacomo | 301 | #ifdef SERVO_DEBUG |
302 | kern_printf("(SERVO: BUFFER FULL)"); |
||
303 | #endif |
||
733 | giacomo | 304 | |
721 | giacomo | 305 | kern_frestore(f); |
717 | giacomo | 306 | return -1; |
307 | } |
||
308 | i++; |
||
309 | } |
||
310 | |||
721 | giacomo | 311 | kern_frestore(f); |
312 | |||
717 | giacomo | 313 | com_irq_send(port, len_msg, msg); |
314 | |||
725 | giacomo | 315 | return RX; |
316 | |||
317 | } |
||
318 | |||
733 | giacomo | 319 | int servo_wait(unsigned port, int RX) { |
730 | giacomo | 320 | |
733 | giacomo | 321 | volatile int start = kern_gettime(NULL), timeout = 0, temp; |
730 | giacomo | 322 | |
733 | giacomo | 323 | /* Active wait until number bytes received */ |
324 | while(RXTX_buff[port][RX] == 0 && timeout == 0) |
||
325 | if ((start + SERVO_TIMEOUT) < kern_gettime(NULL)) timeout = 1; |
||
730 | giacomo | 326 | |
733 | giacomo | 327 | if (timeout == 1) { |
328 | SYS_FLAGS f; |
||
730 | giacomo | 329 | |
733 | giacomo | 330 | f = kern_fsave(); |
725 | giacomo | 331 | |
734 | giacomo | 332 | #ifdef SERVO_DEBUG |
333 | kern_printf("(SERVO: TIMEOUT GET)"); |
||
334 | #endif |
||
335 | |||
733 | giacomo | 336 | temp = RX; |
727 | giacomo | 337 | |
733 | giacomo | 338 | while(RXTX_addr[port][temp] != SERVO_SKIP) { |
339 | RXTX_addr[port][temp] = SERVO_SKIP; |
||
340 | temp--; |
||
341 | } |
||
730 | giacomo | 342 | |
733 | giacomo | 343 | kern_frestore(f); |
725 | giacomo | 344 | |
733 | giacomo | 345 | } |
730 | giacomo | 346 | |
733 | giacomo | 347 | return timeout; |
286 | giacomo | 348 | |
349 | } |
||
350 | |||
358 | giacomo | 351 | int servo_open(int port,int speed) |
286 | giacomo | 352 | { |
353 | int err; |
||
354 | |||
358 | giacomo | 355 | err = com_open((unsigned)(port), (DWORD)speed, SERVO_PARITY, SERVO_LEN, SERVO_STOP); |
356 | |||
717 | giacomo | 357 | com_init_irq((unsigned)(port)); |
358 | |||
359 | com_set_functions(servo_confirm,servo_indication); |
||
360 | |||
721 | giacomo | 361 | com_irq_enable((unsigned)(port),ALL_IRQ); |
362 | |||
286 | giacomo | 363 | return err; |
364 | |||
365 | } |
||
366 | |||
358 | giacomo | 367 | int servo_close(int port) |
286 | giacomo | 368 | { |
369 | int err; |
||
370 | |||
724 | giacomo | 371 | com_irq_disable((unsigned)(port),ALL_IRQ); |
372 | |||
717 | giacomo | 373 | com_close_irq((unsigned)(port)); |
374 | |||
358 | giacomo | 375 | err = com_close((unsigned)(port)); |
286 | giacomo | 376 | |
377 | return err; |
||
378 | |||
379 | } |
||
380 | |||
381 | /* 1000.011w:bbbb.bbbb */ |
||
358 | giacomo | 382 | int servo_set_RS232_baudrate(int port, int baud) |
281 | giacomo | 383 | { |
724 | giacomo | 384 | unsigned char b[2]; |
385 | int spbrg_temp, i; |
||
286 | giacomo | 386 | unsigned char spbrg, w; |
358 | giacomo | 387 | int servo_port = (unsigned)(port); |
286 | giacomo | 388 | |
389 | i = 0; |
||
390 | while(BaudTable[i] != baud && BaudTable[i] != -1) i++; |
||
391 | if (BaudTable[i] == -1) { |
||
392 | kern_printf("SERVO:Error wrong baud rate\n"); |
||
393 | return -1; |
||
394 | } |
||
395 | |||
358 | giacomo | 396 | w = 1; |
453 | giacomo | 397 | spbrg_temp = (SERVO_CLOCK*10 / (16*baud)) - 10; |
412 | giacomo | 398 | if (spbrg_temp>2550) { |
358 | giacomo | 399 | w = 0; |
453 | giacomo | 400 | spbrg_temp = (SERVO_CLOCK*10 / (64*baud)) - 10; |
286 | giacomo | 401 | } |
412 | giacomo | 402 | spbrg = spbrg_temp / 10; |
453 | giacomo | 403 | if (spbrg_temp % 10 > 5) spbrg++; |
412 | giacomo | 404 | |
286 | giacomo | 405 | #ifdef SERVO_DEBUG |
406 | kern_printf("(SERVO:SBPRG %d W %d)",spbrg,w); |
||
358 | giacomo | 407 | #endif |
408 | |||
724 | giacomo | 409 | b[0] = 0x86 | (w & 0x01); |
410 | b[1] = spbrg; |
||
411 | servo_send_msg(servo_port, b, 2, NULL, 0); |
||
358 | giacomo | 412 | |
724 | giacomo | 413 | return 0; |
381 | giacomo | 414 | |
281 | giacomo | 415 | } |
416 | |||
285 | giacomo | 417 | /* 1000.0101 */ |
358 | giacomo | 418 | int servo_get_RS232_baudrate(int port) |
281 | giacomo | 419 | { |
724 | giacomo | 420 | unsigned char b[1],r[2]; |
733 | giacomo | 421 | int res, res_w, res_b, t; |
358 | giacomo | 422 | int servo_port = (unsigned)(port); |
725 | giacomo | 423 | int RX; |
358 | giacomo | 424 | |
724 | giacomo | 425 | b[0] = 0x85; |
725 | giacomo | 426 | RX = servo_send_msg(servo_port, b, 1, r, 2); |
358 | giacomo | 427 | |
732 | giacomo | 428 | if (RX != -1) { |
429 | |||
733 | giacomo | 430 | t = servo_wait(servo_port, RX); |
732 | giacomo | 431 | |
733 | giacomo | 432 | if (t == 0) { |
725 | giacomo | 433 | |
732 | giacomo | 434 | res_w = r[0]; /* bit W */ |
435 | res_b = r[1]; /* byte SPBRG */ |
||
465 | giacomo | 436 | |
732 | giacomo | 437 | if (res_w != -1 && res_b != -1) { |
438 | if (res_w) |
||
439 | res = SERVO_CLOCK / ( 16 * (res_b + 1) ); |
||
440 | else |
||
441 | res = SERVO_CLOCK / ( 64 * (res_b + 1) ); |
||
442 | } else { |
||
443 | return -1; |
||
444 | } |
||
358 | giacomo | 445 | |
732 | giacomo | 446 | return res; |
358 | giacomo | 447 | |
732 | giacomo | 448 | } else return -1; |
449 | |||
450 | } else return -1; |
||
451 | |||
281 | giacomo | 452 | } |
453 | |||
286 | giacomo | 454 | /* 1000.0100 */ |
358 | giacomo | 455 | int servo_store_RS232_baudrate(int port) |
281 | giacomo | 456 | { |
724 | giacomo | 457 | unsigned char b[1]; |
358 | giacomo | 458 | int servo_port = (unsigned)(port); |
459 | |||
724 | giacomo | 460 | b[0] = 0x84; |
461 | servo_send_msg(servo_port, b, 1, NULL, 0); |
||
462 | |||
463 | return 0; |
||
358 | giacomo | 464 | |
281 | giacomo | 465 | } |
466 | |||
289 | giacomo | 467 | /* 1000.1010:llll.llll */ |
358 | giacomo | 468 | int servo_set_period(int port, int period) |
281 | giacomo | 469 | { |
724 | giacomo | 470 | unsigned char b[2]; |
358 | giacomo | 471 | int servo_port = (unsigned)(port); |
472 | |||
724 | giacomo | 473 | b[0] = 0x8A; |
474 | b[1] = (period*1000)/TICK_LEN_PERIOD/8 & 0xFF; |
||
475 | servo_send_msg(servo_port, b, 2, NULL, 0); |
||
381 | giacomo | 476 | |
724 | giacomo | 477 | return 0; |
358 | giacomo | 478 | |
281 | giacomo | 479 | } |
480 | |||
285 | giacomo | 481 | /* 1000.1001 */ |
358 | giacomo | 482 | int servo_get_period(int port) |
281 | giacomo | 483 | { |
724 | giacomo | 484 | unsigned char b[1],r[1]; |
733 | giacomo | 485 | int res, RX, t; |
358 | giacomo | 486 | int servo_port = (unsigned)(port); |
487 | |||
724 | giacomo | 488 | b[0] = 0x89; |
725 | giacomo | 489 | RX = servo_send_msg(servo_port, b, 1, r, 1); |
358 | giacomo | 490 | |
732 | giacomo | 491 | if (RX != -1) { |
492 | |||
733 | giacomo | 493 | t = servo_wait(servo_port, RX); |
732 | giacomo | 494 | |
733 | giacomo | 495 | if (t == 0) { |
725 | giacomo | 496 | |
732 | giacomo | 497 | res = r[0]; |
381 | giacomo | 498 | |
732 | giacomo | 499 | return (((unsigned char)(res))*TICK_LEN_PERIOD/1000*8); |
358 | giacomo | 500 | |
732 | giacomo | 501 | } else return -1; |
502 | |||
503 | } else return -1; |
||
504 | |||
281 | giacomo | 505 | } |
506 | |||
289 | giacomo | 507 | /* 1000.1000 */ |
358 | giacomo | 508 | int servo_store_period(int port) |
281 | giacomo | 509 | { |
724 | giacomo | 510 | unsigned char b[1]; |
358 | giacomo | 511 | int servo_port = (unsigned)(port); |
512 | |||
724 | giacomo | 513 | b[0] = 0x88; |
514 | servo_send_msg(servo_port, b, 1, NULL, 0); |
||
358 | giacomo | 515 | |
724 | giacomo | 516 | return 0; |
381 | giacomo | 517 | |
281 | giacomo | 518 | } |
519 | |||
283 | giacomo | 520 | /* 1000.1100 */ |
358 | giacomo | 521 | int servo_get_setup_switch(int port) |
281 | giacomo | 522 | { |
724 | giacomo | 523 | unsigned char b[1],r[1]; |
733 | giacomo | 524 | int res, RX, t; |
358 | giacomo | 525 | int servo_port = (unsigned)(port); |
526 | |||
724 | giacomo | 527 | b[0] = 0x8C; |
725 | giacomo | 528 | RX = servo_send_msg(servo_port, b, 1, r, 1); |
281 | giacomo | 529 | |
732 | giacomo | 530 | if (RX != -1) { |
531 | |||
733 | giacomo | 532 | t = servo_wait(servo_port, RX); |
732 | giacomo | 533 | |
733 | giacomo | 534 | if (t == 0) { |
725 | giacomo | 535 | |
732 | giacomo | 536 | res = r[0]; |
281 | giacomo | 537 | |
732 | giacomo | 538 | return res; |
283 | giacomo | 539 | |
732 | giacomo | 540 | } else return -1; |
541 | |||
542 | } else return -1; |
||
543 | |||
281 | giacomo | 544 | } |
545 | |||
283 | giacomo | 546 | /* 1000.111s */ |
358 | giacomo | 547 | int servo_set_RC5_switch(int port, int data) |
281 | giacomo | 548 | { |
724 | giacomo | 549 | unsigned char b[1]; |
358 | giacomo | 550 | int servo_port = (unsigned)(port); |
551 | |||
724 | giacomo | 552 | b[0] = 0x8E | (data & 0x01); |
553 | servo_send_msg(servo_port, b, 1, NULL, 0); |
||
281 | giacomo | 554 | |
724 | giacomo | 555 | return 0; |
381 | giacomo | 556 | |
281 | giacomo | 557 | } |
558 | |||
323 | giacomo | 559 | /* 1000.0000:0000.Mmmm */ |
358 | giacomo | 560 | int servo_turn_off(int port, int servo) |
285 | giacomo | 561 | { |
724 | giacomo | 562 | unsigned char b[2]; |
358 | giacomo | 563 | int servo_port = (unsigned)(port); |
564 | |||
565 | if (servo > 15) return -1; |
||
724 | giacomo | 566 | |
567 | b[0] = 0x80; |
||
568 | b[1] = 0x00 | (servo & 0x0F); |
||
569 | servo_send_msg(servo_port, b, 2, NULL, 0); |
||
381 | giacomo | 570 | |
724 | giacomo | 571 | return 0; |
285 | giacomo | 572 | |
573 | } |
||
574 | |||
325 | giacomo | 575 | /* 1000.0000:0001.Mmmm */ |
358 | giacomo | 576 | int servo_turn_on(int port, int servo) |
285 | giacomo | 577 | { |
721 | giacomo | 578 | unsigned char b[2]; |
358 | giacomo | 579 | int servo_port = (unsigned)(port); |
580 | |||
323 | giacomo | 581 | if (servo > 15) return -1; |
358 | giacomo | 582 | |
721 | giacomo | 583 | b[0] = 0x80; |
584 | b[1] = 0x10 | (servo & 0x0F); |
||
585 | servo_send_msg(servo_port, b, 2, NULL, 0); |
||
358 | giacomo | 586 | |
721 | giacomo | 587 | return 0; |
358 | giacomo | 588 | |
285 | giacomo | 589 | } |
590 | |||
291 | giacomo | 591 | /* 1000.0000:0010.0000 */ |
358 | giacomo | 592 | int servo_turn_off_all(int port) |
323 | giacomo | 593 | { |
724 | giacomo | 594 | unsigned char b[2]; |
358 | giacomo | 595 | int servo_port = (unsigned)(port); |
596 | |||
724 | giacomo | 597 | b[0] = 0x80; |
598 | b[1] = 0x20; |
||
599 | servo_send_msg(servo_port, b, 2, NULL, 0); |
||
358 | giacomo | 600 | |
724 | giacomo | 601 | return 0; |
358 | giacomo | 602 | |
285 | giacomo | 603 | } |
604 | |||
291 | giacomo | 605 | /* 1000.0000:0010.0001 */ |
358 | giacomo | 606 | int servo_turn_on_all(int port) |
285 | giacomo | 607 | { |
724 | giacomo | 608 | unsigned char b[2]; |
358 | giacomo | 609 | int servo_port = (unsigned)(port); |
610 | |||
724 | giacomo | 611 | b[0] = 0x80; |
612 | b[1] = 0x21; |
||
613 | servo_send_msg(servo_port, b, 2, NULL, 0); |
||
614 | |||
615 | return 0; |
||
358 | giacomo | 616 | |
285 | giacomo | 617 | } |
618 | |||
323 | giacomo | 619 | /* 1000.0000:0101.000M:mmmm.mmmm */ |
724 | giacomo | 620 | int servo_set_levels(int port, int bank, int mask) |
315 | giacomo | 621 | { |
724 | giacomo | 622 | unsigned char b[3]; |
358 | giacomo | 623 | int servo_port = (unsigned)(port); |
624 | |||
724 | giacomo | 625 | b[0] = 0x80; |
626 | b[1] = 0x50 | (0x01 & bank); |
||
627 | b[2] = (unsigned char)(mask & 0xFF); |
||
628 | servo_send_msg(servo_port, b, 3, NULL, 0); |
||
358 | giacomo | 629 | |
724 | giacomo | 630 | return 0; |
358 | giacomo | 631 | |
315 | giacomo | 632 | } |
633 | |||
323 | giacomo | 634 | /* 1000.0000:0100.000M */ |
358 | giacomo | 635 | int servo_get_levels(int port, int bank) |
297 | giacomo | 636 | { |
724 | giacomo | 637 | unsigned char b[2],r[1]; |
733 | giacomo | 638 | int res, RX, t; |
358 | giacomo | 639 | int servo_port = (unsigned)(port); |
640 | |||
724 | giacomo | 641 | b[0] = 0x80; |
642 | b[1] = 0x40 | (0x01 & bank); |
||
725 | giacomo | 643 | RX = servo_send_msg(servo_port, b, 2, r, 1); |
644 | |||
732 | giacomo | 645 | if (RX != -1) { |
646 | |||
733 | giacomo | 647 | t = servo_wait(servo_port, RX); |
732 | giacomo | 648 | |
733 | giacomo | 649 | if (t == 0) { |
358 | giacomo | 650 | |
732 | giacomo | 651 | res = r[0]; |
358 | giacomo | 652 | |
732 | giacomo | 653 | return res; |
654 | |||
655 | } else return -1; |
||
656 | |||
657 | } else return -1; |
||
658 | |||
297 | giacomo | 659 | } |
660 | |||
661 | /* 1000.0000:1000.0000 */ |
||
358 | giacomo | 662 | int servo_store_levels(int port) |
297 | giacomo | 663 | { |
724 | giacomo | 664 | unsigned char b[2]; |
358 | giacomo | 665 | int servo_port = (unsigned)(port); |
666 | |||
724 | giacomo | 667 | b[0] = 0x80; |
668 | b[1] = 0x80; |
||
669 | servo_send_msg(servo_port, b, 2, NULL, 0); |
||
381 | giacomo | 670 | |
724 | giacomo | 671 | return 0; |
358 | giacomo | 672 | |
297 | giacomo | 673 | } |
674 | |||
493 | giacomo | 675 | int servo_set_max_angle_sec(int port, int servo, int angle_sec) |
290 | giacomo | 676 | { |
677 | |||
381 | giacomo | 678 | servo_table[port][servo].max_angle_sec = angle_sec; |
290 | giacomo | 679 | return 0; |
680 | |||
681 | } |
||
682 | |||
493 | giacomo | 683 | int servo_set_min_angle_sec(int port, int servo, int angle_sec) |
290 | giacomo | 684 | { |
685 | |||
381 | giacomo | 686 | servo_table[port][servo].min_angle_sec = angle_sec; |
290 | giacomo | 687 | return 0; |
688 | |||
689 | } |
||
690 | |||
381 | giacomo | 691 | int servo_set_servo_tick(int port, int servo, int zero_tick, int delta_tick) |
692 | { |
||
693 | |||
694 | if (zero_tick != -1) servo_table[port][servo].zero_tick = zero_tick; |
||
695 | if (delta_tick != -1) servo_table[port][servo].delta_tick = delta_tick; |
||
696 | return 0; |
||
697 | |||
698 | } |
||
699 | |||
323 | giacomo | 700 | /* 0000.Pppp:0000.vvvv:vvvv.vvvv */ |
358 | giacomo | 701 | int servo_set_angle_sec(int port, int servo, int angle_sec) |
281 | giacomo | 702 | { |
717 | giacomo | 703 | unsigned char b[3]; |
730 | giacomo | 704 | int angle_tick, res = 0; |
358 | giacomo | 705 | int servo_port = (unsigned)(port); |
282 | giacomo | 706 | |
323 | giacomo | 707 | if (servo > 15) return -1; |
282 | giacomo | 708 | |
381 | giacomo | 709 | angle_tick = (servo_table[port][servo].zero_tick + angle_sec * |
710 | servo_table[port][servo].delta_tick / |
||
542 | giacomo | 711 | (servo_table[port][servo].max_angle_sec - servo_table[port][servo].min_angle_sec)) * 1000 / TICK_LEN; |
290 | giacomo | 712 | |
717 | giacomo | 713 | b[0] = 0x00 | (servo & 0x0F); |
714 | b[1] = 0x00 | ((angle_tick >> 8) & 0x0F); |
||
715 | b[2] = 0x00 | (angle_tick & 0xFF); |
||
730 | giacomo | 716 | res = servo_send_msg(servo_port, b, 3, NULL, 0); |
358 | giacomo | 717 | |
730 | giacomo | 718 | return res; |
282 | giacomo | 719 | |
281 | giacomo | 720 | } |
721 | |||
323 | giacomo | 722 | /* 0010.Pppp */ |
358 | giacomo | 723 | int servo_store_default_position(int port, int servo) |
290 | giacomo | 724 | { |
724 | giacomo | 725 | unsigned char b[1]; |
730 | giacomo | 726 | int res = 0; |
358 | giacomo | 727 | int servo_port = (unsigned)(port); |
290 | giacomo | 728 | |
323 | giacomo | 729 | if (servo > 15) return -1; |
358 | giacomo | 730 | |
724 | giacomo | 731 | b[0] = 0x20 | (servo & 0x0F); |
730 | giacomo | 732 | res = servo_send_msg(servo_port, b, 1, NULL, 0); |
358 | giacomo | 733 | |
730 | giacomo | 734 | return res; |
358 | giacomo | 735 | |
290 | giacomo | 736 | } |
737 | |||
323 | giacomo | 738 | /* 0001.Pppp */ |
358 | giacomo | 739 | int servo_get_angle_sec(int port, int servo) |
281 | giacomo | 740 | { |
724 | giacomo | 741 | unsigned char b[1],r[2]; |
733 | giacomo | 742 | int res,data,RX,t; |
358 | giacomo | 743 | int servo_port = (unsigned)(port); |
744 | |||
323 | giacomo | 745 | if (servo > 15) return -1; |
358 | giacomo | 746 | |
724 | giacomo | 747 | b[0] = 0x10 | (servo & 0x0F); |
725 | giacomo | 748 | RX = servo_send_msg(servo_port, b, 1, r, 2); |
281 | giacomo | 749 | |
730 | giacomo | 750 | if (RX != -1) { |
725 | giacomo | 751 | |
733 | giacomo | 752 | t = servo_wait(servo_port, RX); |
282 | giacomo | 753 | |
733 | giacomo | 754 | if (t == 0) { |
290 | giacomo | 755 | |
730 | giacomo | 756 | res = (int)(r[0]) << 8; |
757 | res |= r[1]; |
||
282 | giacomo | 758 | |
730 | giacomo | 759 | data = ((res*TICK_LEN/1000) - servo_table[port][servo].zero_tick) * |
760 | (servo_table[port][servo].max_angle_sec - servo_table[port][servo].min_angle_sec) / |
||
761 | servo_table[port][servo].delta_tick; |
||
762 | |||
763 | return data; |
||
764 | |||
765 | } else return -1; |
||
766 | |||
767 | } else return -1; |
||
768 | |||
281 | giacomo | 769 | } |
770 | |||
282 | giacomo | 771 | /* 0100:0aaa */ |
358 | giacomo | 772 | int servo_get_analog(int port, int adport) |
281 | giacomo | 773 | { |
724 | giacomo | 774 | unsigned char b[1],r[2]; |
733 | giacomo | 775 | int res, RX, t; |
358 | giacomo | 776 | int servo_port = (unsigned)(port); |
777 | |||
381 | giacomo | 778 | if (adport > 7) return -1; |
358 | giacomo | 779 | |
724 | giacomo | 780 | b[0] = 0x40 | (adport & 0x07); |
725 | giacomo | 781 | RX = servo_send_msg(servo_port, b, 1, r, 2); |
281 | giacomo | 782 | |
732 | giacomo | 783 | if (RX != -1) { |
725 | giacomo | 784 | |
733 | giacomo | 785 | t = servo_wait(servo_port, RX); |
282 | giacomo | 786 | |
733 | giacomo | 787 | if (t == 0) { |
282 | giacomo | 788 | |
732 | giacomo | 789 | res = (int)r[0] << 8; |
790 | res |= r[1]; |
||
791 | |||
792 | return res; |
||
793 | |||
794 | } else return -1; |
||
795 | |||
796 | } else return -1; |
||
797 | |||
281 | giacomo | 798 | } |
799 |