WebSVN - shark - Blame - Rev 292 - /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
		163	kern_printf("(SERVO WRITE p = %d b = %02X)",port,b);
		164	#endif
281	giacomo	165	com_write(port,THR,b);
		166	return 0;
		167	} else {
292	giacomo	168	#ifdef SERVO_DEBUG
		169	kern_printf("(SERVO TIMEOUT)");
		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
		184	kern_printf("(SERVO READ p = %d b = %02X)",port,b);
		185	#endif
		186	return b;
281	giacomo	187	} else {
292	giacomo	188	#ifdef SERVO_DEBUG
		189	kern_printf("(SERVO TIMEOUT)");
		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;
		218	}
		219
		220	int com_close(unsigned port)
		221	{
		222
		223	com_write(port,IER,0);
		224	com_read(port,IIR);
		225	com_read(port,RBR);
		226
		227	return 0;
		228
		229	}
		230
289	giacomo	231	int servo_open(int port)
286	giacomo	232	{
		233	int err;
		234
289	giacomo	235	servo_port = (unsigned)(port);
		236	err = com_open(servo_port, SERVO_SPEED, SERVO_PARITY, SERVO_LEN, SERVO_STOP);
286	giacomo	237
		238	return err;
		239
		240	}
		241
		242	int servo_close(void)
		243	{
		244	int err;
		245
289	giacomo	246	err = com_close(servo_port);
286	giacomo	247
		248	return err;
		249
		250	}
		251
		252	/* 1000.011w:bbbb.bbbb */
281	giacomo	253	int servo_set_RS232_baudrate(int baud)
		254	{
285	giacomo	255	struct timespec current_time;
		256	unsigned char b;
286	giacomo	257	int err,diff,mindiff,i;
		258	unsigned char spbrg, w;
		259
		260	i = 0;
		261	while(BaudTable[i] != baud && BaudTable[i] != -1) i++;
		262	if (BaudTable[i] == -1) {
		263	kern_printf("SERVO:Error wrong baud rate\n");
		264	return -1;
		265	}
		266
		267	i = 0;
		268	mindiff = 0xFFFFFF;
		269	spbrg = 0;
		270	w = 0;
		271	while(Baud20MhzData[i] != -1) {
		272	diff = abs(Baud20MhzData[i+1] - baud);
		273	if (diff < mindiff) {
		274	mindiff = diff;
		275	spbrg = (unsigned char)(Baud20MhzData[i+1]);
		276	w = 0;
		277	}
		278	diff = abs(Baud20MhzData[i+2] - baud);
		279	if (diff < mindiff) {
		280	mindiff = diff;
		281	spbrg = (unsigned char)(Baud20MhzData[i+2]);
		282	w = 1;
		283	}
		284	i += 3;
		285	}
		286
		287	#ifdef SERVO_DEBUG
		288	kern_printf("(SERVO:SBPRG %d W %d)",spbrg,w);
		289	#endif
		290
285	giacomo	291	timer_expired = 0;
		292	kern_gettime(&current_time);
		293	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		294	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		295
286	giacomo	296	b = 0x86 \| (w & 0x01);
289	giacomo	297	err = com_send(servo_port, b);
		298	err = com_receive(servo_port);
285	giacomo	299	if (err != (int)(b)) timer_expired = 1;
281	giacomo	300
286	giacomo	301	b = spbrg;
289	giacomo	302	err = com_send(servo_port, b);
		303	err = com_receive(servo_port);
285	giacomo	304	if (err != (int)(b)) timer_expired = 1;
281	giacomo	305
285	giacomo	306	if (timeout_event != NIL) kern_event_delete(timeout_event);
286	giacomo	307
289	giacomo	308	com_close(servo_port);
		309	com_open(servo_port, baud, SERVO_PARITY, SERVO_LEN, SERVO_STOP);
286	giacomo	310
285	giacomo	311	if (!timer_expired)
		312	return 0;
		313	else
		314	return -1;
		315
281	giacomo	316	}
		317
