WebSVN - shark - Blame - Rev 734 - /shark/trunk/ports/servo/servo.c

Rev	Author	Line No.	Line
281	giacomo	1	/*
		2	* Project: S.Ha.R.K.
		3	*
		4	* Coordinators:
		5	* Giorgio Buttazzo <giorgio@sssup.it>
		6	* Paolo Gai <pj@gandalf.sssup.it>
		7	*
		8	* Authors :
		9	* Giacomo Guidi <giacomo@gandalf.sssup.it>
381	giacomo	10	* Mauro Marinoni <mauro.marinoni@unipv.it>
281	giacomo	11	* (see the web pages for full authors list)
		12	*
		13	* ReTiS Lab (Scuola Superiore S.Anna - Pisa - Italy)
		14	*
		15	* http://www.sssup.it
		16	* http://retis.sssup.it
		17	* http://shark.sssup.it
		18	*/
		19
		20	/*
		21	* Copyright (C) 2002 Paolo Gai
		22	*
		23	* This program is free software; you can redistribute it and/or modify
		24	* it under the terms of the GNU General Public License as published by
		25	* the Free Software Foundation; either version 2 of the License, or
		26	* (at your option) any later version.
		27	*
		28	* This program is distributed in the hope that it will be useful,
		29	* but WITHOUT ANY WARRANTY; without even the implied warranty of
		30	* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
		31	* GNU General Public License for more details.
		32	*
		33	* You should have received a copy of the GNU General Public License
		34	* along with this program; if not, write to the Free Software
		35	* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
		36	*
		37	*/
		38
282	giacomo	39	#include "kernel/kern.h"
281	giacomo	40
717	giacomo	41	#include "drivers/scom.h"
		42	#include "drivers/scomirq.h"
281	giacomo	43
717	giacomo	44	#include "servo.h"
286	giacomo	45
358	giacomo	46	//#define SERVO_DEBUG
286	giacomo	47
398	giacomo	48	#define SERVO_SPEED 19200
286	giacomo	49	#define SERVO_PARITY NONE
		50	#define SERVO_LEN 8
		51	#define SERVO_STOP 1
282	giacomo	52
358	giacomo	53	#define SERVO_CLOCK 20000000 /* 20MHz */
		54
315	giacomo	55	#define TICK_LEN 1600 /* ns */
316	giacomo	56	#define TICK_LEN_PERIOD 51200 /* ns */
290	giacomo	57
		58	struct servo_data {
		59	int min_angle_sec;
		60	int max_angle_sec;
		61	int delta_tick;
		62	int zero_tick;
		63	};
		64
