Subversion Repositories shark

Rev

Rev 772 | Blame | Compare with Previous | Last modification | View Log | RSS feed

/*
 * 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 "kernel/kern.h"

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

#include "servo.h"

//#define SERVO_DEBUG

#define SERVO_SPEED 19200
#define SERVO_PARITY EVEN
#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}}};

#define SERVO_TIMEOUT 100000

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

#define SERVO_LOCK 0
#define SERVO_UNLOCK 1
#define SERVO_SKIP (BYTE *)(0xFFFFFFFF)

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

void servo_rx_error(unsigned port, unsigned type, unsigned data) {

  #ifdef SERVO_DEBUG
    kern_printf("(SERVO: BUFFER ERROR PORT:%d ERR:%d DATA:%02x)",port,type,data);
  #endif

}

void servo_indication(unsigned port, BYTE data) {

 if (RXTX_addr[port][RX_position[port]] == NULL) {
   if (data != 0) {
     servo_rx_error(port,1,data);
     while(RXTX_addr[port][RX_position[port]] != SERVO_SKIP) {
       RXTX_addr[port][RX_position[port]] = SERVO_SKIP;
       RX_position[port]++;

       if (RX_position[port] >= RXTX_BUFF_MAX) {
         RX_cycle[port]++;
         RX_position[port] = 0;
       }
       if ((RX_cycle[port] == TX_cycle[port] && RX_position[port] > TX_position[port]) ||
            (RX_cycle[port] > TX_cycle[port]))
         servo_rx_error(port,2,data);

     }
   }

   RXTX_addr[port][RX_position[port]] = SERVO_SKIP;

   RX_position[port]++;

   if (RX_position[port] >= RXTX_BUFF_MAX) {
        RX_cycle[port]++;
        RX_position[port] = 0;
   }
   if ((RX_cycle[port] == TX_cycle[port] && RX_position[port] > TX_position[port]) ||
        (RX_cycle[port] > TX_cycle[port]))
     servo_rx_error(port,2,data);

 } else {

   if (RXTX_addr[port][RX_position[port]] != SERVO_SKIP) {

     *RXTX_addr[port][RX_position[port]] = data;
     RXTX_buff[port][RX_position[port]]  = SERVO_UNLOCK; //Unlock the data

     RXTX_addr[port][RX_position[port]] = SERVO_SKIP;

     RX_position[port]++;

     if (RX_position[port] >= RXTX_BUFF_MAX) {
        RX_cycle[port]++;
        RX_position[port] = 0;
     }
     if ((RX_cycle[port] == TX_cycle[port] && RX_position[port] > TX_position[port]) ||
          (RX_cycle[port] > TX_cycle[port]))
       servo_rx_error(port,3,data);

   } else {

     RX_position[port]++;
                                                                                                                             
     if (RX_position[port] >= RXTX_BUFF_MAX) {
        RX_cycle[port]++;
        RX_position[port] = 0;
     }
     if ((RX_cycle[port] == TX_cycle[port] && RX_position[port] > TX_position[port]) ||
          (RX_cycle[port] > TX_cycle[port]))
       servo_rx_error(port,3,data);

     servo_indication(port, data);

   }

 }

}

extern unsigned SCom_Error[4];

void servo_confirm(unsigned port, BYTE msg_status) {

  #ifdef SERVO_DEBUG
    if (msg_status == COM_ERROR) {
      kern_printf("(SCOM: PORT:%d ERROR:%d)",port,SCom_Error[port]);
    }
  #endif

  lock_write[port] = 0;

}

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

  int i = 0, old, oldcycle, RX = 0;
  volatile int start = kern_gettime(NULL), timeout = 0;
  SYS_FLAGS f;

  while(lock_write[port] == 1 && timeout == 0)
     if ((start + SERVO_TIMEOUT) < kern_gettime(NULL)) timeout = 1;

  if (timeout == 1) {

    #ifdef SERVO_DEBUG
      kern_printf("(SERVO: TIMEOUT SEND)");
    #endif

    return -1;
  }

  lock_write[port] = 1;

  f = kern_fsave();

  old = TX_position[port];
  oldcycle = TX_cycle[port];

  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_cycle[port]++;
        TX_position[port] = 0;
    }
    if (TX_position[port] == RX_position[port]) {
        TX_position[port] = old;
        TX_cycle[port] = oldcycle;
        lock_write[port] = 0;

        #ifdef SERVO_DEBUG
          kern_printf("(SERVO: BUFFER FULL)");
        #endif

        kern_frestore(f);
        return -1;
    }
    i++;
  }

  i = 0;
  while(i < len_res) {
    RXTX_buff[port][TX_position[port]] = SERVO_LOCK; //Lock the data
    RXTX_addr[port][TX_position[port]] = res+i;
    RX = TX_position[port];
    TX_position[port]++;
    if (TX_position[port] >= RXTX_BUFF_MAX) {
        TX_cycle[port]++;
        TX_position[port] = 0;
    }
    if (TX_position[port] == RX_position[port]) {
        TX_position[port] = old;
        TX_cycle[port] = oldcycle;
        lock_write[port] = 0;

        #ifdef SERVO_DEBUG
          kern_printf("(SERVO: BUFFER FULL)");
        #endif

        kern_frestore(f);
        return -1;
    }
    i++;
  }

  /* SERVO_SKIP close the command sequence */
  RXTX_addr[port][TX_position[port]] = SERVO_SKIP;
  TX_position[port]++;
  if (TX_position[port] >= RXTX_BUFF_MAX) {
    TX_cycle[port]++;
    TX_position[port] = 0;
  }
  if (TX_position[port] == RX_position[port]) {
    TX_position[port] = old;
    TX_cycle[port] = oldcycle;
    lock_write[port] = 0;
                                                                                                                             
    #ifdef SERVO_DEBUG
      kern_printf("(SERVO: BUFFER FULL)");
    #endif
                                                                                                                             
    kern_frestore(f);
    return -1;
  }

  kern_frestore(f);

  com_irq_send(port, len_msg, msg);

  return RX;

}

