WebSVN - shark - Blame - Rev 295 - /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>
		10	* (see the web pages for full authors list)
		11	*
		12	* ReTiS Lab (Scuola Superiore S.Anna - Pisa - Italy)
		13	*
		14	* http://www.sssup.it
		15	* http://retis.sssup.it
		16	* http://shark.sssup.it
		17	*/
		18
		19	/*
		20	* Copyright (C) 2002 Paolo Gai
		21	*
		22	* This program is free software; you can redistribute it and/or modify
		23	* it under the terms of the GNU General Public License as published by
		24	* the Free Software Foundation; either version 2 of the License, or
		25	* (at your option) any later version.
		26	*
		27	* This program is distributed in the hope that it will be useful,
		28	* but WITHOUT ANY WARRANTY; without even the implied warranty of
		29	* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
		30	* GNU General Public License for more details.
		31	*
		32	* You should have received a copy of the GNU General Public License
		33	* along with this program; if not, write to the Free Software
		34	* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
		35	*
		36	*/
		37
		38	#include <ll/sys/ll/ll-instr.h>
282	giacomo	39	#include "kernel/kern.h"
281	giacomo	40	#include "servo.h"
		41
		42	#define THR 0
		43	#define RBR 0
		44	#define IER 1
		45	#define FCR 2
		46	#define IIR 2
		47	#define LCR 3
		48	#define MCR 4
		49	#define LSR 5
		50	#define MSR 6
		51	#define SPad 7
		52
286	giacomo	53	/* Parity value */
		54	#define NONE 0
		55	#define ODD 1
		56	#define EVEN 3
		57
281	giacomo	58	#define barrier() __asm__ __volatile__("" ::: "memory");
		59
286	giacomo	60	#define SERVO_DEBUG
		61
282	giacomo	62	#define SERVO_TIMEOUT 100000 /* us */
281	giacomo	63
286	giacomo	64	#define SERVO_SPEED 38400
		65	#define SERVO_PARITY NONE
		66	#define SERVO_LEN 8
		67	#define SERVO_STOP 1
282	giacomo	68
290	giacomo	69	#define TICK_LEN 1600 /* us */
		70
		71	struct servo_data {
		72	int min_angle_sec;
		73	int max_angle_sec;
		74	int delta_tick;
		75	int zero_tick;
		76	};
		77
		78	struct servo_data servo_table[] = {
		79	{-324000, 324000, 1800, 1600},
		80	{-324000, 324000, 1800, 1600},
		81	{-324000, 324000, 1800, 1600},
		82	{-324000, 324000, 1800, 1600},
		83	{-324000, 324000, 1800, 1600},
		84	{-324000, 324000, 1800, 1600},
		85	{-324000, 324000, 1800, 1600},
		86	{-324000, 324000, 1800, 1600}};
		87
281	giacomo	88	int timer_expired = 0;
283	giacomo	89	int timeout_event;
289	giacomo	90	int servo_port;
281	giacomo	91
286	giacomo	92	const unsigned com_base[] = {0x03F8,0x02F8,0x03E8,0x02E8};
		93
		94	const int BaudTable[] = {
		95	1200,
		96	2400,
		97	4800,
		98	9600,
		99	14400,
		100	19200,
		101	38400,
		102	57600,
		103	115200,
		104	-1};
		105
		106	/* 20MHz: SPBRG, BAUD BRGH=0, BAUD BRGH=1 */
		107	const int Baud20MhzData[] = {
		108	255, 1221, 4882,
		109	254, 1225, 4902,
		110	253, 1230, 4921,
		111	252, 1235, 4941,
		112	133, 2332, 9328,
		113	132, 2350, 9398,
		114	131, 2367, 9470,
		115	130, 2385, 9542,
		116	129, 2404, 9615,
		117	128, 2422, 9690,
		118	127, 2441, 9766,
		119	126, 2461, 9842,
		120	88, 0, 14045,
		121	87, 0, 14204,
		122	86, 0, 14368,
		123	85, 0, 14535,
		124	84, 0, 14706,
		125	66, 4664, 18657,
		126	65, 4735, 18939,
		127	64, 4808, 19231,
		128	63, 4883, 19531,
		129	32, 9470, 37879,
		130	31, 9766, 39062,
		131	21, 14205, 56818,
		132	15, 19531, 0,
		133	10, 0, 113636,
		134	7, 39062, 0,
		135	-1, -1, -1};
		136
