Subversion Repositories shark

Rev

Rev 750 | 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
 
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
 
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