WebSVN - shark - Blame - Rev 315 - /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
315	giacomo	69	#define TICK_LEN 1600 /* ns */
290	giacomo	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;
315	giacomo	276	spbrg = (unsigned char)(Baud20MhzData[i]);
286	giacomo	277	w = 0;
		278	}
		279	diff = abs(Baud20MhzData[i+2] - baud);
		280	if (diff < mindiff) {
		281	mindiff = diff;
315	giacomo	282	spbrg = (unsigned char)(Baud20MhzData[i]);
286	giacomo	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
315	giacomo	389	b = (period*1000)/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)
315	giacomo	424	return (resTICK_LEN/10008);
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
315	giacomo	637	/* 1000.0000:0011.0000:mmmm.mmmm */
		638	int servo_set_levels(int mask)
		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 = 0x30;
		655	err = com_send(servo_port, b);
		656	err = com_receive(servo_port);
		657	if (err != (int)(b)) timer_expired = 1;
		658
		659	b = (unsigned char)(mask & 0xFF);
		660	err = com_send(servo_port, b);
		661	err = com_receive(servo_port);
		662	if (err != (int)(b)) timer_expired = 1;
		663
		664	if (timeout_event != NIL) kern_event_delete(timeout_event);
		665
		666	if (!timer_expired)
		667	return 0;
		668	else
		669	return -1;
		670
		671	}
		672
297	giacomo	673	/* 1000.0000:0100.0000 */
315	giacomo	674	int servo_get_levels(void)
297	giacomo	675	{
		676	struct timespec current_time;
		677	unsigned char b;
		678	int err;
		679
		680	timer_expired = 0;
		681	kern_gettime(&current_time);
		682	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		683	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		684
		685	b = 0x80;
		686	err = com_send(servo_port, b);
		687	err = com_receive(servo_port);
		688	if (err != (int)(b)) timer_expired = 1;
		689
		690	b = 0x40;
		691	err = com_send(servo_port, b);
		692	err = com_receive(servo_port);
		693	if (err != (int)(b)) timer_expired = 1;
		694	err = com_receive(servo_port);
		695
		696	if (timeout_event != NIL) kern_event_delete(timeout_event);
		697
		698	if (!timer_expired)
		699	return 0;
		700	else
		701	return -1;
		702
		703	}
		704
		705	/* 1000.0000:1000.0000 */
		706	int servo_store_levels(void)
		707	{
		708	struct timespec current_time;
		709	unsigned char b;
		710	int err;
		711
		712	timer_expired = 0;
		713	kern_gettime(&current_time);
		714	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		715	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		716
		717	b = 0x80;
		718	err = com_send(servo_port, b);
		719	err = com_receive(servo_port);
		720	if (err != (int)(b)) timer_expired = 1;
		721
		722	b = 0x80;
		723	err = com_send(servo_port, b);
		724	err = com_receive(servo_port);
		725	if (err != (int)(b)) timer_expired = 1;
		726
		727	if (timeout_event != NIL) kern_event_delete(timeout_event);
		728
		729	if (!timer_expired)
		730	return 0;
		731	else
		732	return -1;
		733
		734	}
		735
290	giacomo	736	int servo_set_max_angle(int servo, int angle_sec)
		737	{
		738
		739	servo_table[servo].max_angle_sec = angle_sec;
		740	return 0;
		741
		742	}
		743
		744	int servo_set_min_angle(int servo, int angle_sec)
		745	{
		746
		747	servo_table[servo].min_angle_sec = angle_sec;
		748	return 0;
		749
		750	}
		751