282	giacomo	137	void set_timer_expired(void *arg)
281	giacomo	138	{
283	giacomo	139	timeout_event = NIL;
281	giacomo	140	timer_expired = 1;
		141	}
		142
		143	unsigned com_read(unsigned port,unsigned reg)
		144	{
		145	unsigned b;
		146	if (port > 3 \|\| reg > 7) return(0);
		147	b = ll_in(com_base[port]+reg);
		148	return(b);
		149	}
		150
		151	void com_write(unsigned port,unsigned reg,unsigned value)
		152	{
		153	if (port > 3 \|\| reg > 7) return;
		154	ll_out(com_base[port]+reg,value);
		155	}
		156
		157	int com_send(unsigned port,BYTE b)
		158	{
		159	while ((com_read(port,LSR) & 32) == 0 && !timer_expired)
		160	barrier();
		161	if (!timer_expired) {
292	giacomo	162	#ifdef SERVO_DEBUG
295	giacomo	163	kern_printf("(SERVO WRITE p = %d b = %02x)",port,b);
292	giacomo	164	#endif
281	giacomo	165	com_write(port,THR,b);
		166	return 0;
		167	} else {
292	giacomo	168	#ifdef SERVO_DEBUG
295	giacomo	169	kern_printf("(WRITE TIMEOUT)");
292	giacomo	170	#endif
281	giacomo	171	return -1;
		172	}
		173	}
		174
		175	int com_receive(unsigned port)
		176	{
292	giacomo	177	int b;
		178
281	giacomo	179	while ((com_read(port,LSR) & 1) == 0 && !timer_expired)
		180	barrier();
		181	if (!timer_expired) {
292	giacomo	182	b = (int)(com_read(port,RBR));
		183	#ifdef SERVO_DEBUG
295	giacomo	184	kern_printf("(SERVO READ p = %d b = %02x)",port,b);
292	giacomo	185	#endif
		186	return b;
281	giacomo	187	} else {
292	giacomo	188	#ifdef SERVO_DEBUG
295	giacomo	189	kern_printf("(READ TIMEOUT)");
292	giacomo	190	#endif
281	giacomo	191	return -1;
		192	}
		193	}
		194
286	giacomo	195	int com_open(unsigned port,DWORD speed,BYTE parity,BYTE len,BYTE stop)
		196	{
		197	unsigned long div,b_mask;
		198
		199	/* Now set up the serial link */
		200	b_mask = (parity & 3) * 8 + (stop & 1) * 4 + ((len - 5) & 3);
		201	div = 115200L / speed;
		202	/* Clear serial interrupt enable register */
		203	com_write(port,IER,0);
		204	/* Empty input buffer */
		205	com_read(port,RBR);
		206	/* Activate DLAB bit for speed setting */
		207	com_write(port,LCR,0x80);
		208	/* Load baud divisor */
		209	com_write(port,0,div & 0x00FF);
		210	div >>= 8;
		211	com_write(port,1,div & 0x00FF);
		212	/* Load control word (parity,stop bit,bit len) */
		213	com_write(port,LCR,b_mask);
		214	/* Attiva OUT1 & OUT2 */
		215	com_write(port,MCR,0x0C);
		216
		217	return 0;
295	giacomo	218
286	giacomo	219	}
		220
		221	int com_close(unsigned port)
		222	{
		223
		224	com_write(port,IER,0);
		225	com_read(port,IIR);
		226	com_read(port,RBR);
		227
		228	return 0;
		229
		230	}
		231
289	giacomo	232	int servo_open(int port)
286	giacomo	233	{
		234	int err;
		235
289	giacomo	236	servo_port = (unsigned)(port);
		237	err = com_open(servo_port, SERVO_SPEED, SERVO_PARITY, SERVO_LEN, SERVO_STOP);
286	giacomo	238
		239	return err;
		240
		241	}
		242
		243	int servo_close(void)
		244	{
		245	int err;
		246
289	giacomo	247	err = com_close(servo_port);
286	giacomo	248
		249	return err;
		250
		251	}
		252
		253	/* 1000.011w:bbbb.bbbb */
