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>

 *   (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 "servo.h"

#define THR 0

#define RBR 0

#define IER 1

#define FCR 2

#define IIR 2

#define LCR 3

#define MCR 4

#define LSR 5

#define MSR 6

#define SPad 7

/* Parity value */

#define NONE    0

#define ODD     1

#define EVEN    3

#define barrier() __asm__ __volatile__("" ::: "memory");

#define SERVO_DEBUG

#define SERVO_TIMEOUT 100000 /* us */

#define SERVO_SPEED 38400

#define SERVO_PARITY NONE

#define SERVO_LEN 8

#define SERVO_STOP 1

#define TICK_LEN 1600 /* us */

struct servo_data {

  int min_angle_sec;

  int max_angle_sec;

  int delta_tick;

  int zero_tick;

};

struct servo_data servo_table[] = {

  {-324000, 324000, 1800, 1600},

  {-324000, 324000, 1800, 1600},

  {-324000, 324000, 1800, 1600},

  {-324000, 324000, 1800, 1600},

  {-324000, 324000, 1800, 1600},

  {-324000, 324000, 1800, 1600},

  {-324000, 324000, 1800, 1600},

  {-324000, 324000, 1800, 1600}};

int timer_expired = 0;

int timeout_event;

int servo_port;

const unsigned com_base[] = {0x03F8,0x02F8,0x03E8,0x02E8};

const int BaudTable[] = {

        1200,

        2400,

        4800,

        9600,

        14400,

        19200,

        38400,

        57600,

        115200,

        230400,

        -1};

/* 20MHz: SPBRG, BAUD BRGH=0, BAUD BRGH=1 */

const int Baud20MhzData[] = {

  255,   1221,   4882,

  254,   1225,   4902,

  253,   1230,   4921,

  252,   1235,   4941,

  133,   2332,   9328,

  132,   2350,   9398,

  131,   2367,   9470,

  130,   2385,   9542,

  129,   2404,   9615,

  128,   2422,   9690,

  127,   2441,   9766,

  126,   2461,   9842,

   88,      0,  14045,

   87,      0,  14204,

   86,      0,  14368,

   85,      0,  14535,

   84,      0,  14706,

   66,   4664,  18657,

   65,   4735,  18939,

   64,   4808,  19231,

   63,   4883,  19531,

   32,   9470,  37879,

   31,   9766,  39062,

   21,  14205,  56818,

   15,  19531,      0,

   10,      0, 113636,

    7,  39062,      0,

   -1,     -1,     -1};

void set_timer_expired(void *arg)

{

  timeout_event = NIL;

  timer_expired = 1;

}

unsigned com_read(unsigned port,unsigned reg)

{

    unsigned b;

    if (port > 3 || reg > 7) return(0);

    b = ll_in(com_base[port]+reg);

    return(b);

}

void com_write(unsigned port,unsigned reg,unsigned value)

{

    if (port > 3 || reg > 7) return;

    ll_out(com_base[port]+reg,value);

}

int com_send(unsigned port,BYTE b)

{

    while ((com_read(port,LSR) & 32) == 0 && !timer_expired)

      barrier();

    if (!timer_expired) {

      com_write(port,THR,b);

      return 0;

    } else {

      return -1;

    }

}

int com_receive(unsigned port)

{

    while ((com_read(port,LSR) & 1) == 0 && !timer_expired)

      barrier();

    if (!timer_expired) {

      return((int)(com_read(port,RBR)));

    } else {

      return -1;

    }

}

int com_open(unsigned port,DWORD speed,BYTE parity,BYTE len,BYTE stop)

{

    unsigned long div,b_mask;

    /* Now set up the serial link */

    b_mask = (parity & 3) * 8 + (stop & 1) * 4 + ((len - 5) & 3);

    div = 115200L / speed;

    /* Clear serial interrupt enable register */

    com_write(port,IER,0);

    /* Empty input buffer */

    com_read(port,RBR);

    /* Activate DLAB bit for speed setting */

    com_write(port,LCR,0x80);

    /* Load baud divisor */

    com_write(port,0,div & 0x00FF);

    div >>= 8;

    com_write(port,1,div & 0x00FF);

    /* Load control word (parity,stop bit,bit len) */

    com_write(port,LCR,b_mask);

    /* Attiva OUT1 & OUT2 */

    com_write(port,MCR,0x0C);

    return 0;

}

int com_close(unsigned port)

{

  com_write(port,IER,0);

  com_read(port,IIR);

  com_read(port,RBR);

  return 0;

}

int servo_open(int port)

{

  int err;

  servo_port = (unsigned)(port);

  err = com_open(servo_port, SERVO_SPEED, SERVO_PARITY, SERVO_LEN, SERVO_STOP);

  return err;

}

int servo_close(void)

{

  int err;

  err = com_close(servo_port);

  return err;

}

/* 1000.011w:bbbb.bbbb */

int servo_set_RS232_baudrate(int baud)

{

  struct timespec current_time;

  unsigned char b;

  int err,diff,mindiff,i;

  unsigned char spbrg, w;

  i = 0;

  while(BaudTable[i] != baud && BaudTable[i] != -1) i++;

  if (BaudTable[i] == -1) {

    kern_printf("SERVO:Error wrong baud rate\n");

    return -1;

  }

  i = 0;

  mindiff = 0xFFFFFF;

  spbrg = 0;

  w = 0;

  while(Baud20MhzData[i] != -1) {

    diff = abs(Baud20MhzData[i+1] - baud);

    if (diff < mindiff) {

      mindiff = diff;

      spbrg = (unsigned char)(Baud20MhzData[i+1]);

      w = 0;

    }

    diff = abs(Baud20MhzData[i+2] - baud);

    if (diff < mindiff) {

      mindiff = diff;

      spbrg = (unsigned char)(Baud20MhzData[i+2]);

      w = 1;

    }

    i += 3;

  }

  #ifdef SERVO_DEBUG

    kern_printf("(SERVO:SBPRG %d W %d)",spbrg,w);

  #endif                                         

  timer_expired = 0;

  kern_gettime(&current_time);

  ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);

  timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);

  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(void)