381	giacomo	65	struct servo_data servo_table[4][16] = {
		66	{{-324000, 324000, 1200, 1600},
316	giacomo	67	{-324000, 324000, 1200, 1600},
		68	{-324000, 324000, 1200, 1600},
		69	{-324000, 324000, 1200, 1600},
		70	{-324000, 324000, 1200, 1600},
		71	{-324000, 324000, 1200, 1600},
		72	{-324000, 324000, 1200, 1600},
		73	{-324000, 324000, 1200, 1600},
335	giacomo	74	{-324000, 324000, 1200, 1600},
		75	{-324000, 324000, 1200, 1600},
		76	{-324000, 324000, 1200, 1600},
		77	{-324000, 324000, 1200, 1600},
		78	{-324000, 324000, 1200, 1600},
		79	{-324000, 324000, 1200, 1600},
		80	{-324000, 324000, 1200, 1600},
381	giacomo	81	{-324000, 324000, 1200, 1600}},
		82	{{-324000, 324000, 1200, 1600},
335	giacomo	83	{-324000, 324000, 1200, 1600},
381	giacomo	84	{-324000, 324000, 1200, 1600},
		85	{-324000, 324000, 1200, 1600},
		86	{-324000, 324000, 1200, 1600},
		87	{-324000, 324000, 1200, 1600},
		88	{-324000, 324000, 1200, 1600},
		89	{-324000, 324000, 1200, 1600},
		90	{-324000, 324000, 1200, 1600},
		91	{-324000, 324000, 1200, 1600},
		92	{-324000, 324000, 1200, 1600},
		93	{-324000, 324000, 1200, 1600},
		94	{-324000, 324000, 1200, 1600},
		95	{-324000, 324000, 1200, 1600},
		96	{-324000, 324000, 1200, 1600},
		97	{-324000, 324000, 1200, 1600}},
		98	{{-324000, 324000, 1200, 1600},
		99	{-324000, 324000, 1200, 1600},
		100	{-324000, 324000, 1200, 1600},
		101	{-324000, 324000, 1200, 1600},
		102	{-324000, 324000, 1200, 1600},
		103	{-324000, 324000, 1200, 1600},
		104	{-324000, 324000, 1200, 1600},
		105	{-324000, 324000, 1200, 1600},
		106	{-324000, 324000, 1200, 1600},
		107	{-324000, 324000, 1200, 1600},
		108	{-324000, 324000, 1200, 1600},
		109	{-324000, 324000, 1200, 1600},
		110	{-324000, 324000, 1200, 1600},
		111	{-324000, 324000, 1200, 1600},
		112	{-324000, 324000, 1200, 1600},
		113	{-324000, 324000, 1200, 1600}},
		114	{{-324000, 324000, 1200, 1600},
		115	{-324000, 324000, 1200, 1600},
		116	{-324000, 324000, 1200, 1600},
		117	{-324000, 324000, 1200, 1600},
		118	{-324000, 324000, 1200, 1600},
		119	{-324000, 324000, 1200, 1600},
		120	{-324000, 324000, 1200, 1600},
		121	{-324000, 324000, 1200, 1600},
		122	{-324000, 324000, 1200, 1600},
		123	{-324000, 324000, 1200, 1600},
		124	{-324000, 324000, 1200, 1600},
		125	{-324000, 324000, 1200, 1600},
		126	{-324000, 324000, 1200, 1600},
		127	{-324000, 324000, 1200, 1600},
		128	{-324000, 324000, 1200, 1600},
		129	{-324000, 324000, 1200, 1600}}};
290	giacomo	130
734	giacomo	131	#define SERVO_TIMEOUT 20000
730	giacomo	132
717	giacomo	133	#define RXTX_BUFF_MAX 100
726	giacomo	134	volatile static BYTE RXTX_buff[4][RXTX_BUFF_MAX];
		135	volatile static BYTE *RXTX_addr[4][RXTX_BUFF_MAX];
725	giacomo	136	volatile unsigned int RX_position[4] = {0,0,0,0};
		137	volatile unsigned int TX_position[4] = {0,0,0,0};
730	giacomo	138	volatile unsigned int RX_cycle[4] = {0,0,0,0};
		139	volatile unsigned int TX_cycle[4] = {0,0,0,0};
728	giacomo	140	volatile unsigned int lock_write[4] = {0,0,0,0};
286	giacomo	141
733	giacomo	142	#define SERVO_LOCK 0
		143	#define SERVO_UNLOCK 1
		144	#define SERVO_SKIP (BYTE *)(0xFFFFFFFF)
		145
286	giacomo	146	const int BaudTable[] = {
		147	1200,
		148	2400,
		149	4800,
		150	9600,
		151	14400,
		152	19200,
		153	38400,
		154	57600,
		155	115200,
		156	-1};
		157
721	giacomo	158	void servo_rx_error(unsigned port, unsigned type) {
292	giacomo	159
734	giacomo	160	#ifdef SERVO_DEBUG
		161	kern_printf("(SERVO: BUFFER ERROR PORT:%d ERR:%d)",port,type);
		162	#endif
717	giacomo	163
281	giacomo	164	}
		165