281	giacomo	254	int servo_set_RS232_baudrate(int baud)
		255	{
285	giacomo	256	struct timespec current_time;
		257	unsigned char b;
286	giacomo	258	int err,diff,mindiff,i;
		259	unsigned char spbrg, w;
		260
		261	i = 0;
		262	while(BaudTable[i] != baud && BaudTable[i] != -1) i++;
		263	if (BaudTable[i] == -1) {
		264	kern_printf("SERVO:Error wrong baud rate\n");
		265	return -1;
		266	}
		267
		268	i = 0;
		269	mindiff = 0xFFFFFF;
		270	spbrg = 0;
		271	w = 0;
		272	while(Baud20MhzData[i] != -1) {
		273	diff = abs(Baud20MhzData[i+1] - baud);
		274	if (diff < mindiff) {
		275	mindiff = diff;
		276	spbrg = (unsigned char)(Baud20MhzData[i+1]);
		277	w = 0;
		278	}
		279	diff = abs(Baud20MhzData[i+2] - baud);
		280	if (diff < mindiff) {
		281	mindiff = diff;
		282	spbrg = (unsigned char)(Baud20MhzData[i+2]);
		283	w = 1;
		284	}
		285	i += 3;
		286	}
		287
		288	#ifdef SERVO_DEBUG
		289	kern_printf("(SERVO:SBPRG %d W %d)",spbrg,w);
		290	#endif
		291
285	giacomo	292	timer_expired = 0;
		293	kern_gettime(&current_time);
		294	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		295	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		296
286	giacomo	297	b = 0x86 \| (w & 0x01);
289	giacomo	298	err = com_send(servo_port, b);
		299	err = com_receive(servo_port);
285	giacomo	300	if (err != (int)(b)) timer_expired = 1;
281	giacomo	301
286	giacomo	302	b = spbrg;
289	giacomo	303	err = com_send(servo_port, b);
		304	err = com_receive(servo_port);
285	giacomo	305	if (err != (int)(b)) timer_expired = 1;
281	giacomo	306
285	giacomo	307	if (timeout_event != NIL) kern_event_delete(timeout_event);
286	giacomo	308
289	giacomo	309	com_close(servo_port);
		310	com_open(servo_port, baud, SERVO_PARITY, SERVO_LEN, SERVO_STOP);
286	giacomo	311
285	giacomo	312	if (!timer_expired)
		313	return 0;
		314	else
		315	return -1;
		316
281	giacomo	317	}
		318
285	giacomo	319	/* 1000.0101 */
281	giacomo	320	int servo_get_RS232_baudrate(void)
		321	{
285	giacomo	322	struct timespec current_time;
		323	unsigned char b;
		324	int err,res;
		325
		326	timer_expired = 0;
		327	kern_gettime(&current_time);
		328	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		329	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		330
		331	b = 0x85;
289	giacomo	332	err = com_send(servo_port, b);
		333	err = com_receive(servo_port);
285	giacomo	334	if (err != (int)(b)) timer_expired = 1;
289	giacomo	335	res = com_receive(servo_port);
285	giacomo	336
		337	if (timeout_event != NIL) kern_event_delete(timeout_event);
		338
		339	if (!timer_expired)
		340	return res;
		341	else
		342	return -1;
281	giacomo	343
		344	}
		345
286	giacomo	346	/* 1000.0100 */
281	giacomo	347	int servo_store_RS232_buadrate(void)
		348	{
285	giacomo	349	struct timespec current_time;
		350	unsigned char b;
		351	int err;
		352
		353	timer_expired = 0;
		354	kern_gettime(&current_time);
		355	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		356	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		357
286	giacomo	358	b = 0x84;
289	giacomo	359	err = com_send(servo_port, b);
		360	err = com_receive(servo_port);
285	giacomo	361	if (err != (int)(b)) timer_expired = 1;
		362
		363	if (timeout_event != NIL) kern_event_delete(timeout_event);
		364
		365	if (!timer_expired)
		366	return 0;
		367	else
		368	return -1;
281	giacomo	369
		370	}
		371
289	giacomo	372	/* 1000.1010:llll.llll */
281	giacomo	373	int servo_set_period(int period)
		374	{
285	giacomo	375	struct timespec current_time;
		376	unsigned char b;
		377	int err;
		378
		379	timer_expired = 0;
		380	kern_gettime(&current_time);
		381	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		382	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		383
289	giacomo	384	b = 0x8A;
		385	err = com_send(servo_port, b);
		386	err = com_receive(servo_port);
285	giacomo	387	if (err != (int)(b)) timer_expired = 1;
		388
290	giacomo	389	b = period/TICK_LEN/8 & 0xFF;
289	giacomo	390	err = com_send(servo_port, b);
		391	err = com_receive(servo_port);
285	giacomo	392	if (err != (int)(b)) timer_expired = 1;
		393
		394	if (timeout_event != NIL) kern_event_delete(timeout_event);
		395
		396	if (!timer_expired)
		397	return 0;
		398	else
		399	return -1;
281	giacomo	400
		401	}
		402
