Subversion Repositories shark

/*
 * Project: S.Ha.R.K.
 *
 * Coordinators:
 *   Giorgio Buttazzo    <giorgio@sssup.it>
 *   Paolo Gai           <pj@gandalf.sssup.it>
 *
 * Authors     :
 *   Giacomo Guidi       <giacomo@gandalf.sssup.it>
 *   Mauro Marinoni      <mauro.marinoni@unipv.it>
 *   (see the web pages for full authors list)
 *
 * ReTiS Lab (Scuola Superiore S.Anna - Pisa - Italy)
 *
 * http://www.sssup.it
 * http://retis.sssup.it
 * http://shark.sssup.it
 */

/*
 * Copyright (C) 2002 Paolo Gai
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */

#include <ll/sys/ll/ll-instr.h>
#include "kernel/kern.h"

#include "drivers/scom.h"
#include "drivers/scomirq.h"

#include "servo.h"

//#define SERVO_DEBUG
//#define SERVO_TIMEOUT_EVENT

#define SERVO_TIMEOUT 200000 /* us */

#define SERVO_SPEED 19200
#define SERVO_PARITY NONE
#define SERVO_LEN 8
#define SERVO_STOP 1

#define SERVO_CLOCK 20000000 /* 20MHz */

#define TICK_LEN 1600 /* ns */
#define TICK_LEN_PERIOD 51200 /* ns */

struct servo_data {
  int min_angle_sec;
  int max_angle_sec;
  int delta_tick;
  int zero_tick;
};

struct servo_data servo_table[4][16] = {
  {{-324000, 324000, 1200, 1600},
  {-324000, 324000, 1200, 1600},
  {-324000, 324000, 1200, 1600},
  {-324000, 324000, 1200, 1600},
  {-324000, 324000, 1200, 1600},
  {-324000, 324000, 1200, 1600},
  {-324000, 324000, 1200, 1600},
  {-324000, 324000, 1200, 1600},
  {-324000, 324000, 1200, 1600},
  {-324000, 324000, 1200, 1600},
  {-324000, 324000, 1200, 1600},
  {-324000, 324000, 1200, 1600},
  {-324000, 324000, 1200, 1600},
  {-324000, 324000, 1200, 1600},
  {-324000, 324000, 1200, 1600},
  {-324000, 324000, 1200, 1600}},
  {{-324000, 324000, 1200, 1600},
  {-324000, 324000, 1200, 1600},
  {-324000, 324000, 1200, 1600},
  {-324000, 324000, 1200, 1600},
  {-324000, 324000, 1200, 1600},
  {-324000, 324000, 1200, 1600},
  {-324000, 324000, 1200, 1600},
  {-324000, 324000, 1200, 1600},
  {-324000, 324000, 1200, 1600},
  {-324000, 324000, 1200, 1600},
  {-324000, 324000, 1200, 1600},
  {-324000, 324000, 1200, 1600},
  {-324000, 324000, 1200, 1600},
  {-324000, 324000, 1200, 1600},
  {-324000, 324000, 1200, 1600},
  {-324000, 324000, 1200, 1600}},
  {{-324000, 324000, 1200, 1600},
  {-324000, 324000, 1200, 1600},
  {-324000, 324000, 1200, 1600},
  {-324000, 324000, 1200, 1600},
  {-324000, 324000, 1200, 1600},
  {-324000, 324000, 1200, 1600},
  {-324000, 324000, 1200, 1600},
  {-324000, 324000, 1200, 1600},
  {-324000, 324000, 1200, 1600},
  {-324000, 324000, 1200, 1600},
  {-324000, 324000, 1200, 1600},
  {-324000, 324000, 1200, 1600},
  {-324000, 324000, 1200, 1600},
  {-324000, 324000, 1200, 1600},
  {-324000, 324000, 1200, 1600},
  {-324000, 324000, 1200, 1600}},
  {{-324000, 324000, 1200, 1600},
  {-324000, 324000, 1200, 1600},
  {-324000, 324000, 1200, 1600},
  {-324000, 324000, 1200, 1600},
  {-324000, 324000, 1200, 1600},
  {-324000, 324000, 1200, 1600},
  {-324000, 324000, 1200, 1600},
  {-324000, 324000, 1200, 1600},
  {-324000, 324000, 1200, 1600},
  {-324000, 324000, 1200, 1600},
  {-324000, 324000, 1200, 1600},
  {-324000, 324000, 1200, 1600},
  {-324000, 324000, 1200, 1600},
  {-324000, 324000, 1200, 1600},
  {-324000, 324000, 1200, 1600},
  {-324000, 324000, 1200, 1600}}};