{

  struct timespec current_time;

  unsigned char b;

  int err,res;

  timer_expired = 0;

  kern_gettime(&current_time);

  ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);

  timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);

  b = 0x85;

  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.0100 */

int servo_store_RS232_buadrate(void)

{

  struct timespec current_time;

  unsigned char b;

  int err;

  timer_expired = 0;

  kern_gettime(&current_time);

  ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);

  timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);

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

{

  struct timespec current_time;

  unsigned char b;

  int err;

  timer_expired = 0;

  kern_gettime(&current_time);

  ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);

  timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);

  b = 0x8A;

  err = com_send(servo_port, b);

  err = com_receive(servo_port);

  if (err != (int)(b)) timer_expired = 1;

  b = period/TICK_LEN/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(void)

{

  struct timespec current_time;

  unsigned char b;

  int err,res;

  timer_expired = 0;

  kern_gettime(&current_time);

  ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);

  timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);

  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 (res*TICK_LEN*8);

  else

    return -1;

}

/* 1000.1000 */

int servo_store_period(void)

{

  struct timespec current_time;

  unsigned char b;

  int err;

  timer_expired = 0;

  kern_gettime(&current_time);

  ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);

  timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);

  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(void)

{

  struct timespec current_time;

  unsigned char b;

  int err,res;                                                                                                                        

  timer_expired = 0;

  kern_gettime(&current_time);

  ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);

  timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);  

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

{

  struct timespec current_time;

  unsigned char b;

  int err;

  timer_expired = 0;

  kern_gettime(&current_time);

  ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);

  timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);

  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.0mmm */

int servo_turn_off(int servo)

{

  struct timespec current_time;

  unsigned char b;

  int err;

  if (servo > 7) return -1;

  timer_expired = 0;

  kern_gettime(&current_time);

  ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);

  timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);

  b = 0x80;

  err = com_send(servo_port, b);

  err = com_receive(servo_port);

  if (err != (int)(b)) timer_expired = 1;

  b = 0x00 | (servo & 0x07);

  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.0mmm */

int servo_turn_on(int servo)

{

  struct timespec current_time;

  unsigned char b;

  int err;

  if (servo > 7) return -1;

  timer_expired = 0;

  kern_gettime(&current_time);

  ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);

  timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);

  b = 0x80;

  err = com_send(servo_port, b);

  err = com_receive(servo_port);

  if (err != (int)(b)) timer_expired = 1;

  b = 0x10 | (servo & 0x07);

  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(void)

{                                                                                                                              struct timespec current_time;

  unsigned char b;

  int err;

  timer_expired = 0;

  kern_gettime(&current_time);

  ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);

  timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);

  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(void)

{

  struct timespec current_time;

  unsigned char b;

  int err;

  timer_expired = 0;

  kern_gettime(&current_time);

  ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);

  timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);

  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;

}

int servo_set_max_angle(int servo, int angle_sec)

{

  servo_table[servo].max_angle_sec = angle_sec;

  return 0;

}

int servo_set_min_angle(int servo, int angle_sec)

{

  servo_table[servo].min_angle_sec = angle_sec;

  return 0;

}

/* 0000.0ppp:0000.vvvv:vvvv.vvvv */

int servo_set_angle_sec(int servo, int angle_sec)

{

  struct timespec current_time;

  unsigned char b;

  int err, angle_tick;

  if (servo > 7) return -1;

  if (angle_sec > servo_table[servo].max_angle_sec ||

      angle_sec < servo_table[servo].min_angle_sec) return -1;

  angle_tick = servo_table[servo].zero_tick + angle_sec * servo_table[servo].delta_tick /

              (servo_table[servo].max_angle_sec - servo_table[servo].min_angle_sec);

  timer_expired = 0;

  kern_gettime(&current_time);

  ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);

  timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);

  b = 0x00 | (servo & 0x07);

  err = com_send(servo_port, b);

  err = com_receive(servo_port);

  if (err != (int)(b)) timer_expired = 1;

  b = 0x00 | ((angle_tick >> 8) & 0x0F);

  err = com_send(servo_port, b);

  err = com_receive(servo_port);

  if (err != (int)(b)) timer_expired = 1;

  b = 0x00 | (angle_tick & 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;

}

/* 0010.0ppp */

int servo_store_default_position(int servo)

{

  struct timespec current_time;

  unsigned char b;

  int err;

  if (servo > 7) return -1;

  timer_expired = 0;

  kern_gettime(&current_time);

  ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);

  timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);

  b = 0x20 | (servo & 0x07);

  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.0ppp */

int servo_get_angle_sec(int servo)

{

  struct timespec current_time;

  unsigned char b;

  int err,res,data;

  if (servo > 7) return -1;

  timer_expired = 0;

  kern_gettime(&current_time);

  ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);

  timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);

  b = 0x10 | (servo & 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);

  data = (res - servo_table[servo].zero_tick) *

          (servo_table[servo].max_angle_sec - servo_table[servo].min_angle_sec) /

           servo_table[servo].delta_tick;

  if (!timer_expired)

    return data;

  else

    return -1;

}

/* 0100:0aaa */

int servo_get_analog(int port)

{

  struct timespec current_time;

  unsigned char b;

  int err,res;

  if (port > 4) return -1;

  timer_expired = 0;