285	giacomo	403	/* 1000.1001 */
281	giacomo	404	int servo_get_period(void)
		405	{
285	giacomo	406	struct timespec current_time;
		407	unsigned char b;
		408	int err,res;
		409
		410	timer_expired = 0;
		411	kern_gettime(&current_time);
		412	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		413	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		414
		415	b = 0x89;
289	giacomo	416	err = com_send(servo_port, b);
		417	err = com_receive(servo_port);
285	giacomo	418	if (err != (int)(b)) timer_expired = 1;
289	giacomo	419	res = com_receive(servo_port);
285	giacomo	420
		421	if (timeout_event != NIL) kern_event_delete(timeout_event);
		422
		423	if (!timer_expired)
290	giacomo	424	return (resTICK_LEN8);
285	giacomo	425	else
		426	return -1;
281	giacomo	427
		428	}
		429
289	giacomo	430	/* 1000.1000 */
281	giacomo	431	int servo_store_period(void)
		432	{
285	giacomo	433	struct timespec current_time;
		434	unsigned char b;
		435	int err;
		436
		437	timer_expired = 0;
		438	kern_gettime(&current_time);
		439	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		440	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		441
289	giacomo	442	b = 0x88;
		443	err = com_send(servo_port, b);
		444	err = com_receive(servo_port);
285	giacomo	445	if (err != (int)(b)) timer_expired = 1;
		446
		447	if (timeout_event != NIL) kern_event_delete(timeout_event);
		448
		449	if (!timer_expired)
		450	return 0;
		451	else
		452	return -1;
281	giacomo	453
		454	}
		455
283	giacomo	456	/* 1000.1100 */
281	giacomo	457	int servo_get_setup_switch(void)
		458	{
283	giacomo	459	struct timespec current_time;
		460	unsigned char b;
		461	int err,res;
		462	timer_expired = 0;
		463	kern_gettime(&current_time);
		464	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		465	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
281	giacomo	466
283	giacomo	467	b = 0x8C;
289	giacomo	468	err = com_send(servo_port, b);
		469	err = com_receive(servo_port);
283	giacomo	470	if (err != (int)(b)) timer_expired = 1;
289	giacomo	471	res = com_receive(servo_port);
281	giacomo	472
283	giacomo	473	if (timeout_event != NIL) kern_event_delete(timeout_event);
		474
		475	if (!timer_expired)
		476	return res;
		477	else
		478	return -1;
		479
281	giacomo	480	}
		481
283	giacomo	482	/* 1000.111s */
281	giacomo	483	int servo_set_RC5_switch(int data)
		484	{
283	giacomo	485	struct timespec current_time;
		486	unsigned char b;
		487	int err;
		488
		489	timer_expired = 0;
		490	kern_gettime(&current_time);
		491	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		492	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
281	giacomo	493
283	giacomo	494	b = 0x8E \| (data & 0x01);
289	giacomo	495	err = com_send(servo_port, b);
		496	err = com_receive(servo_port);
283	giacomo	497	if (err != (int)(b)) timer_expired = 1;
		498
		499	if (timeout_event != NIL) kern_event_delete(timeout_event);
		500
		501	if (!timer_expired)
		502	return 0;
		503	else
		504	return -1;
		505
281	giacomo	506	}
		507
285	giacomo	508	/* 1000.0000:0000.0mmm */
		509	int servo_turn_off(int servo)
		510	{
		511
		512	struct timespec current_time;
		513	unsigned char b;
		514	int err;
		515
		516	if (servo > 7) return -1;
		517
		518	timer_expired = 0;
		519	kern_gettime(&current_time);
		520	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		521	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		522
		523	b = 0x80;
289	giacomo	524	err = com_send(servo_port, b);
		525	err = com_receive(servo_port);
285	giacomo	526	if (err != (int)(b)) timer_expired = 1;
		527
		528	b = 0x00 \| (servo & 0x07);
289	giacomo	529	err = com_send(servo_port, b);
		530	err = com_receive(servo_port);
285	giacomo	531	if (err != (int)(b)) timer_expired = 1;
		532
		533	if (timeout_event != NIL) kern_event_delete(timeout_event);
		534
		535	if (!timer_expired)
		536	return 0;
		537	else
		538	return -1;
		539
		540	}
		541