int servo_wait(unsigned port, int RX) {

  volatile int start = kern_gettime(NULL), timeout = 0, temp;

  /* Active wait until number bytes received */
  while(RXTX_buff[port][RX] == SERVO_LOCK && timeout == 0)
     if ((start + SERVO_TIMEOUT) < kern_gettime(NULL)) timeout = 1;  

  if (timeout == 1) {
    SYS_FLAGS f;

    #ifdef SERVO_DEBUG
      kern_printf("(SERVO: TIMEOUT GET)");
    #endif

    f = kern_fsave();

    temp = RX;
   
    while(RXTX_addr[port][temp] != SERVO_SKIP) {
      RXTX_addr[port][temp] = SERVO_SKIP;
      RXTX_buff[port][temp] = SERVO_UNLOCK;
      temp--;
      if (temp < 0) temp = RXTX_BUFF_MAX-1;
    }
   
    kern_frestore(f);

  }

  return timeout;

}

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((unsigned)(port),servo_confirm,servo_indication);

  com_irq_enable((unsigned)(port),ALL_IRQ);

  return err;

}

int servo_close(int port)
{
  int err;

  com_irq_disable((unsigned)(port),ALL_IRQ);

  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)
{
  unsigned char b[2];
  int 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

  b[0] = 0x86 | (w & 0x01);
  b[1] = spbrg;
  servo_send_msg(servo_port, b, 2, NULL, 0);

  return 0;

}

