Rev 727 |
Rev 730 |
Go to most recent revision |
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 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 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 = 0;
volatile unsigned int TX_cycle = 0;
volatile unsigned int servo_skip[4] = {0,0,0,0};
volatile unsigned int lock_write[4] = {0,0,0,0};
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++;
RX_position[port] = 0;
}
if ((RX_cycle == TX_cycle && RX_position[port] > TX_position[port]) ||
(RX_cycle > TX_cycle))
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++;
RX_position[port] = 0;
}
if ((RX_cycle == TX_cycle && RX_position[port] > TX_position[port]) ||
(RX_cycle > TX_cycle))
servo_rx_error(port,3);
}
}
extern int SCom_Error;
void servo_confirm(unsigned port, BYTE msg_status) {
if (msg_status == COM_ERROR) {
kern_printf("(SERVO: PORT:%d ERROR:%d)",port,SCom_Error);
servo_skip[port] = 1;
}
if (msg_status == COM_OK) {
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;
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++;
TX_position[port] = 0;
}
if (TX_position[port] == RX_position[port]) {
TX_position[port] = old;
TX_cycle = oldcycle;
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++;
TX_position[port] = 0;
}
if (TX_position[port] == RX_position[port]) {
TX_position[port] = old;
TX_cycle = oldcycle;
kern_frestore(f);
return -1;
}
i++;
}
kern_frestore(f);
com_irq_send(port, len_msg, msg);
return RX;
}
int servo_wait(unsigned port, int RX) {
servo_skip[port] = 0;
/* Active wait until number bytes received */
while (RXTX_buff[port][RX] == 0 && servo_skip[port] == 0);
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);
servo_wait(servo_port, RX);
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;
}
/* 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);
servo_wait(servo_port, RX);
res = r[0];
return (((unsigned char)(res))*TICK_LEN_PERIOD/1000*8);
}
/* 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);
servo_wait(servo_port, RX);
res = r[0];
return res;
}
/* 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);
servo_wait(servo_port, RX);
res = r[0];
return res;
}
/* 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;
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)
{
unsigned char b[1];
int servo_port = (unsigned)(port);
if (servo > 15) return -1;
b[0] = 0x20 | (servo & 0x0F);
servo_send_msg(servo_port, b, 1, NULL, 0);
return 0;
}
/* 0001.Pppp */
int servo_get_angle_sec(int port, int servo)
{
unsigned char b[1],r[2];
int res,data,RX;
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);
servo_wait(servo_port, RX);
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;
}
/* 0100:0aaa */
int servo_get_analog(int port, int adport)
{
unsigned char b[1],r[2];
int res, RX;
int servo_port = (unsigned)(port);
if (adport > 7) return -1;
b[0] = 0x40 | (adport & 0x07);
RX = servo_send_msg(servo_port, b, 1, r, 2);
servo_wait(servo_port, RX);
res = r[0] << 8;
res |= r[1];
return res;
}