291	giacomo	542	/* 1000.0000:0001.0mmm */
285	giacomo	543	int servo_turn_on(int servo)
		544	{
		545
		546	struct timespec current_time;
		547	unsigned char b;
		548	int err;
		549
		550	if (servo > 7) return -1;
		551
		552	timer_expired = 0;
		553	kern_gettime(&current_time);
		554	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		555	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		556
		557	b = 0x80;
289	giacomo	558	err = com_send(servo_port, b);
		559	err = com_receive(servo_port);
285	giacomo	560	if (err != (int)(b)) timer_expired = 1;
		561
291	giacomo	562	b = 0x10 \| (servo & 0x07);
289	giacomo	563	err = com_send(servo_port, b);
		564	err = com_receive(servo_port);
285	giacomo	565	if (err != (int)(b)) timer_expired = 1;
		566
		567	if (timeout_event != NIL) kern_event_delete(timeout_event);
		568
		569	if (!timer_expired)
		570	return 0;
		571	else
		572	return -1;
		573
		574	}
		575
291	giacomo	576	/* 1000.0000:0010.0000 */
285	giacomo	577	int servo_turn_off_all(void)
		578	{ struct timespec current_time;
		579	unsigned char b;
		580	int err;
		581
		582	timer_expired = 0;
		583	kern_gettime(&current_time);
		584	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		585	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		586
		587	b = 0x80;
289	giacomo	588	err = com_send(servo_port, b);
		589	err = com_receive(servo_port);
285	giacomo	590	if (err != (int)(b)) timer_expired = 1;
		591
291	giacomo	592	b = 0x20;
289	giacomo	593	err = com_send(servo_port, b);
		594	err = com_receive(servo_port);
285	giacomo	595	if (err != (int)(b)) timer_expired = 1;
		596
		597	if (timeout_event != NIL) kern_event_delete(timeout_event);
		598
		599	if (!timer_expired)
		600	return 0;
		601	else
		602	return -1;
		603
		604	}
		605
291	giacomo	606	/* 1000.0000:0010.0001 */
285	giacomo	607	int servo_turn_on_all(void)
		608	{
		609	struct timespec current_time;
		610	unsigned char b;
		611	int err;
		612
		613	timer_expired = 0;
		614	kern_gettime(&current_time);
		615	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		616	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		617
		618	b = 0x80;
289	giacomo	619	err = com_send(servo_port, b);
		620	err = com_receive(servo_port);
285	giacomo	621	if (err != (int)(b)) timer_expired = 1;
		622
291	giacomo	623	b = 0x21;
289	giacomo	624	err = com_send(servo_port, b);
		625	err = com_receive(servo_port);
285	giacomo	626	if (err != (int)(b)) timer_expired = 1;
		627
		628	if (timeout_event != NIL) kern_event_delete(timeout_event);
		629
		630	if (!timer_expired)
		631	return 0;
		632	else
		633	return -1;
		634
		635	}
		636
290	giacomo	637	int servo_set_max_angle(int servo, int angle_sec)
		638	{
		639
		640	servo_table[servo].max_angle_sec = angle_sec;
		641	return 0;
		642
		643	}
		644
		645	int servo_set_min_angle(int servo, int angle_sec)
		646	{
		647
		648	servo_table[servo].min_angle_sec = angle_sec;
		649	return 0;
		650
		651	}
		652