int timer_expired = 0;
int timeout_event;

#define RXTX_BUFF_MAX 100
static BYTE RXTX_buff[4][RXTX_BUFF_MAX];
static BYTE *RXTX_addr[4][RXTX_BUFF_MAX];
unsigned int RX_position[4] = {0,0,0,0};
unsigned int TX_position[4] = {0,0,0,0};

const int BaudTable[] = {
        1200,
        2400,
        4800,
        9600,
        14400,
        19200,
        38400,
        57600,
        115200,
        -1};

void set_timer_expired(void *arg)
{
  timeout_event = NIL;
  timer_expired = 1;
}

void servo_rx_error(unsigned port) {

  kern_printf("SERVO: RX ERROR COM%d\n",port);

}

void servo_indication(unsigned port, BYTE data) {

 if (RXTX_addr[port][RX_position[port]] == NULL) {
   if (data != RXTX_buff[port][RX_position[port]])
     servo_rx_error(port);
   RX_position[port]++;
   if (RX_position[port] >= RXTX_BUFF_MAX) RX_position[port] = 0;
   if (TX_position[port] == RX_position[port])
     servo_rx_error(port);
 } else {
   *RXTX_addr[port][RX_position[port]] = data;
   RX_position[port]++;
   if (RX_position[port] >= RXTX_BUFF_MAX) RX_position[port] = 0;
   if (TX_position[port] == RX_position[port])
     servo_rx_error(port);
 }

 cprintf("(TXRX:%d:%d)",TX_position[port],RX_position[port]);

}

void servo_confirm(unsigned port, BYTE msg_status) {

  #ifdef SERVO_DEBUG
  if (msg_status == COM_ERROR)
    kern_printf("(SERVO:COM%d ERROR)",port);
  #endif

}

int servo_send_msg(unsigned port, BYTE *msg, unsigned len_msg, BYTE *res, unsigned len_res) {

  int i = 0, old;
 
  old = TX_position[port];

  cprintf("(SEND:%d:%d)",len_msg,len_res);

  while(i < len_msg) {
    RXTX_buff[port][TX_position[port]] = msg[i];
    RXTX_addr[port][TX_position[port]] = NULL;
    TX_position[port]++;
    if (TX_position[port] >= RXTX_BUFF_MAX) TX_position[port] = 0;
    if (TX_position[port] == RX_position[port]) {
        TX_position[port] = old;
        cprintf("Error\n");
        return -1;
    }
    i++;
  }

  i = 0;
  while(i < len_res) {
    RXTX_buff[port][TX_position[port]] = 0;
    RXTX_addr[port][TX_position[port]] = res+i;
    TX_position[port]++;
    if (TX_position[port] >= RXTX_BUFF_MAX) TX_position[port] = 0;
    if (TX_position[port] == RX_position[port]) {
        TX_position[port] = old;
        cprintf("Error\n");
        return -1;
    }
    i++;
  }

  com_irq_send(port, len_msg, msg);

  cprintf("(TXRX:%d:%d)",TX_position[port],RX_position[port]);

  return 0;

}