285	giacomo	318	/* 1000.0101 */
281	giacomo	319	int servo_get_RS232_baudrate(void)
		320	{
285	giacomo	321	struct timespec current_time;
		322	unsigned char b;
		323	int err,res;
		324
		325	timer_expired = 0;
		326	kern_gettime(&current_time);
		327	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		328	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		329
		330	b = 0x85;
289	giacomo	331	err = com_send(servo_port, b);
		332	err = com_receive(servo_port);
285	giacomo	333	if (err != (int)(b)) timer_expired = 1;
289	giacomo	334	res = com_receive(servo_port);
285	giacomo	335
		336	if (timeout_event != NIL) kern_event_delete(timeout_event);
		337
		338	if (!timer_expired)
		339	return res;
		340	else
		341	return -1;
281	giacomo	342
		343	}
		344
286	giacomo	345	/* 1000.0100 */
281	giacomo	346	int servo_store_RS232_buadrate(void)
		347	{
285	giacomo	348	struct timespec current_time;
		349	unsigned char b;
		350	int err;
		351
		352	timer_expired = 0;
		353	kern_gettime(&current_time);
		354	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		355	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		356
286	giacomo	357	b = 0x84;
289	giacomo	358	err = com_send(servo_port, b);
		359	err = com_receive(servo_port);
285	giacomo	360	if (err != (int)(b)) timer_expired = 1;
		361
		362	if (timeout_event != NIL) kern_event_delete(timeout_event);
		363
		364	if (!timer_expired)
		365	return 0;
		366	else
		367	return -1;
281	giacomo	368
		369	}
		370
289	giacomo	371	/* 1000.1010:llll.llll */
281	giacomo	372	int servo_set_period(int period)
		373	{
285	giacomo	374	struct timespec current_time;
		375	unsigned char b;
		376	int err;
		377
		378	timer_expired = 0;
		379	kern_gettime(&current_time);
		380	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		381	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		382
289	giacomo	383	b = 0x8A;
		384	err = com_send(servo_port, b);
		385	err = com_receive(servo_port);
285	giacomo	386	if (err != (int)(b)) timer_expired = 1;
		387
290	giacomo	388	b = period/TICK_LEN/8 & 0xFF;
289	giacomo	389	err = com_send(servo_port, b);
		390	err = com_receive(servo_port);
285	giacomo	391	if (err != (int)(b)) timer_expired = 1;
		392
		393	if (timeout_event != NIL) kern_event_delete(timeout_event);
		394
		395	if (!timer_expired)
		396	return 0;
		397	else
		398	return -1;
281	giacomo	399
		400	}
		401
285	giacomo	402	/* 1000.1001 */
281	giacomo	403	int servo_get_period(void)
		404	{
285	giacomo	405	struct timespec current_time;
		406	unsigned char b;
		407	int err,res;
		408
		409	timer_expired = 0;
		410	kern_gettime(&current_time);
		411	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		412	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		413
		414	b = 0x89;
289	giacomo	415	err = com_send(servo_port, b);
		416	err = com_receive(servo_port);
285	giacomo	417	if (err != (int)(b)) timer_expired = 1;
289	giacomo	418	res = com_receive(servo_port);
285	giacomo	419
		420	if (timeout_event != NIL) kern_event_delete(timeout_event);
		421
		422	if (!timer_expired)
290	giacomo	423	return (resTICK_LEN8);
285	giacomo	424	else
		425	return -1;
281	giacomo	426
		427	}
		428