717	giacomo	166	void servo_indication(unsigned port, BYTE data) {
727	giacomo	167
717	giacomo	168	if (RXTX_addr[port][RX_position[port]] == NULL) {
		169	if (data != RXTX_buff[port][RX_position[port]])
721	giacomo	170	servo_rx_error(port,1);
726	giacomo	171
733	giacomo	172	RXTX_addr[port][RX_position[port]] = SERVO_SKIP;
		173
717	giacomo	174	RX_position[port]++;
721	giacomo	175
		176	if (RX_position[port] >= RXTX_BUFF_MAX) {
730	giacomo	177	RX_cycle[port]++;
721	giacomo	178	RX_position[port] = 0;
		179	}
730	giacomo	180	if ((RX_cycle[port] == TX_cycle[port] && RX_position[port] > TX_position[port]) \|\|
		181	(RX_cycle[port] > TX_cycle[port]))
721	giacomo	182	servo_rx_error(port,2);
		183
717	giacomo	184	} else {
726	giacomo	185
733	giacomo	186	if (RXTX_addr[port][RX_position[port]] != SERVO_SKIP) {
721	giacomo	187
733	giacomo	188	*RXTX_addr[port][RX_position[port]] = data;
		189	RXTX_buff[port][RX_position[port]] = SERVO_UNLOCK; //Unlock the data
		190
		191	RXTX_addr[port][RX_position[port]] = SERVO_SKIP;
		192
		193	RX_position[port]++;
		194
		195	if (RX_position[port] >= RXTX_BUFF_MAX) {
730	giacomo	196	RX_cycle[port]++;
721	giacomo	197	RX_position[port] = 0;
733	giacomo	198	}
		199	if ((RX_cycle[port] == TX_cycle[port] && RX_position[port] > TX_position[port]) \|\|
		200	(RX_cycle[port] > TX_cycle[port]))
		201	servo_rx_error(port,3);
		202
		203	} else {
		204
		205	RX_position[port]++;
		206
		207	if (RX_position[port] >= RXTX_BUFF_MAX) {
		208	RX_cycle[port]++;
		209	RX_position[port] = 0;
		210	}
		211	if ((RX_cycle[port] == TX_cycle[port] && RX_position[port] > TX_position[port]) \|\|
		212	(RX_cycle[port] > TX_cycle[port]))
		213	servo_rx_error(port,3);
		214
		215	servo_indication(port, data);
		216
721	giacomo	217	}
726	giacomo	218
717	giacomo	219	}
295	giacomo	220
286	giacomo	221	}
		222
730	giacomo	223	extern unsigned SCom_Error[4];
727	giacomo	224
717	giacomo	225	void servo_confirm(unsigned port, BYTE msg_status) {
286	giacomo	226
734	giacomo	227	#ifdef SERVO_DEBUG
		228	if (msg_status == COM_ERROR) {
		229	kern_printf("(SCOM: PORT:%d ERROR:%d)",port,SCom_Error[port]);
		230	}
		231	#endif
286	giacomo	232
730	giacomo	233	lock_write[port] = 0;
727	giacomo	234
717	giacomo	235	}
		236
		237	int servo_send_msg(unsigned port, BYTE msg, unsigned len_msg, BYTE res, unsigned len_res) {
		238
726	giacomo	239	int i = 0, old, oldcycle, RX = 0;
733	giacomo	240	volatile int start = kern_gettime(NULL), timeout = 0;
721	giacomo	241	SYS_FLAGS f;
		242
733	giacomo	243	while(lock_write[port] == 1 && timeout == 0)
		244	if ((start + SERVO_TIMEOUT) < kern_gettime(NULL)) timeout = 1;
		245
		246	if (timeout == 1) {
734	giacomo	247
		248	#ifdef SERVO_DEBUG
		249	kern_printf("(SERVO: TIMEOUT SEND)");
		250	#endif
		251
733	giacomo	252	lock_write[port] = 0;
		253	return -1;
		254	}
		255
728	giacomo	256	lock_write[port] = 1;
727	giacomo	257
721	giacomo	258	f = kern_fsave();
		259
717	giacomo	260	old = TX_position[port];
730	giacomo	261	oldcycle = TX_cycle[port];
717	giacomo	262
		263	while(i < len_msg) {
		264	RXTX_buff[port][TX_position[port]] = msg[i];
		265	RXTX_addr[port][TX_position[port]] = NULL;
		266	TX_position[port]++;
721	giacomo	267	if (TX_position[port] >= RXTX_BUFF_MAX) {
730	giacomo	268	TX_cycle[port]++;
721	giacomo	269	TX_position[port] = 0;
		270	}
717	giacomo	271	if (TX_position[port] == RX_position[port]) {
		272	TX_position[port] = old;
730	giacomo	273	TX_cycle[port] = oldcycle;
		274	lock_write[port] = 0;
733	giacomo	275
734	giacomo	276	#ifdef SERVO_DEBUG
		277	kern_printf("(SERVO: BUFFER FULL)");
		278	#endif
733	giacomo	279
721	giacomo	280	kern_frestore(f);
717	giacomo	281	return -1;
		282	}
		283	i++;
		284	}
		285
		286	i = 0;
		287	while(i < len_res) {
733	giacomo	288	RXTX_buff[port][TX_position[port]] = SERVO_LOCK; //Lock the data
717	giacomo	289	RXTX_addr[port][TX_position[port]] = res+i;
726	giacomo	290	RX = TX_position[port];
717	giacomo	291	TX_position[port]++;
721	giacomo	292	if (TX_position[port] >= RXTX_BUFF_MAX) {
730	giacomo	293	TX_cycle[port]++;
721	giacomo	294	TX_position[port] = 0;
		295	}
717	giacomo	296	if (TX_position[port] == RX_position[port]) {
		297	TX_position[port] = old;
730	giacomo	298	TX_cycle[port] = oldcycle;
		299	lock_write[port] = 0;
733	giacomo	300
734	giacomo	301	#ifdef SERVO_DEBUG
		302	kern_printf("(SERVO: BUFFER FULL)");
		303	#endif
733	giacomo	304
721	giacomo	305	kern_frestore(f);
717	giacomo	306	return -1;
		307	}
		308	i++;
		309	}
		310
721	giacomo	311	kern_frestore(f);
		312
717	giacomo	313	com_irq_send(port, len_msg, msg);
		314
725	giacomo	315	return RX;
		316
		317	}
		318