int servo_open(int port,int speed)
{
  int err;

  err = com_open((unsigned)(port), (DWORD)speed, SERVO_PARITY, SERVO_LEN, SERVO_STOP);

  com_init_irq((unsigned)(port));

  com_set_functions(servo_confirm,servo_indication);

  return err;

}

int servo_close(int port)
{
  int err;

  com_close_irq((unsigned)(port));

  err = com_close((unsigned)(port));

  return err;

}

/* 1000.011w:bbbb.bbbb */
int servo_set_RS232_baudrate(int port, int baud)
{
  #ifdef SERVO_TIMEOUT_EVENT
    struct timespec current_time;
  #endif
  unsigned char b;
  int err, spbrg_temp, i;
  unsigned char spbrg, w;
  int servo_port = (unsigned)(port);

  i = 0;
  while(BaudTable[i] != baud && BaudTable[i] != -1) i++;
  if (BaudTable[i] == -1) {
    kern_printf("SERVO:Error wrong baud rate\n");
    return -1;
  }

  w = 1;
  spbrg_temp = (SERVO_CLOCK*10 / (16*baud)) - 10;
  if (spbrg_temp>2550) {
    w = 0;
    spbrg_temp = (SERVO_CLOCK*10 / (64*baud)) - 10;
  }
  spbrg = spbrg_temp / 10;
  if (spbrg_temp % 10 > 5) spbrg++;
 
  #ifdef SERVO_DEBUG
    kern_printf("(SERVO:SBPRG %d W %d)",spbrg,w);
  #endif

  timer_expired = 0;

  #ifdef SERVO_TIMEOUT_EVENT
    kern_gettime(&current_time);
    ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
    timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
  #else
    timeout_event = NIL;
  #endif

  b = 0x86 | (w & 0x01);
  err = com_send(servo_port, b);
  err = com_receive(servo_port);
  if (err != (int)(b)) timer_expired = 1;

  b = spbrg;
  err = com_send(servo_port, b);
  err = com_receive(servo_port);
  if (err != (int)(b)) timer_expired = 1;

  if (timeout_event != NIL) kern_event_delete(timeout_event);

  /*com_close(servo_port);
  com_open(servo_port, baud, SERVO_PARITY, SERVO_LEN, SERVO_STOP);*/

  if (!timer_expired)
    return 0;
  else
    return -1;

}

/* 1000.0101 */
int servo_get_RS232_baudrate(int port)
{
  #ifdef SERVO_TIMEOUT_EVENT
    struct timespec current_time;
  #endif
  unsigned char b;
  int err, res, res_w, res_b;
  int servo_port = (unsigned)(port);

  timer_expired = 0;
 
  #ifdef SERVO_TIMEOUT_EVENT
    kern_gettime(&current_time);
    ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
    timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
  #else
    timeout_event = NIL;
  #endif  

  b = 0x85;
  err = com_send(servo_port, b);
  err = com_receive(servo_port);
  if (err != (int)(b)) timer_expired = 1;
 
  res_w = com_receive(servo_port); /* bit W */
  res_b = com_receive(servo_port); /* byte SPBRG */
 
  if (res_w != -1 && res_b != -1) {
    if (res_w)
      res = SERVO_CLOCK / ( 16 * (res_b + 1) );
    else
      res = SERVO_CLOCK / ( 64 * (res_b + 1) );
  } else {
    return -1;
  }

  if (timeout_event != NIL) kern_event_delete(timeout_event);

  if (!timer_expired)
    return res;
  else
    return -1;

}

/* 1000.0100 */
int servo_store_RS232_baudrate(int port)
{
  #ifdef SERVO_TIMEOUT_EVENT
    struct timespec current_time;
  #endif
  unsigned char b;
  int err;
  int servo_port = (unsigned)(port);

  timer_expired = 0;
  #ifdef SERVO_TIMEOUT_EVENT
    kern_gettime(&current_time);
    ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
    timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
  #else
    timeout_event = NIL;
  #endif
 

  b = 0x84;
  err = com_send(servo_port, b);
  err = com_receive(servo_port);
  if (err != (int)(b)) timer_expired = 1;

  if (timeout_event != NIL) kern_event_delete(timeout_event);

  if (!timer_expired)
    return 0;
  else
    return -1;

}