289	giacomo	429	/* 1000.1000 */
281	giacomo	430	int servo_store_period(void)
		431	{
285	giacomo	432	struct timespec current_time;
		433	unsigned char b;
		434	int err;
		435
		436	timer_expired = 0;
		437	kern_gettime(&current_time);
		438	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		439	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		440
289	giacomo	441	b = 0x88;
		442	err = com_send(servo_port, b);
		443	err = com_receive(servo_port);
285	giacomo	444	if (err != (int)(b)) timer_expired = 1;
		445
		446	if (timeout_event != NIL) kern_event_delete(timeout_event);
		447
		448	if (!timer_expired)
		449	return 0;
		450	else
		451	return -1;
281	giacomo	452
		453	}
		454
283	giacomo	455	/* 1000.1100 */
281	giacomo	456	int servo_get_setup_switch(void)
		457	{
283	giacomo	458	struct timespec current_time;
		459	unsigned char b;
		460	int err,res;
		461	timer_expired = 0;
		462	kern_gettime(&current_time);
		463	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		464	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
281	giacomo	465
283	giacomo	466	b = 0x8C;
289	giacomo	467	err = com_send(servo_port, b);
		468	err = com_receive(servo_port);
283	giacomo	469	if (err != (int)(b)) timer_expired = 1;
289	giacomo	470	res = com_receive(servo_port);
281	giacomo	471
283	giacomo	472	if (timeout_event != NIL) kern_event_delete(timeout_event);
		473
		474	if (!timer_expired)
		475	return res;
		476	else
		477	return -1;
		478
281	giacomo	479	}
		480
283	giacomo	481	/* 1000.111s */
281	giacomo	482	int servo_set_RC5_switch(int data)
		483	{
283	giacomo	484	struct timespec current_time;
		485	unsigned char b;
		486	int err;
		487
		488	timer_expired = 0;
		489	kern_gettime(&current_time);
		490	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		491	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
281	giacomo	492
283	giacomo	493	b = 0x8E \| (data & 0x01);
289	giacomo	494	err = com_send(servo_port, b);
		495	err = com_receive(servo_port);
283	giacomo	496	if (err != (int)(b)) timer_expired = 1;
		497
		498	if (timeout_event != NIL) kern_event_delete(timeout_event);
		499
		500	if (!timer_expired)
		501	return 0;
		502	else
		503	return -1;
		504
281	giacomo	505	}
		506
285	giacomo	507	/* 1000.0000:0000.0mmm */
		508	int servo_turn_off(int servo)
		509	{
		510
		511	struct timespec current_time;
		512	unsigned char b;
		513	int err;
		514
		515	if (servo > 7) return -1;
		516
		517	timer_expired = 0;
		518	kern_gettime(&current_time);
		519	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		520	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		521
		522	b = 0x80;
289	giacomo	523	err = com_send(servo_port, b);
		524	err = com_receive(servo_port);
285	giacomo	525	if (err != (int)(b)) timer_expired = 1;
		526
		527	b = 0x00 \| (servo & 0x07);
289	giacomo	528	err = com_send(servo_port, b);
		529	err = com_receive(servo_port);
285	giacomo	530	if (err != (int)(b)) timer_expired = 1;
		531
		532	if (timeout_event != NIL) kern_event_delete(timeout_event);
		533
		534	if (!timer_expired)
		535	return 0;
		536	else
		537	return -1;
		538
		539	}
		540
291	giacomo	541	/* 1000.0000:0001.0mmm */
285	giacomo	542	int servo_turn_on(int servo)
		543	{
		544
		545	struct timespec current_time;
		546	unsigned char b;
		547	int err;
		548
		549	if (servo > 7) return -1;
		550
		551	timer_expired = 0;
		552	kern_gettime(&current_time);
		553	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		554	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		555
		556	b = 0x80;
289	giacomo	557	err = com_send(servo_port, b);
		558	err = com_receive(servo_port);
285	giacomo	559	if (err != (int)(b)) timer_expired = 1;
		560
291	giacomo	561	b = 0x10 \| (servo & 0x07);
289	giacomo	562	err = com_send(servo_port, b);
		563	err = com_receive(servo_port);
285	giacomo	564	if (err != (int)(b)) timer_expired = 1;
		565
		566	if (timeout_event != NIL) kern_event_delete(timeout_event);
		567
		568	if (!timer_expired)
		569	return 0;
		570	else
		571	return -1;
		572
		573	}
		574