733	giacomo	319	int servo_wait(unsigned port, int RX) {
730	giacomo	320
733	giacomo	321	volatile int start = kern_gettime(NULL), timeout = 0, temp;
730	giacomo	322
733	giacomo	323	/* Active wait until number bytes received */
		324	while(RXTX_buff[port][RX] == 0 && timeout == 0)
		325	if ((start + SERVO_TIMEOUT) < kern_gettime(NULL)) timeout = 1;
730	giacomo	326
733	giacomo	327	if (timeout == 1) {
		328	SYS_FLAGS f;
730	giacomo	329
733	giacomo	330	f = kern_fsave();
725	giacomo	331
734	giacomo	332	#ifdef SERVO_DEBUG
		333	kern_printf("(SERVO: TIMEOUT GET)");
		334	#endif
		335
733	giacomo	336	temp = RX;
727	giacomo	337
733	giacomo	338	while(RXTX_addr[port][temp] != SERVO_SKIP) {
		339	RXTX_addr[port][temp] = SERVO_SKIP;
		340	temp--;
		341	}
730	giacomo	342
733	giacomo	343	kern_frestore(f);
725	giacomo	344
733	giacomo	345	}
730	giacomo	346
733	giacomo	347	return timeout;
286	giacomo	348
		349	}
		350
358	giacomo	351	int servo_open(int port,int speed)
286	giacomo	352	{
		353	int err;
		354
358	giacomo	355	err = com_open((unsigned)(port), (DWORD)speed, SERVO_PARITY, SERVO_LEN, SERVO_STOP);
		356
717	giacomo	357	com_init_irq((unsigned)(port));
		358
		359	com_set_functions(servo_confirm,servo_indication);
		360
721	giacomo	361	com_irq_enable((unsigned)(port),ALL_IRQ);
		362
286	giacomo	363	return err;
		364
		365	}
		366
358	giacomo	367	int servo_close(int port)
286	giacomo	368	{
		369	int err;
		370
724	giacomo	371	com_irq_disable((unsigned)(port),ALL_IRQ);
		372
717	giacomo	373	com_close_irq((unsigned)(port));
		374
358	giacomo	375	err = com_close((unsigned)(port));
286	giacomo	376
		377	return err;
		378
		379	}
		380
		381	/* 1000.011w:bbbb.bbbb */
