WebSVN - shark - Rev 338 - /shark/trunk/ports/servo/servo.c

/*
* 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 /* 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[] = {
{-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;
int servo_port;

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

const int BaudTable[] = {
1200,
2400,
4800,
9600,
14400,
19200,
38400,
57600,
115200,
-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) {
#ifdef SERVO_DEBUG
kern_printf("(SERVO WRITE p = %d b = %02x)",port,b);
#endif
com_write(port,THR,b);
return 0;
} else {
#ifdef SERVO_DEBUG
kern_printf("(WRITE TIMEOUT)");
#endif
return -1;
}
}

int com_receive(unsigned port)
{
int b;

while ((com_read(port,LSR) & 1) == 0 && !timer_expired)
barrier();
if (!timer_expired) {
b = (int)(com_read(port,RBR));
#ifdef SERVO_DEBUG
kern_printf("(SERVO READ p = %d b = %02x)",port,b);
#endif
return b;
} else {
#ifdef SERVO_DEBUG
kern_printf("(READ TIMEOUT)");
#endif
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]);
w = 0;
}
diff = abs(Baud20MhzData[i+2] - baud);
if (diff < mindiff) {
mindiff = diff;
spbrg = (unsigned char)(Baud20MhzData[i]);
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 = 0xFF - ((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(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 ((0xFF - (unsigned char)(res))*TICK_LEN_PERIOD/1000*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.Mmmm */
int servo_turn_off(int servo)
{

struct timespec current_time;
unsigned char b;
int err;
if (servo > 15) 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 & 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 servo)
{

struct timespec current_time;
unsigned char b;
int err;

if (servo > 15) 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 & 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(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;

}

/* 1000.0000:0101.000M:mmmm.mmmm */
int servo_set_levels(int bank,int mask)
{
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 = 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 bank)
{
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 = 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(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 = 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(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.Pppp: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 > 15) 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)) * 1000 / TICK_LEN;

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 & 0x0F);
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.Pppp */
int servo_store_default_position(int servo)
{
struct timespec current_time;
unsigned char b;
int err;

if (servo > 15) 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 & 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 servo)
{
struct timespec current_time;
unsigned char b;
int err,res,data;

if (servo > 15) 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 & 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[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 > 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 = 0x40 | (port & 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;

}

Subversion Repositories shark

(root)/shark/trunk/ports/servo/servo.c - Rev 338