Subversion Repositories shark

Rev

Rev 738 | Rev 771 | 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 
 
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 
 
738 giacomo 131 
#define SERVO_TIMEOUT 1000000
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 
 
394 
  com_set_functions(servo_confirm,servo_indication);
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 
 
282 giacomo 806 
/* 0100:0aaa */
358 giacomo 807 
int servo_get_analog(int port, int adport)
281 giacomo 808 
{
724 giacomo 809 
  unsigned char b[1],r[2];
733 giacomo 810 
  int res, RX, t;
358 giacomo 811 
  int servo_port = (unsigned)(port);
812 
 
381 giacomo 813 
  if (adport > 7) return -1;
358 giacomo 814 
 
724 giacomo 815 
  b[0] = 0x40 | (adport & 0x07);
725 giacomo 816 
  RX = servo_send_msg(servo_port, b, 1, r, 2);
281 giacomo 817 
 
732 giacomo 818 
  if (RX != -1) {
725 giacomo 819 
 
733 giacomo 820 
    t = servo_wait(servo_port, RX);
282 giacomo 821 
 
733 giacomo 822 
    if (t == 0) {
282 giacomo 823 
 
732 giacomo 824 
      res = (int)r[0] << 8;
825 
      res |= r[1];
826 
 
827 
      return res;
828 
 
829 
    } else return -1;
830 
 
831 
  } else return -1;
832 
 
281 giacomo 833 
}
834