291	giacomo	575	/* 1000.0000:0010.0000 */
285	giacomo	576	int servo_turn_off_all(void)
		577	{ struct timespec current_time;
		578	unsigned char b;
		579	int err;
		580
		581	timer_expired = 0;
		582	kern_gettime(&current_time);
		583	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		584	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		585
		586	b = 0x80;
289	giacomo	587	err = com_send(servo_port, b);
		588	err = com_receive(servo_port);
285	giacomo	589	if (err != (int)(b)) timer_expired = 1;
		590
291	giacomo	591	b = 0x20;
289	giacomo	592	err = com_send(servo_port, b);
		593	err = com_receive(servo_port);
285	giacomo	594	if (err != (int)(b)) timer_expired = 1;
		595
		596	if (timeout_event != NIL) kern_event_delete(timeout_event);
		597
		598	if (!timer_expired)
		599	return 0;
		600	else
		601	return -1;
		602
		603	}
		604
291	giacomo	605	/* 1000.0000:0010.0001 */
285	giacomo	606	int servo_turn_on_all(void)
		607	{
		608	struct timespec current_time;
		609	unsigned char b;
		610	int err;
		611
		612	timer_expired = 0;
		613	kern_gettime(&current_time);
		614	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		615	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		616
		617	b = 0x80;
289	giacomo	618	err = com_send(servo_port, b);
		619	err = com_receive(servo_port);
285	giacomo	620	if (err != (int)(b)) timer_expired = 1;
		621
291	giacomo	622	b = 0x21;
289	giacomo	623	err = com_send(servo_port, b);
		624	err = com_receive(servo_port);
285	giacomo	625	if (err != (int)(b)) timer_expired = 1;
		626
		627	if (timeout_event != NIL) kern_event_delete(timeout_event);
		628
		629	if (!timer_expired)
		630	return 0;
		631	else
		632	return -1;
		633
		634	}
		635
290	giacomo	636	int servo_set_max_angle(int servo, int angle_sec)
		637	{
		638
		639	servo_table[servo].max_angle_sec = angle_sec;
		640	return 0;
		641
		642	}
		643
		644	int servo_set_min_angle(int servo, int angle_sec)
		645	{
		646
		647	servo_table[servo].min_angle_sec = angle_sec;
		648	return 0;
		649
		650	}
		651
282	giacomo	652	/* 0000.0ppp:0000.vvvv:vvvv.vvvv */
290	giacomo	653	int servo_set_angle_sec(int servo, int angle_sec)
281	giacomo	654	{
282	giacomo	655	struct timespec current_time;
		656	unsigned char b;
290	giacomo	657	int err, angle_tick;
282	giacomo	658
		659	if (servo > 7) return -1;
		660
290	giacomo	661	if (angle_sec > servo_table[servo].max_angle_sec \|\|
		662	angle_sec < servo_table[servo].min_angle_sec) return -1;
		663
		664	angle_tick = servo_table[servo].zero_tick + angle_sec * servo_table[servo].delta_tick /
		665	(servo_table[servo].max_angle_sec - servo_table[servo].min_angle_sec);
		666
282	giacomo	667	timer_expired = 0;
		668	kern_gettime(&current_time);
		669	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		670	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		671
		672	b = 0x00 \| (servo & 0x07);
289	giacomo	673	err = com_send(servo_port, b);
		674	err = com_receive(servo_port);
283	giacomo	675	if (err != (int)(b)) timer_expired = 1;
282	giacomo	676
290	giacomo	677	b = 0x00 \| ((angle_tick >> 8) & 0x0F);
289	giacomo	678	err = com_send(servo_port, b);
		679	err = com_receive(servo_port);
283	giacomo	680	if (err != (int)(b)) timer_expired = 1;
282	giacomo	681
290	giacomo	682	b = 0x00 \| (angle_tick & 0xFF);
289	giacomo	683	err = com_send(servo_port, b);
		684	err = com_receive(servo_port);
283	giacomo	685	if (err != (int)(b)) timer_expired = 1;
282	giacomo	686
283	giacomo	687	if (timeout_event != NIL) kern_event_delete(timeout_event);
282	giacomo	688
283	giacomo	689	if (!timer_expired)
		690	return 0;
		691	else
		692	return -1;
281	giacomo	693
		694	}
		695