282	giacomo	653	/* 0000.0ppp:0000.vvvv:vvvv.vvvv */
290	giacomo	654	int servo_set_angle_sec(int servo, int angle_sec)
281	giacomo	655	{
282	giacomo	656	struct timespec current_time;
		657	unsigned char b;
290	giacomo	658	int err, angle_tick;
282	giacomo	659
		660	if (servo > 7) return -1;
		661
290	giacomo	662	if (angle_sec > servo_table[servo].max_angle_sec \|\|
		663	angle_sec < servo_table[servo].min_angle_sec) return -1;
		664
		665	angle_tick = servo_table[servo].zero_tick + angle_sec * servo_table[servo].delta_tick /
		666	(servo_table[servo].max_angle_sec - servo_table[servo].min_angle_sec);
		667
282	giacomo	668	timer_expired = 0;
		669	kern_gettime(&current_time);
		670	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		671	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		672
		673	b = 0x00 \| (servo & 0x07);
289	giacomo	674	err = com_send(servo_port, b);
		675	err = com_receive(servo_port);
283	giacomo	676	if (err != (int)(b)) timer_expired = 1;
282	giacomo	677
290	giacomo	678	b = 0x00 \| ((angle_tick >> 8) & 0x0F);
289	giacomo	679	err = com_send(servo_port, b);
		680	err = com_receive(servo_port);
283	giacomo	681	if (err != (int)(b)) timer_expired = 1;
282	giacomo	682
290	giacomo	683	b = 0x00 \| (angle_tick & 0xFF);
289	giacomo	684	err = com_send(servo_port, b);
		685	err = com_receive(servo_port);
283	giacomo	686	if (err != (int)(b)) timer_expired = 1;
282	giacomo	687
283	giacomo	688	if (timeout_event != NIL) kern_event_delete(timeout_event);
282	giacomo	689
283	giacomo	690	if (!timer_expired)
		691	return 0;
		692	else
		693	return -1;
281	giacomo	694
		695	}
		696
291	giacomo	697	/* 0010.0ppp */
290	giacomo	698	int servo_store_default_position(int servo)
		699	{
		700	struct timespec current_time;
		701	unsigned char b;
		702	int err;
		703
		704	if (servo > 7) return -1;
		705
		706	timer_expired = 0;
		707	kern_gettime(&current_time);
		708	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		709	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		710
291	giacomo	711	b = 0x20 \| (servo & 0x07);
290	giacomo	712	err = com_send(servo_port, b);
		713	err = com_receive(servo_port);
		714	if (err != (int)(b)) timer_expired = 1;
		715
		716	if (timeout_event != NIL) kern_event_delete(timeout_event);
		717
		718	if (!timer_expired)
		719	return 0;
		720	else
		721	return -1;
		722
		723	}
		724
291	giacomo	725	/* 0001.0ppp */
290	giacomo	726	int servo_get_angle_sec(int servo)
281	giacomo	727	{
282	giacomo	728	struct timespec current_time;
		729	unsigned char b;
290	giacomo	730	int err,res,data;
282	giacomo	731
		732	if (servo > 7) return -1;
		733
		734	timer_expired = 0;
		735	kern_gettime(&current_time);
		736	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		737	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
281	giacomo	738
291	giacomo	739	b = 0x10 \| (servo & 0x07);
289	giacomo	740	err = com_send(servo_port, b);
		741	err = com_receive(servo_port);
283	giacomo	742	if (err != (int)(b)) timer_expired = 1;
289	giacomo	743	res = com_receive(servo_port) << 8;
		744	res \|= com_receive(servo_port);
282	giacomo	745
283	giacomo	746	if (timeout_event != NIL) kern_event_delete(timeout_event);
282	giacomo	747
290	giacomo	748	data = (res - servo_table[servo].zero_tick) *
		749	(servo_table[servo].max_angle_sec - servo_table[servo].min_angle_sec) /
		750	servo_table[servo].delta_tick;
		751
282	giacomo	752	if (!timer_expired)
290	giacomo	753	return data;
282	giacomo	754	else
		755	return -1;
		756
281	giacomo	757	}
		758
282	giacomo	759	/* 0100:0aaa */
281	giacomo	760	int servo_get_analog(int port)
		761	{
		762
282	giacomo	763	struct timespec current_time;
		764	unsigned char b;
283	giacomo	765	int err,res;
282	giacomo	766
		767	if (port > 4) return -1;
		768
		769	timer_expired = 0;
		770	kern_gettime(&current_time);
		771	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		772	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
281	giacomo	773
282	giacomo	774	b = 0x40 \| (port & 7);
289	giacomo	775	err = com_send(servo_port, b);
		776	err = com_receive(servo_port);
283	giacomo	777	if (err != (int)(b)) timer_expired = 1;
289	giacomo	778	res = com_receive(servo_port) << 8;
		779	res \|= com_receive(servo_port);
282	giacomo	780
283	giacomo	781	if (timeout_event != NIL) kern_event_delete(timeout_event);
282	giacomo	782
		783	if (!timer_expired)
		784	return res;
		785	else
		786	return -1;
		787
281	giacomo	788	}
		789

Subversion Repositories shark

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