/* 1000.1010:llll.llll */
int servo_set_period(int port, int period)
{
  #ifdef SERVO_TIMEOUT_EVENT
    struct timespec current_time;
  #endif
  unsigned char b;
  int err;
  int servo_port = (unsigned)(port);

  timer_expired = 0;
  #ifdef SERVO_TIMEOUT_EVENT
    kern_gettime(&current_time);
    ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
    timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
  #else
    timeout_event = NIL;
  #endif
 
  b = 0x8A;
  err = com_send(servo_port, b);
  err = com_receive(servo_port);
  if (err != (int)(b)) timer_expired = 1;

  b = (period*1000)/TICK_LEN_PERIOD/8 & 0xFF;
  err = com_send(servo_port, b);
  err = com_receive(servo_port);
  if (err != (int)(b)) timer_expired = 1;

  if (timeout_event != NIL) kern_event_delete(timeout_event);

  if (!timer_expired)
    return 0;
  else
    return -1;

}

/* 1000.1001 */
int servo_get_period(int port)
{
  #ifdef SERVO_TIMEOUT_EVENT
    struct timespec current_time;
  #endif
  unsigned char b;
  int err,res;
  int servo_port = (unsigned)(port);

  timer_expired = 0;

  #ifdef SERVO_TIMEOUT_EVENT
    kern_gettime(&current_time);
    ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
    timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
  #else
    timeout_event = NIL;
  #endif

  b = 0x89;
  err = com_send(servo_port, b);
  err = com_receive(servo_port);
  if (err != (int)(b)) timer_expired = 1;
  res = com_receive(servo_port);

  if (timeout_event != NIL) kern_event_delete(timeout_event);

  if (!timer_expired)
    return (((unsigned char)(res))*TICK_LEN_PERIOD/1000*8);
  else
    return -1;

}

/* 1000.1000 */
int servo_store_period(int port)
{
  #ifdef SERVO_TIMEOUT_EVENT
    struct timespec current_time;
  #endif
  unsigned char b;
  int err;
  int servo_port = (unsigned)(port);

  timer_expired = 0;

  #ifdef SERVO_TIMEOUT_EVENT
    kern_gettime(&current_time);
    ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
    timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
  #else
    timeout_event = NIL;
  #endif

  b = 0x88;
  err = com_send(servo_port, b);
  err = com_receive(servo_port);
  if (err != (int)(b)) timer_expired = 1;

  if (timeout_event != NIL) kern_event_delete(timeout_event);

  if (!timer_expired)
    return 0;
  else
    return -1;

}

/* 1000.1100 */
int servo_get_setup_switch(int port)
{
  #ifdef SERVO_TIMEOUT_EVENT
    struct timespec current_time;
  #endif
  unsigned char b;
  int err,res;
  int servo_port = (unsigned)(port);

  timer_expired = 0;
 
  #ifdef SERVO_TIMEOUT_EVENT
    kern_gettime(&current_time);
    ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
    timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
  #else
    timeout_event = NIL;
  #endif

  b = 0x8C;
  err = com_send(servo_port, b);
  err = com_receive(servo_port);
  if (err != (int)(b)) timer_expired = 1;
  res = com_receive(servo_port);

  if (timeout_event != NIL) kern_event_delete(timeout_event);

  if (!timer_expired)
    return res;
  else
    return -1;

}