358	giacomo	382	int servo_set_RS232_baudrate(int port, int baud)
281	giacomo	383	{
724	giacomo	384	unsigned char b[2];
		385	int spbrg_temp, i;
286	giacomo	386	unsigned char spbrg, w;
358	giacomo	387	int servo_port = (unsigned)(port);
286	giacomo	388
		389	i = 0;
		390	while(BaudTable[i] != baud && BaudTable[i] != -1) i++;
		391	if (BaudTable[i] == -1) {
		392	kern_printf("SERVO:Error wrong baud rate\n");
		393	return -1;
		394	}
		395
358	giacomo	396	w = 1;
453	giacomo	397	spbrg_temp = (SERVO_CLOCK10 / (16baud)) - 10;
412	giacomo	398	if (spbrg_temp>2550) {
358	giacomo	399	w = 0;
453	giacomo	400	spbrg_temp = (SERVO_CLOCK10 / (64baud)) - 10;
286	giacomo	401	}
412	giacomo	402	spbrg = spbrg_temp / 10;
453	giacomo	403	if (spbrg_temp % 10 > 5) spbrg++;
412	giacomo	404
286	giacomo	405	#ifdef SERVO_DEBUG
		406	kern_printf("(SERVO:SBPRG %d W %d)",spbrg,w);
358	giacomo	407	#endif
		408
724	giacomo	409	b[0] = 0x86 \| (w & 0x01);
		410	b[1] = spbrg;
		411	servo_send_msg(servo_port, b, 2, NULL, 0);
358	giacomo	412
724	giacomo	413	return 0;
381	giacomo	414
281	giacomo	415	}
		416
285	giacomo	417	/* 1000.0101 */
358	giacomo	418	int servo_get_RS232_baudrate(int port)
281	giacomo	419	{
724	giacomo	420	unsigned char b[1],r[2];
733	giacomo	421	int res, res_w, res_b, t;
358	giacomo	422	int servo_port = (unsigned)(port);
725	giacomo	423	int RX;
358	giacomo	424
724	giacomo	425	b[0] = 0x85;
725	giacomo	426	RX = servo_send_msg(servo_port, b, 1, r, 2);
358	giacomo	427
732	giacomo	428	if (RX != -1) {
		429
733	giacomo	430	t = servo_wait(servo_port, RX);
732	giacomo	431
733	giacomo	432	if (t == 0) {
725	giacomo	433
732	giacomo	434	res_w = r[0]; /* bit W */
		435	res_b = r[1]; /* byte SPBRG */
465	giacomo	436
732	giacomo	437	if (res_w != -1 && res_b != -1) {
		438	if (res_w)
		439	res = SERVO_CLOCK / ( 16 * (res_b + 1) );
		440	else
		441	res = SERVO_CLOCK / ( 64 * (res_b + 1) );
		442	} else {
		443	return -1;
		444	}
358	giacomo	445
732	giacomo	446	return res;
358	giacomo	447
732	giacomo	448	} else return -1;
		449
		450	} else return -1;
		451
281	giacomo	452	}
		453
286	giacomo	454	/* 1000.0100 */
358	giacomo	455	int servo_store_RS232_baudrate(int port)
281	giacomo	456	{
724	giacomo	457	unsigned char b[1];
358	giacomo	458	int servo_port = (unsigned)(port);
		459
724	giacomo	460	b[0] = 0x84;
		461	servo_send_msg(servo_port, b, 1, NULL, 0);
		462
		463	return 0;
358	giacomo	464
281	giacomo	465	}
		466
289	giacomo	467	/* 1000.1010:llll.llll */
358	giacomo	468	int servo_set_period(int port, int period)
281	giacomo	469	{
724	giacomo	470	unsigned char b[2];
358	giacomo	471	int servo_port = (unsigned)(port);
		472
724	giacomo	473	b[0] = 0x8A;
		474	b[1] = (period*1000)/TICK_LEN_PERIOD/8 & 0xFF;
		475	servo_send_msg(servo_port, b, 2, NULL, 0);
381	giacomo	476
724	giacomo	477	return 0;
358	giacomo	478
281	giacomo	479	}
		480
