Subversion Repositories shark

Rev

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