/* 1000.111s */
int servo_set_RC5_switch(int port, int data)
{
  #ifdef SERVO_TIMEOUT_EVENT
    struct timespec current_time;
  #endif
  unsigned char b;
  int err;
  int servo_port = (unsigned)(port);

  timer_expired = 0;

  #ifdef SERVO_TIMEOUT_EVENT
    kern_gettime(&current_time);
    ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
    timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
  #else
    timeout_event = NIL;
  #endif  

  b = 0x8E | (data & 0x01);
  err = com_send(servo_port, b);
  err = com_receive(servo_port);
  if (err != (int)(b)) timer_expired = 1;

  if (timeout_event != NIL) kern_event_delete(timeout_event);

  if (!timer_expired)
    return 0;
  else
    return -1;

}

/* 1000.0000:0000.Mmmm */
int servo_turn_off(int port, int servo)
{

  #ifdef SERVO_TIMEOUT_EVENT
    struct timespec current_time;
  #endif
  unsigned char b;
  int err;
  int servo_port = (unsigned)(port);

  if (servo > 15) return -1;
  timer_expired = 0;
 
  #ifdef SERVO_TIMEOUT_EVENT
    kern_gettime(&current_time);
    ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
    timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
  #else
    timeout_event = NIL;
  #endif

  b = 0x80;
  err = com_send(servo_port, b);
  err = com_receive(servo_port);
  if (err != (int)(b)) timer_expired = 1;

  b = 0x00 | (servo & 0x0F);
  err = com_send(servo_port, b);
  err = com_receive(servo_port);
  if (err != (int)(b)) timer_expired = 1;

  if (timeout_event != NIL) kern_event_delete(timeout_event);

  if (!timer_expired)
    return 0;
  else
    return -1;

}

/* 1000.0000:0001.Mmmm */
int servo_turn_on(int port, int servo)
{

  #ifdef SERVO_TIMEOUT_EVENT
    struct timespec current_time;
  #endif
  unsigned char b;
  int err;
  int servo_port = (unsigned)(port);

  if (servo > 15) return -1;

  timer_expired = 0;
 
  #ifdef SERVO_TIMEOUT_EVENT
    kern_gettime(&current_time);
    ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
    timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
  #else
    timeout_event = NIL;
  #endif

  b = 0x80;
  err = com_send(servo_port, b);
  err = com_receive(servo_port);
  if (err != (int)(b)) timer_expired = 1;

  b = 0x10 | (servo & 0x0F);
  err = com_send(servo_port, b);
  err = com_receive(servo_port);
  if (err != (int)(b)) timer_expired = 1;

  if (timeout_event != NIL) kern_event_delete(timeout_event);

  if (!timer_expired)
    return 0;
  else
    return -1;

}

/* 1000.0000:0010.0000 */
int servo_turn_off_all(int port)
{
  #ifdef SERVO_TIMEOUT_EVENT
    struct timespec current_time;
  #endif
  unsigned char b;
  int err;
  int servo_port = (unsigned)(port);

  timer_expired = 0;
 
  #ifdef SERVO_TIMEOUT_EVENT
    kern_gettime(&current_time);
    ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
    timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
  #else
    timeout_event = NIL;
  #endif  

  b = 0x80;
  err = com_send(servo_port, b);
  err = com_receive(servo_port);
  if (err != (int)(b)) timer_expired = 1;

  b = 0x20;
  err = com_send(servo_port, b);
  err = com_receive(servo_port);
  if (err != (int)(b)) timer_expired = 1;

  if (timeout_event != NIL) kern_event_delete(timeout_event);

  if (!timer_expired)
    return 0;
  else
    return -1;

}

/* 1000.0000:0010.0001 */
int servo_turn_on_all(int port)
{
  #ifdef SERVO_TIMEOUT_EVENT
    struct timespec current_time;
  #endif
  unsigned char b;
  int err;
  int servo_port = (unsigned)(port);

  timer_expired = 0;

  #ifdef SERVO_TIMEOUT_EVENT
    kern_gettime(&current_time);
    ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
    timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
  #else
    timeout_event = NIL;
  #endif  

  b = 0x80;
  err = com_send(servo_port, b);
  err = com_receive(servo_port);
  if (err != (int)(b)) timer_expired = 1;

  b = 0x21;
  err = com_send(servo_port, b);
  err = com_receive(servo_port);
  if (err != (int)(b)) timer_expired = 1;

  if (timeout_event != NIL) kern_event_delete(timeout_event);

  if (!timer_expired)
    return 0;
  else
    return -1;

}