/* 1000.0101 */
int servo_get_RS232_baudrate(int port)
{
  unsigned char b[1],r[2];
  int res, res_w, res_b, t;
  int servo_port = (unsigned)(port);
  int RX;

  b[0] = 0x85;
  RX = servo_send_msg(servo_port, b, 1, r, 2);

  if (RX != -1) {
                                                                                                                             
    t = servo_wait(servo_port, RX);
                                                                                                                             
    if (t == 0) {

      res_w = r[0]; /* bit W */
      res_b = r[1]; /* 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;
      }

      return res;

    } else return -1;

  } else return -1;

}

/* 1000.0100 */
int servo_store_RS232_baudrate(int port)
{
  unsigned char b[1];
  int servo_port = (unsigned)(port);

  b[0] = 0x84;
  servo_send_msg(servo_port, b, 1, NULL, 0);
   
  return 0;

}

/* 1000.1010:llll.llll */
int servo_set_period(int port, int period)
{
  unsigned char b[2];
  int servo_port = (unsigned)(port);

  b[0] = 0x8A;
  b[1] = (period*1000)/TICK_LEN_PERIOD/8 & 0xFF;
  servo_send_msg(servo_port, b, 2, NULL, 0);  
 
  return 0;

}

/* 1000.1001 */
int servo_get_period(int port)
{
  unsigned char b[1],r[1];
  int res, RX, t;
  int servo_port = (unsigned)(port);

  b[0] = 0x89;
  RX = servo_send_msg(servo_port, b, 1, r, 1);

  if (RX != -1) {
                                                                                                                             
    t = servo_wait(servo_port, RX);
                                                                                                                             
    if (t == 0) {

      res = r[0];

      return (((unsigned char)(res))*TICK_LEN_PERIOD/1000*8);

    } else return -1;

  } else return -1;

}

/* 1000.1000 */
int servo_store_period(int port)
{
  unsigned char b[1];
  int servo_port = (unsigned)(port);

  b[0] = 0x88;
  servo_send_msg(servo_port, b, 1, NULL, 0);

  return 0;

}

/* 1000.1100 */
int servo_get_setup_switch(int port)
{
  unsigned char b[1],r[1];
  int res, RX, t;
  int servo_port = (unsigned)(port);

  b[0] = 0x8C;
  RX = servo_send_msg(servo_port, b, 1, r, 1);  

  if (RX != -1) {
                                                                                                                             
    t = servo_wait(servo_port, RX);
                                                                                                                             
    if (t == 0) {

       res = r[0];

       return res;

    } else return -1;

  } else return -1;

}

/* 1000.111s */
int servo_set_RC5_switch(int port, int data)
{
  unsigned char b[1];
  int servo_port = (unsigned)(port);

  b[0] = 0x8E | (data & 0x01);
  servo_send_msg(servo_port, b, 1, NULL, 0);

  return 0;

}

/* 1000.0000:0000.Mmmm */
int servo_turn_off(int port, int servo)
{
  unsigned char b[2];
  int servo_port = (unsigned)(port);

  if (servo > 15) return -1;

  b[0] = 0x80;
  b[1] = 0x00 | (servo & 0x0F);
  servo_send_msg(servo_port, b, 2, NULL, 0);
 
  return 0;

}

/* 1000.0000:0001.Mmmm */
int servo_turn_on(int port, int servo)
{
  unsigned char b[2];
  int servo_port = (unsigned)(port);

  if (servo > 15) return -1;

  b[0] = 0x80;
  b[1] = 0x10 | (servo & 0x0F);
  servo_send_msg(servo_port, b, 2, NULL, 0);

  return 0;

}

/* 1000.0000:0010.0000 */
int servo_turn_off_all(int port)
{
  unsigned char b[2];
  int servo_port = (unsigned)(port);

  b[0] = 0x80;
  b[1] = 0x20;
  servo_send_msg(servo_port, b, 2, NULL, 0);

  return 0;

}

/* 1000.0000:0010.0001 */
int servo_turn_on_all(int port)
{
  unsigned char b[2];
  int servo_port = (unsigned)(port);

  b[0] = 0x80;
  b[1] = 0x21;
  servo_send_msg(servo_port, b, 2, NULL, 0);
   
  return 0;

}

/* 1000.0000:0101.000M:mmmm.mmmm */
int servo_set_levels(int port, int bank, int mask)
{
  unsigned char b[3];
  int servo_port = (unsigned)(port);

  b[0] = 0x80;
  b[1] = 0x50 | (0x01 & bank);
  b[2] = (unsigned char)(mask & 0xFF);
  servo_send_msg(servo_port, b, 3, NULL, 0);

  return 0;

}

/* 1000.0000:0100.000M */
int servo_get_levels(int port, int bank)
{
  unsigned char b[2],r[1];
  int res, RX, t;
  int servo_port = (unsigned)(port);

  b[0] = 0x80;
  b[1] = 0x40 | (0x01 & bank);
  RX = servo_send_msg(servo_port, b, 2, r, 1);

  if (RX != -1) {
                                                                                                                             
    t = servo_wait(servo_port, RX);
                                                                                                                             
    if (t == 0) {

      res = r[0];

      return res;

    } else return -1;

  } else return -1;

}

/* 1000.0000:1000.0000 */
int servo_store_levels(int port)
{
  unsigned char b[2];
  int servo_port = (unsigned)(port);

  b[0] = 0x80;
  b[1] = 0x80;
  servo_send_msg(servo_port, b, 2, NULL, 0);
 
  return 0;

}

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, res = 0;
  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);
  res = servo_send_msg(servo_port, b, 3, NULL, 0);

  return res;

}

/* 0010.Pppp */
int servo_store_default_position(int port, int servo)
{
  unsigned char b[1];
  int res = 0;
  int servo_port = (unsigned)(port);

  if (servo > 15) return -1;

  b[0] = 0x20 | (servo & 0x0F);
  res = servo_send_msg(servo_port, b, 1, NULL, 0);  

  return res;

}

/* 0001.Pppp */
int servo_get_angle_sec(int port, int servo)
{
  unsigned char b[1],r[2];
  int res,data,RX,t;
  int servo_port = (unsigned)(port);

  if (servo > 15) return -1;

  b[0] = 0x10 | (servo & 0x0F);
  RX = servo_send_msg(servo_port, b, 1, r, 2);

  if (RX != -1) {

    t = servo_wait(servo_port, RX);

    if (t == 0) {

      res = (int)(r[0]) << 8;
      res |= r[1];

      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;

      return data;

    } else return -1;

  } else return -1;

}

/* 0100:baaa */
int servo_get_analog(int port, int adport, int nbit)
{
  unsigned char b[1],r[2];
  int res, RX, t, nb;
  int servo_port = (unsigned)(port);

  if (adport > 7) return -1;
  nb = (nbit==8) ? 0 : 1;

  b[0] = 0x40 | (adport & 0x07);
  if (nb) {
    RX = servo_send_msg(servo_port, b, 1, r, 2);
  } else {
    b[0] |= 0x08;
    RX = servo_send_msg(servo_port, b, 1, r, 1);
  }

  if (RX != -1) {

    t = servo_wait(servo_port, RX);

    if (t == 0) {

      if (nb) {
        res = (int)r[0] << 8;
        res |= r[1];
      } else {
        res = r[0];
      }

      return res;

    } else return -1;
                                                                                                                             
  } else return -1;

}