285	giacomo	481	/* 1000.1001 */
358	giacomo	482	int servo_get_period(int port)
281	giacomo	483	{
724	giacomo	484	unsigned char b[1],r[1];
733	giacomo	485	int res, RX, t;
358	giacomo	486	int servo_port = (unsigned)(port);
		487
724	giacomo	488	b[0] = 0x89;
725	giacomo	489	RX = servo_send_msg(servo_port, b, 1, r, 1);
358	giacomo	490
732	giacomo	491	if (RX != -1) {
		492
733	giacomo	493	t = servo_wait(servo_port, RX);
732	giacomo	494
733	giacomo	495	if (t == 0) {
725	giacomo	496
732	giacomo	497	res = r[0];
381	giacomo	498
732	giacomo	499	return (((unsigned char)(res))TICK_LEN_PERIOD/10008);
358	giacomo	500
732	giacomo	501	} else return -1;
		502
		503	} else return -1;
		504
281	giacomo	505	}
		506
289	giacomo	507	/* 1000.1000 */
358	giacomo	508	int servo_store_period(int port)
281	giacomo	509	{
724	giacomo	510	unsigned char b[1];
358	giacomo	511	int servo_port = (unsigned)(port);
		512
724	giacomo	513	b[0] = 0x88;
		514	servo_send_msg(servo_port, b, 1, NULL, 0);
358	giacomo	515
724	giacomo	516	return 0;
381	giacomo	517
281	giacomo	518	}
		519
283	giacomo	520	/* 1000.1100 */
358	giacomo	521	int servo_get_setup_switch(int port)
281	giacomo	522	{
724	giacomo	523	unsigned char b[1],r[1];
733	giacomo	524	int res, RX, t;
358	giacomo	525	int servo_port = (unsigned)(port);
		526
724	giacomo	527	b[0] = 0x8C;
725	giacomo	528	RX = servo_send_msg(servo_port, b, 1, r, 1);
281	giacomo	529
732	giacomo	530	if (RX != -1) {
		531
733	giacomo	532	t = servo_wait(servo_port, RX);
732	giacomo	533
733	giacomo	534	if (t == 0) {
725	giacomo	535
732	giacomo	536	res = r[0];
281	giacomo	537
732	giacomo	538	return res;
283	giacomo	539
732	giacomo	540	} else return -1;
		541
		542	} else return -1;
		543
281	giacomo	544	}
		545
283	giacomo	546	/* 1000.111s */
358	giacomo	547	int servo_set_RC5_switch(int port, int data)
281	giacomo	548	{
724	giacomo	549	unsigned char b[1];
358	giacomo	550	int servo_port = (unsigned)(port);
		551
724	giacomo	552	b[0] = 0x8E \| (data & 0x01);
		553	servo_send_msg(servo_port, b, 1, NULL, 0);
281	giacomo	554
724	giacomo	555	return 0;
381	giacomo	556
281	giacomo	557	}
		558
323	giacomo	559	/* 1000.0000:0000.Mmmm */
358	giacomo	560	int servo_turn_off(int port, int servo)
285	giacomo	561	{
724	giacomo	562	unsigned char b[2];
358	giacomo	563	int servo_port = (unsigned)(port);
		564
		565	if (servo > 15) return -1;
724	giacomo	566
		567	b[0] = 0x80;
		568	b[1] = 0x00 \| (servo & 0x0F);
		569	servo_send_msg(servo_port, b, 2, NULL, 0);
381	giacomo	570
724	giacomo	571	return 0;
285	giacomo	572
		573	}
		574
325	giacomo	575	/* 1000.0000:0001.Mmmm */
358	giacomo	576	int servo_turn_on(int port, int servo)
285	giacomo	577	{
721	giacomo	578	unsigned char b[2];
358	giacomo	579	int servo_port = (unsigned)(port);
		580
323	giacomo	581	if (servo > 15) return -1;
358	giacomo	582
721	giacomo	583	b[0] = 0x80;
		584	b[1] = 0x10 \| (servo & 0x0F);
		585	servo_send_msg(servo_port, b, 2, NULL, 0);
358	giacomo	586
721	giacomo	587	return 0;
358	giacomo	588
285	giacomo	589	}
		590