291	giacomo	696	/* 0010.0ppp */
290	giacomo	697	int servo_store_default_position(int servo)
		698	{
		699	struct timespec current_time;
		700	unsigned char b;
		701	int err;
		702
		703	if (servo > 7) return -1;
		704
		705	timer_expired = 0;
		706	kern_gettime(&current_time);
		707	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		708	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		709
291	giacomo	710	b = 0x20 \| (servo & 0x07);
290	giacomo	711	err = com_send(servo_port, b);
		712	err = com_receive(servo_port);
		713	if (err != (int)(b)) timer_expired = 1;
		714
		715	if (timeout_event != NIL) kern_event_delete(timeout_event);
		716
		717	if (!timer_expired)
		718	return 0;
		719	else
		720	return -1;
		721
		722	}
		723
291	giacomo	724	/* 0001.0ppp */
290	giacomo	725	int servo_get_angle_sec(int servo)
281	giacomo	726	{
282	giacomo	727	struct timespec current_time;
		728	unsigned char b;
290	giacomo	729	int err,res,data;
282	giacomo	730
		731	if (servo > 7) return -1;
		732
		733	timer_expired = 0;
		734	kern_gettime(&current_time);
		735	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		736	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
281	giacomo	737
291	giacomo	738	b = 0x10 \| (servo & 0x07);
289	giacomo	739	err = com_send(servo_port, b);
		740	err = com_receive(servo_port);
283	giacomo	741	if (err != (int)(b)) timer_expired = 1;
289	giacomo	742	res = com_receive(servo_port) << 8;
		743	res \|= com_receive(servo_port);
282	giacomo	744
283	giacomo	745	if (timeout_event != NIL) kern_event_delete(timeout_event);
282	giacomo	746
290	giacomo	747	data = (res - servo_table[servo].zero_tick) *
		748	(servo_table[servo].max_angle_sec - servo_table[servo].min_angle_sec) /
		749	servo_table[servo].delta_tick;
		750
282	giacomo	751	if (!timer_expired)
290	giacomo	752	return data;
282	giacomo	753	else
		754	return -1;
		755
281	giacomo	756	}
		757
282	giacomo	758	/* 0100:0aaa */
281	giacomo	759	int servo_get_analog(int port)
		760	{
		761
282	giacomo	762	struct timespec current_time;
		763	unsigned char b;
283	giacomo	764	int err,res;
282	giacomo	765
		766	if (port > 4) return -1;
		767
		768	timer_expired = 0;
		769	kern_gettime(&current_time);
		770	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		771	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
281	giacomo	772
282	giacomo	773	b = 0x40 \| (port & 7);
289	giacomo	774	err = com_send(servo_port, b);
		775	err = com_receive(servo_port);
283	giacomo	776	if (err != (int)(b)) timer_expired = 1;
289	giacomo	777	res = com_receive(servo_port) << 8;
		778	res \|= com_receive(servo_port);
282	giacomo	779
283	giacomo	780	if (timeout_event != NIL) kern_event_delete(timeout_event);
282	giacomo	781
		782	if (!timer_expired)
		783	return res;
		784	else
		785	return -1;
		786
281	giacomo	787	}
		788

Subversion Repositories shark

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