282	giacomo	752	/* 0000.0ppp:0000.vvvv:vvvv.vvvv */
290	giacomo	753	int servo_set_angle_sec(int servo, int angle_sec)
281	giacomo	754	{
282	giacomo	755	struct timespec current_time;
		756	unsigned char b;
290	giacomo	757	int err, angle_tick;
282	giacomo	758
		759	if (servo > 7) return -1;
		760
290	giacomo	761	if (angle_sec > servo_table[servo].max_angle_sec \|\|
		762	angle_sec < servo_table[servo].min_angle_sec) return -1;
		763
		764	angle_tick = servo_table[servo].zero_tick + angle_sec * servo_table[servo].delta_tick /
		765	(servo_table[servo].max_angle_sec - servo_table[servo].min_angle_sec);
		766
282	giacomo	767	timer_expired = 0;
		768	kern_gettime(&current_time);
		769	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		770	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		771
		772	b = 0x00 \| (servo & 0x07);
289	giacomo	773	err = com_send(servo_port, b);
		774	err = com_receive(servo_port);
283	giacomo	775	if (err != (int)(b)) timer_expired = 1;
282	giacomo	776
290	giacomo	777	b = 0x00 \| ((angle_tick >> 8) & 0x0F);
289	giacomo	778	err = com_send(servo_port, b);
		779	err = com_receive(servo_port);
283	giacomo	780	if (err != (int)(b)) timer_expired = 1;
282	giacomo	781
290	giacomo	782	b = 0x00 \| (angle_tick & 0xFF);
289	giacomo	783	err = com_send(servo_port, b);
		784	err = com_receive(servo_port);
283	giacomo	785	if (err != (int)(b)) timer_expired = 1;
282	giacomo	786
283	giacomo	787	if (timeout_event != NIL) kern_event_delete(timeout_event);
282	giacomo	788
283	giacomo	789	if (!timer_expired)
		790	return 0;
		791	else
		792	return -1;
281	giacomo	793
		794	}
		795
291	giacomo	796	/* 0010.0ppp */
290	giacomo	797	int servo_store_default_position(int servo)
		798	{
		799	struct timespec current_time;
		800	unsigned char b;
		801	int err;
		802
		803	if (servo > 7) return -1;
		804
		805	timer_expired = 0;
		806	kern_gettime(&current_time);
		807	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		808	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		809
291	giacomo	810	b = 0x20 \| (servo & 0x07);
290	giacomo	811	err = com_send(servo_port, b);
		812	err = com_receive(servo_port);
		813	if (err != (int)(b)) timer_expired = 1;
		814
		815	if (timeout_event != NIL) kern_event_delete(timeout_event);
		816
		817	if (!timer_expired)
		818	return 0;
		819	else
		820	return -1;
		821
		822	}
		823
291	giacomo	824	/* 0001.0ppp */
290	giacomo	825	int servo_get_angle_sec(int servo)
281	giacomo	826	{
282	giacomo	827	struct timespec current_time;
		828	unsigned char b;
290	giacomo	829	int err,res,data;
282	giacomo	830
		831	if (servo > 7) return -1;
		832
		833	timer_expired = 0;
		834	kern_gettime(&current_time);
		835	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		836	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
281	giacomo	837
291	giacomo	838	b = 0x10 \| (servo & 0x07);
289	giacomo	839	err = com_send(servo_port, b);
		840	err = com_receive(servo_port);
283	giacomo	841	if (err != (int)(b)) timer_expired = 1;
289	giacomo	842	res = com_receive(servo_port) << 8;
		843	res \|= com_receive(servo_port);
282	giacomo	844
283	giacomo	845	if (timeout_event != NIL) kern_event_delete(timeout_event);
282	giacomo	846
290	giacomo	847	data = (res - servo_table[servo].zero_tick) *
		848	(servo_table[servo].max_angle_sec - servo_table[servo].min_angle_sec) /
		849	servo_table[servo].delta_tick;
		850
282	giacomo	851	if (!timer_expired)
290	giacomo	852	return data;
282	giacomo	853	else
		854	return -1;
		855
281	giacomo	856	}
		857
282	giacomo	858	/* 0100:0aaa */
281	giacomo	859	int servo_get_analog(int port)
		860	{
		861
282	giacomo	862	struct timespec current_time;
		863	unsigned char b;
283	giacomo	864	int err,res;
282	giacomo	865
		866	if (port > 4) return -1;
		867
		868	timer_expired = 0;
		869	kern_gettime(&current_time);
		870	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		871	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
281	giacomo	872
282	giacomo	873	b = 0x40 \| (port & 7);
289	giacomo	874	err = com_send(servo_port, b);
		875	err = com_receive(servo_port);
283	giacomo	876	if (err != (int)(b)) timer_expired = 1;
289	giacomo	877	res = com_receive(servo_port) << 8;
		878	res \|= com_receive(servo_port);
282	giacomo	879
283	giacomo	880	if (timeout_event != NIL) kern_event_delete(timeout_event);
282	giacomo	881
		882	if (!timer_expired)
		883	return res;
		884	else
		885	return -1;
		886
281	giacomo	887	}
		888

Subversion Repositories shark

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