291	giacomo	591	/* 1000.0000:0010.0000 */
358	giacomo	592	int servo_turn_off_all(int port)
323	giacomo	593	{
724	giacomo	594	unsigned char b[2];
358	giacomo	595	int servo_port = (unsigned)(port);
		596
724	giacomo	597	b[0] = 0x80;
		598	b[1] = 0x20;
		599	servo_send_msg(servo_port, b, 2, NULL, 0);
358	giacomo	600
724	giacomo	601	return 0;
358	giacomo	602
285	giacomo	603	}
		604
291	giacomo	605	/* 1000.0000:0010.0001 */
358	giacomo	606	int servo_turn_on_all(int port)
285	giacomo	607	{
724	giacomo	608	unsigned char b[2];
358	giacomo	609	int servo_port = (unsigned)(port);
		610
724	giacomo	611	b[0] = 0x80;
		612	b[1] = 0x21;
		613	servo_send_msg(servo_port, b, 2, NULL, 0);
		614
		615	return 0;
358	giacomo	616
285	giacomo	617	}
		618
323	giacomo	619	/* 1000.0000:0101.000M:mmmm.mmmm */
724	giacomo	620	int servo_set_levels(int port, int bank, int mask)
315	giacomo	621	{
724	giacomo	622	unsigned char b[3];
358	giacomo	623	int servo_port = (unsigned)(port);
		624
724	giacomo	625	b[0] = 0x80;
		626	b[1] = 0x50 \| (0x01 & bank);
		627	b[2] = (unsigned char)(mask & 0xFF);
		628	servo_send_msg(servo_port, b, 3, NULL, 0);
358	giacomo	629
724	giacomo	630	return 0;
358	giacomo	631
315	giacomo	632	}
		633
323	giacomo	634	/* 1000.0000:0100.000M */
358	giacomo	635	int servo_get_levels(int port, int bank)
297	giacomo	636	{
724	giacomo	637	unsigned char b[2],r[1];
733	giacomo	638	int res, RX, t;
358	giacomo	639	int servo_port = (unsigned)(port);
		640
724	giacomo	641	b[0] = 0x80;
		642	b[1] = 0x40 \| (0x01 & bank);
725	giacomo	643	RX = servo_send_msg(servo_port, b, 2, r, 1);
		644
732	giacomo	645	if (RX != -1) {
		646
733	giacomo	647	t = servo_wait(servo_port, RX);
732	giacomo	648
733	giacomo	649	if (t == 0) {
358	giacomo	650
732	giacomo	651	res = r[0];
358	giacomo	652
732	giacomo	653	return res;
		654
		655	} else return -1;
		656
		657	} else return -1;
		658
297	giacomo	659	}
		660
		661	/* 1000.0000:1000.0000 */
358	giacomo	662	int servo_store_levels(int port)
297	giacomo	663	{
724	giacomo	664	unsigned char b[2];
358	giacomo	665	int servo_port = (unsigned)(port);
		666
724	giacomo	667	b[0] = 0x80;
		668	b[1] = 0x80;
		669	servo_send_msg(servo_port, b, 2, NULL, 0);
381	giacomo	670
724	giacomo	671	return 0;
358	giacomo	672
297	giacomo	673	}
		674
493	giacomo	675	int servo_set_max_angle_sec(int port, int servo, int angle_sec)
290	giacomo	676	{
		677
381	giacomo	678	servo_table[port][servo].max_angle_sec = angle_sec;
290	giacomo	679	return 0;
		680
		681	}
		682
493	giacomo	683	int servo_set_min_angle_sec(int port, int servo, int angle_sec)
290	giacomo	684	{
		685
381	giacomo	686	servo_table[port][servo].min_angle_sec = angle_sec;
290	giacomo	687	return 0;
		688
		689	}
		690
381	giacomo	691	int servo_set_servo_tick(int port, int servo, int zero_tick, int delta_tick)
		692	{
		693
		694	if (zero_tick != -1) servo_table[port][servo].zero_tick = zero_tick;
		695	if (delta_tick != -1) servo_table[port][servo].delta_tick = delta_tick;
		696	return 0;
		697
		698	}
		699