/* 1000.0000:0101.000M:mmmm.mmmm */
int servo_set_levels(int port, int bank,int mask)
{
  #ifdef SERVO_TIMEOUT_EVENT
    struct timespec current_time;
  #endif
  unsigned char b;
  int err;
  int servo_port = (unsigned)(port);

  timer_expired = 0;
 
  #ifdef SERVO_TIMEOUT_EVENT
    kern_gettime(&current_time);
    ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
    timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
  #else
    timeout_event = NIL;
  #endif

  b = 0x80;
  err = com_send(servo_port, b);
  err = com_receive(servo_port);
  if (err != (int)(b)) timer_expired = 1;

  b = 0x50 | (0x01 & bank);
  err = com_send(servo_port, b);
  err = com_receive(servo_port);
  if (err != (int)(b)) timer_expired = 1;

  b = (unsigned char)(mask & 0xFF);
  err = com_send(servo_port, b);
  err = com_receive(servo_port);
  if (err != (int)(b)) timer_expired = 1;

  if (timeout_event != NIL) kern_event_delete(timeout_event);

  if (!timer_expired)
    return 0;
  else
    return -1;

}

/* 1000.0000:0100.000M */
int servo_get_levels(int port, int bank)
{
  #ifdef SERVO_TIMEOUT_EVENT
    struct timespec current_time;
  #endif
  unsigned char b;
  int err;
  int servo_port = (unsigned)(port);

  timer_expired = 0;
 
  #ifdef SERVO_TIMEOUT_EVENT
    kern_gettime(&current_time);
    ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
    timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
  #else
    timeout_event = NIL;
  #endif

  b = 0x80;
  err = com_send(servo_port, b);
  err = com_receive(servo_port);
  if (err != (int)(b)) timer_expired = 1;

  b = 0x40 | (0x01 & bank);
  err = com_send(servo_port, b);
  err = com_receive(servo_port);
  if (err != (int)(b)) timer_expired = 1;
  err = com_receive(servo_port);

  if (timeout_event != NIL) kern_event_delete(timeout_event);

  if (!timer_expired)
    return err;
  else
    return -1;

}

/* 1000.0000:1000.0000 */
int servo_store_levels(int port)
{
  #ifdef SERVO_TIMEOUT_EVENT
    struct timespec current_time;
  #endif
  unsigned char b;
  int err;
  int servo_port = (unsigned)(port);

  timer_expired = 0;
 
  #ifdef SERVO_TIMEOUT_EVENT
    kern_gettime(&current_time);
    ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
    timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
  #else
    timeout_event = NIL;
  #endif

  b = 0x80;
  err = com_send(servo_port, b);
  err = com_receive(servo_port);
  if (err != (int)(b)) timer_expired = 1;

  b = 0x80;
  err = com_send(servo_port, b);
  err = com_receive(servo_port);
  if (err != (int)(b)) timer_expired = 1;

  if (timeout_event != NIL) kern_event_delete(timeout_event);

  if (!timer_expired)
    return 0;
  else
    return -1;

}

int servo_set_max_angle_sec(int port, int servo, int angle_sec)
{

  servo_table[port][servo].max_angle_sec = angle_sec;
  return 0;

}

int servo_set_min_angle_sec(int port, int servo, int angle_sec)
{

  servo_table[port][servo].min_angle_sec = angle_sec;
  return 0;

}

