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

#include "drivers/scom.h"

#include "drivers/scomirq.h"

#include "servo.h"

//#define SERVO_DEBUG

#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}}};

//#define SERVO_TIMEOUT 10000

#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 servo_skip[4] = {0,0,0,0};

volatile unsigned int lock_write[4] = {0,0,0,0};

volatile int servo_timer[4] = {NIL,NIL,NIL,NIL};

const int BaudTable[] = {

        1200,

        2400,

        4800,

        9600,

        14400,

        19200,

        38400,

        57600,

        115200,

        -1};

void servo_rx_error(unsigned port, unsigned type) {

  kern_printf("(SERVO: BUFFER ERROR PORT:%d ERR:%d)",port,type);

}

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,1);

   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);

 } else {

   *RXTX_addr[port][RX_position[port]] = data;

   RXTX_buff[port][RX_position[port]]  = 1; //Unlock the data

   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);

 }

}

extern unsigned SCom_Error[4];

void servo_confirm(unsigned port, BYTE msg_status) {

  if (msg_status == COM_ERROR) {

    kern_printf("(SERVO: PORT:%d ERROR:%d)",port,SCom_Error[port]);

  }

  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;

  SYS_FLAGS f;

  while(lock_write[port] == 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;

        kern_frestore(f);

        return -1;

    }

    i++;

  }

  i = 0;

  while(i < len_res) {

    RXTX_buff[port][TX_position[port]] = 0; //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;

        kern_frestore(f);

        return -1;

    }

    i++;

  }

  kern_frestore(f);

  com_irq_send(port, len_msg, msg);

  return RX;

}

void servo_timeout(void *arg) {

  unsigned port = (unsigned)(arg);

  servo_timer[port] = NIL;

  servo_skip[port] = 1;

  kern_printf("(SERVO: TIMEOUT PORT:%d)",port);

}

int servo_wait(unsigned port, int RX) {

  #ifdef SERVO_TIMEOUT

    struct timespec acttime;

  #endif

  servo_skip[port] = 0;

  #ifdef SERVO_TIMEOUT

    kern_gettime(&acttime);

    ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&acttime);

    servo_timer[port] = kern_event_post(&acttime,servo_timeout,(void *)(port));

  #endif

  /* Active wait until number bytes received */

  while (RXTX_buff[port][RX] == 0 && servo_skip[port] == 0);

  #ifdef SERVO_TIMEOUT

  if (servo_timer[port] != NIL)

    kern_event_delete(servo_timer[port]);

  #endif

  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);

  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;

  int servo_port = (unsigned)(port);

  int RX;

  b[0] = 0x85;

  RX = servo_send_msg(servo_port, b, 1, r, 2);

  if (RX != -1) {

    servo_wait(servo_port, RX);

    if (servo_skip[port] == 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;

  int servo_port = (unsigned)(port);

  b[0] = 0x89;

  RX = servo_send_msg(servo_port, b, 1, r, 1);

  if (RX != -1) {

    servo_wait(servo_port, RX);

    if (servo_skip[port] == 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;

  int servo_port = (unsigned)(port);

  b[0] = 0x8C;

  RX = servo_send_msg(servo_port, b, 1, r, 1);  

  if (RX != -1) {

    servo_wait(servo_port, RX);

    if (servo_skip[port] == 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;

  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) {

    servo_wait(servo_port, RX);

    if (servo_skip[port] == 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;