WebSVN - shark - Blame - Rev 297 - /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
297	giacomo	637	/* 1000.0000:0100.0000 */
		638	int servo_get_servo_levels(void)
		639	{
		640	struct timespec current_time;
		641	unsigned char b;
		642	int err;
		643
		644	timer_expired = 0;
		645	kern_gettime(&current_time);
		646	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		647	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		648
		649	b = 0x80;
		650	err = com_send(servo_port, b);
		651	err = com_receive(servo_port);
		652	if (err != (int)(b)) timer_expired = 1;
		653
		654	b = 0x40;
		655	err = com_send(servo_port, b);
		656	err = com_receive(servo_port);
		657	if (err != (int)(b)) timer_expired = 1;
		658	err = com_receive(servo_port);
		659
		660	if (timeout_event != NIL) kern_event_delete(timeout_event);
		661
		662	if (!timer_expired)
		663	return 0;
		664	else
		665	return -1;
		666
		667	}
		668
		669	/* 1000.0000:1000.0000 */
		670	int servo_store_levels(void)
		671	{
		672	struct timespec current_time;
		673	unsigned char b;
		674	int err;
		675
		676	timer_expired = 0;
		677	kern_gettime(&current_time);
		678	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		679	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		680
		681	b = 0x80;
		682	err = com_send(servo_port, b);
		683	err = com_receive(servo_port);
		684	if (err != (int)(b)) timer_expired = 1;
		685
		686	b = 0x80;
		687	err = com_send(servo_port, b);
		688	err = com_receive(servo_port);
		689	if (err != (int)(b)) timer_expired = 1;
		690
		691	if (timeout_event != NIL) kern_event_delete(timeout_event);
		692
		693	if (!timer_expired)
		694	return 0;
		695	else
		696	return -1;
		697
		698	}
		699
290	giacomo	700	int servo_set_max_angle(int servo, int angle_sec)
		701	{
		702
		703	servo_table[servo].max_angle_sec = angle_sec;
		704	return 0;
		705
		706	}
		707
		708	int servo_set_min_angle(int servo, int angle_sec)
		709	{
		710
		711	servo_table[servo].min_angle_sec = angle_sec;
		712	return 0;
		713
		714	}
		715
282	giacomo	716	/* 0000.0ppp:0000.vvvv:vvvv.vvvv */
290	giacomo	717	int servo_set_angle_sec(int servo, int angle_sec)
281	giacomo	718	{
282	giacomo	719	struct timespec current_time;
		720	unsigned char b;
290	giacomo	721	int err, angle_tick;
282	giacomo	722
		723	if (servo > 7) return -1;
		724
290	giacomo	725	if (angle_sec > servo_table[servo].max_angle_sec \|\|
		726	angle_sec < servo_table[servo].min_angle_sec) return -1;
		727
		728	angle_tick = servo_table[servo].zero_tick + angle_sec * servo_table[servo].delta_tick /
		729	(servo_table[servo].max_angle_sec - servo_table[servo].min_angle_sec);
		730
282	giacomo	731	timer_expired = 0;
		732	kern_gettime(&current_time);
		733	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		734	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		735
		736	b = 0x00 \| (servo & 0x07);
289	giacomo	737	err = com_send(servo_port, b);
		738	err = com_receive(servo_port);
283	giacomo	739	if (err != (int)(b)) timer_expired = 1;
282	giacomo	740
290	giacomo	741	b = 0x00 \| ((angle_tick >> 8) & 0x0F);
289	giacomo	742	err = com_send(servo_port, b);
		743	err = com_receive(servo_port);
283	giacomo	744	if (err != (int)(b)) timer_expired = 1;
282	giacomo	745
290	giacomo	746	b = 0x00 \| (angle_tick & 0xFF);
289	giacomo	747	err = com_send(servo_port, b);
		748	err = com_receive(servo_port);
283	giacomo	749	if (err != (int)(b)) timer_expired = 1;
282	giacomo	750
283	giacomo	751	if (timeout_event != NIL) kern_event_delete(timeout_event);
282	giacomo	752
283	giacomo	753	if (!timer_expired)
		754	return 0;
		755	else
		756	return -1;
281	giacomo	757
		758	}
		759
291	giacomo	760	/* 0010.0ppp */
290	giacomo	761	int servo_store_default_position(int servo)
		762	{
		763	struct timespec current_time;
		764	unsigned char b;
		765	int err;
		766
		767	if (servo > 7) 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);
		773
291	giacomo	774	b = 0x20 \| (servo & 0x07);
290	giacomo	775	err = com_send(servo_port, b);
		776	err = com_receive(servo_port);
		777	if (err != (int)(b)) timer_expired = 1;
		778
		779	if (timeout_event != NIL) kern_event_delete(timeout_event);
		780
		781	if (!timer_expired)
		782	return 0;
		783	else
		784	return -1;
		785
		786	}
		787
291	giacomo	788	/* 0001.0ppp */
290	giacomo	789	int servo_get_angle_sec(int servo)
281	giacomo	790	{
282	giacomo	791	struct timespec current_time;
		792	unsigned char b;
290	giacomo	793	int err,res,data;
282	giacomo	794
		795	if (servo > 7) return -1;
		796
		797	timer_expired = 0;
		798	kern_gettime(&current_time);
		799	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		800	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
281	giacomo	801
291	giacomo	802	b = 0x10 \| (servo & 0x07);
289	giacomo	803	err = com_send(servo_port, b);
		804	err = com_receive(servo_port);
283	giacomo	805	if (err != (int)(b)) timer_expired = 1;
289	giacomo	806	res = com_receive(servo_port) << 8;
		807	res \|= com_receive(servo_port);
282	giacomo	808
283	giacomo	809	if (timeout_event != NIL) kern_event_delete(timeout_event);
282	giacomo	810
290	giacomo	811	data = (res - servo_table[servo].zero_tick) *
		812	(servo_table[servo].max_angle_sec - servo_table[servo].min_angle_sec) /
		813	servo_table[servo].delta_tick;
		814
282	giacomo	815	if (!timer_expired)
290	giacomo	816	return data;
282	giacomo	817	else
		818	return -1;
		819
281	giacomo	820	}
		821
282	giacomo	822	/* 0100:0aaa */
281	giacomo	823	int servo_get_analog(int port)
		824	{
		825
282	giacomo	826	struct timespec current_time;
		827	unsigned char b;
283	giacomo	828	int err,res;
282	giacomo	829
		830	if (port > 4) return -1;
		831
		832	timer_expired = 0;
		833	kern_gettime(&current_time);
		834	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		835	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
281	giacomo	836
282	giacomo	837	b = 0x40 \| (port & 7);
289	giacomo	838	err = com_send(servo_port, b);
		839	err = com_receive(servo_port);
283	giacomo	840	if (err != (int)(b)) timer_expired = 1;
289	giacomo	841	res = com_receive(servo_port) << 8;
		842	res \|= com_receive(servo_port);
282	giacomo	843
283	giacomo	844	if (timeout_event != NIL) kern_event_delete(timeout_event);
282	giacomo	845
		846	if (!timer_expired)
		847	return res;
		848	else
		849	return -1;
		850
281	giacomo	851	}
		852

Subversion Repositories shark

(root)/shark/trunk/ports/servo/servo.c @ 379 - Rev 297