int servo_set_servo_tick(int port, int servo, int zero_tick, int delta_tick)
{

  if (zero_tick != -1) servo_table[port][servo].zero_tick = zero_tick;
  if (delta_tick != -1) servo_table[port][servo].delta_tick = delta_tick;
  return 0;

}

/* 0000.Pppp:0000.vvvv:vvvv.vvvv */
int servo_set_angle_sec(int port, int servo, int angle_sec)
{
  unsigned char b[3];
  int angle_tick;
  int servo_port = (unsigned)(port);

  if (servo > 15) return -1;

  angle_tick = (servo_table[port][servo].zero_tick + angle_sec *
                servo_table[port][servo].delta_tick /
                (servo_table[port][servo].max_angle_sec - servo_table[port][servo].min_angle_sec)) * 1000 / TICK_LEN;

  b[0] = 0x00 | (servo & 0x0F);
  b[1] = 0x00 | ((angle_tick >> 8) & 0x0F);
  b[2] = 0x00 | (angle_tick & 0xFF);
  servo_send_msg(servo_port, b, 3, NULL, 0);

  return 0;

}

/* 0010.Pppp */
int servo_store_default_position(int port, int servo)
{
  #ifdef SERVO_TIMEOUT_EVENT
    struct timespec current_time;
  #endif
  unsigned char b;
  int err;
  int servo_port = (unsigned)(port);

  if (servo > 15) return -1;

  timer_expired = 0;
 
  #ifdef SERVO_TIMEOUT_EVENT
    kern_gettime(&current_time);
    ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
    timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
  #else
    timeout_event = NIL;
  #endif
 
  b = 0x20 | (servo & 0x0F);
  err = com_send(servo_port, b);
  err = com_receive(servo_port);
  if (err != (int)(b)) timer_expired = 1;

  if (timeout_event != NIL) kern_event_delete(timeout_event);

  if (!timer_expired)
    return 0;
  else
    return -1;

}

/* 0001.Pppp */
int servo_get_angle_sec(int port, int servo)
{
  #ifdef SERVO_TIMEOUT_EVENT
    struct timespec current_time;
  #endif
  unsigned char b;
  int err,res,data;
  int servo_port = (unsigned)(port);

  if (servo > 15) return -1;

  timer_expired = 0;
 
  #ifdef SERVO_TIMEOUT_EVENT
    kern_gettime(&current_time);
    ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
    timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
  #else
    timeout_event = NIL;
  #endif

  b = 0x10 | (servo & 0x0F);
  err = com_send(servo_port, b);
  err = com_receive(servo_port);
  if (err != (int)(b)) timer_expired = 1;
  res = com_receive(servo_port) << 8;
  res |= com_receive(servo_port);

  if (timeout_event != NIL) kern_event_delete(timeout_event);

  data = ((res*TICK_LEN/1000) - servo_table[port][servo].zero_tick) *
          (servo_table[port][servo].max_angle_sec - servo_table[port][servo].min_angle_sec) /
           servo_table[port][servo].delta_tick;

  if (!timer_expired)
    return data;
  else
    return -1;

}

/* 0100:0aaa */
int servo_get_analog(int port, int adport)
{

  #ifdef SERVO_TIMEOUT_EVENT
    struct timespec current_time;
  #endif
  unsigned char b;
  int err,res;
  int servo_port = (unsigned)(port);

  if (adport > 7) return -1;

  timer_expired = 0;
 
  #ifdef SERVO_TIMEOUT_EVENT
    kern_gettime(&current_time);
    ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
    timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
  #else
    timeout_event = NIL;
  #endif

  b = 0x40 | (adport & 0x07);
  err = com_send(servo_port, b);
  err = com_receive(servo_port);
  if (err != (int)(b)) timer_expired = 1;
  res = com_receive(servo_port) << 8;
  res |= com_receive(servo_port);

  if (timeout_event != NIL) kern_event_delete(timeout_event);

  if (!timer_expired)
    return res;
  else
    return -1;

}