323	giacomo	700	/* 0000.Pppp:0000.vvvv:vvvv.vvvv */
358	giacomo	701	int servo_set_angle_sec(int port, int servo, int angle_sec)
281	giacomo	702	{
717	giacomo	703	unsigned char b[3];
730	giacomo	704	int angle_tick, res = 0;
358	giacomo	705	int servo_port = (unsigned)(port);
282	giacomo	706
323	giacomo	707	if (servo > 15) return -1;
282	giacomo	708
381	giacomo	709	angle_tick = (servo_table[port][servo].zero_tick + angle_sec *
		710	servo_table[port][servo].delta_tick /
542	giacomo	711	(servo_table[port][servo].max_angle_sec - servo_table[port][servo].min_angle_sec)) * 1000 / TICK_LEN;
290	giacomo	712
717	giacomo	713	b[0] = 0x00 \| (servo & 0x0F);
		714	b[1] = 0x00 \| ((angle_tick >> 8) & 0x0F);
		715	b[2] = 0x00 \| (angle_tick & 0xFF);
730	giacomo	716	res = servo_send_msg(servo_port, b, 3, NULL, 0);
358	giacomo	717
730	giacomo	718	return res;
282	giacomo	719
281	giacomo	720	}
		721
323	giacomo	722	/* 0010.Pppp */
358	giacomo	723	int servo_store_default_position(int port, int servo)
290	giacomo	724	{
724	giacomo	725	unsigned char b[1];
730	giacomo	726	int res = 0;
358	giacomo	727	int servo_port = (unsigned)(port);
290	giacomo	728
323	giacomo	729	if (servo > 15) return -1;
358	giacomo	730
724	giacomo	731	b[0] = 0x20 \| (servo & 0x0F);
730	giacomo	732	res = servo_send_msg(servo_port, b, 1, NULL, 0);
358	giacomo	733
730	giacomo	734	return res;
358	giacomo	735
290	giacomo	736	}
		737
323	giacomo	738	/* 0001.Pppp */
358	giacomo	739	int servo_get_angle_sec(int port, int servo)
281	giacomo	740	{
724	giacomo	741	unsigned char b[1],r[2];
733	giacomo	742	int res,data,RX,t;
358	giacomo	743	int servo_port = (unsigned)(port);
		744
323	giacomo	745	if (servo > 15) return -1;
358	giacomo	746
724	giacomo	747	b[0] = 0x10 \| (servo & 0x0F);
725	giacomo	748	RX = servo_send_msg(servo_port, b, 1, r, 2);
281	giacomo	749
730	giacomo	750	if (RX != -1) {
725	giacomo	751
733	giacomo	752	t = servo_wait(servo_port, RX);
282	giacomo	753
733	giacomo	754	if (t == 0) {
290	giacomo	755
730	giacomo	756	res = (int)(r[0]) << 8;
		757	res \|= r[1];
282	giacomo	758
730	giacomo	759	data = ((resTICK_LEN/1000) - servo_table[port][servo].zero_tick)
		760	(servo_table[port][servo].max_angle_sec - servo_table[port][servo].min_angle_sec) /
		761	servo_table[port][servo].delta_tick;
		762
		763	return data;
		764
		765	} else return -1;
		766
		767	} else return -1;
		768
281	giacomo	769	}
		770
282	giacomo	771	/* 0100:0aaa */
358	giacomo	772	int servo_get_analog(int port, int adport)
281	giacomo	773	{
724	giacomo	774	unsigned char b[1],r[2];
733	giacomo	775	int res, RX, t;
358	giacomo	776	int servo_port = (unsigned)(port);
		777
381	giacomo	778	if (adport > 7) return -1;
358	giacomo	779
724	giacomo	780	b[0] = 0x40 \| (adport & 0x07);
725	giacomo	781	RX = servo_send_msg(servo_port, b, 1, r, 2);
281	giacomo	782
732	giacomo	783	if (RX != -1) {
725	giacomo	784
733	giacomo	785	t = servo_wait(servo_port, RX);
282	giacomo	786
733	giacomo	787	if (t == 0) {
282	giacomo	788
732	giacomo	789	res = (int)r[0] << 8;
		790	res \|= r[1];
		791
		792	return res;
		793
		794	} else return -1;
		795
		796	} else return -1;
		797
281	giacomo	798	}
		799

Subversion Repositories shark

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