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

Rev	Author	Line No.	Line
281	giacomo	1	/*
		2	* Project: S.Ha.R.K.
		3	*
		4	* Coordinators:
		5	* Giorgio Buttazzo <giorgio@sssup.it>
		6	* Paolo Gai <pj@gandalf.sssup.it>
		7	*
		8	* Authors :
		9	* Giacomo Guidi <giacomo@gandalf.sssup.it>
381	giacomo	10	* Mauro Marinoni <mauro.marinoni@unipv.it>
281	giacomo	11	* (see the web pages for full authors list)
		12	*
		13	* ReTiS Lab (Scuola Superiore S.Anna - Pisa - Italy)
		14	*
		15	* http://www.sssup.it
		16	* http://retis.sssup.it
		17	* http://shark.sssup.it
		18	*/
		19
		20	/*
		21	* Copyright (C) 2002 Paolo Gai
		22	*
		23	* This program is free software; you can redistribute it and/or modify
		24	* it under the terms of the GNU General Public License as published by
		25	* the Free Software Foundation; either version 2 of the License, or
		26	* (at your option) any later version.
		27	*
		28	* This program is distributed in the hope that it will be useful,
		29	* but WITHOUT ANY WARRANTY; without even the implied warranty of
		30	* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
		31	* GNU General Public License for more details.
		32	*
		33	* You should have received a copy of the GNU General Public License
		34	* along with this program; if not, write to the Free Software
		35	* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
		36	*
		37	*/
		38
		39	#include <ll/sys/ll/ll-instr.h>
282	giacomo	40	#include "kernel/kern.h"
281	giacomo	41	#include "servo.h"
		42
		43	#define THR 0
		44	#define RBR 0
		45	#define IER 1
		46	#define FCR 2
		47	#define IIR 2
		48	#define LCR 3
		49	#define MCR 4
		50	#define LSR 5
		51	#define MSR 6
		52	#define SPad 7
		53
286	giacomo	54	/* Parity value */
		55	#define NONE 0
		56	#define ODD 1
		57	#define EVEN 3
		58
281	giacomo	59	#define barrier() __asm__ __volatile__("" ::: "memory");
		60
358	giacomo	61	//#define SERVO_DEBUG
493	giacomo	62	//#define SERVO_TIMEOUT_EVENT
286	giacomo	63
358	giacomo	64	#define SERVO_TIMEOUT 200000 /* us */
281	giacomo	65
398	giacomo	66	#define SERVO_SPEED 19200
286	giacomo	67	#define SERVO_PARITY NONE
		68	#define SERVO_LEN 8
		69	#define SERVO_STOP 1
282	giacomo	70
358	giacomo	71	#define SERVO_CLOCK 20000000 /* 20MHz */
		72
315	giacomo	73	#define TICK_LEN 1600 /* ns */
316	giacomo	74	#define TICK_LEN_PERIOD 51200 /* ns */
290	giacomo	75
		76	struct servo_data {
		77	int min_angle_sec;
		78	int max_angle_sec;
		79	int delta_tick;
		80	int zero_tick;
		81	};
		82
381	giacomo	83	struct servo_data servo_table[4][16] = {
		84	{{-324000, 324000, 1200, 1600},
316	giacomo	85	{-324000, 324000, 1200, 1600},
		86	{-324000, 324000, 1200, 1600},
		87	{-324000, 324000, 1200, 1600},
		88	{-324000, 324000, 1200, 1600},
		89	{-324000, 324000, 1200, 1600},
		90	{-324000, 324000, 1200, 1600},
		91	{-324000, 324000, 1200, 1600},
335	giacomo	92	{-324000, 324000, 1200, 1600},
		93	{-324000, 324000, 1200, 1600},
		94	{-324000, 324000, 1200, 1600},
		95	{-324000, 324000, 1200, 1600},
		96	{-324000, 324000, 1200, 1600},
		97	{-324000, 324000, 1200, 1600},
		98	{-324000, 324000, 1200, 1600},
381	giacomo	99	{-324000, 324000, 1200, 1600}},
		100	{{-324000, 324000, 1200, 1600},
335	giacomo	101	{-324000, 324000, 1200, 1600},
381	giacomo	102	{-324000, 324000, 1200, 1600},
		103	{-324000, 324000, 1200, 1600},
		104	{-324000, 324000, 1200, 1600},
		105	{-324000, 324000, 1200, 1600},
		106	{-324000, 324000, 1200, 1600},
		107	{-324000, 324000, 1200, 1600},
		108	{-324000, 324000, 1200, 1600},
		109	{-324000, 324000, 1200, 1600},
		110	{-324000, 324000, 1200, 1600},
		111	{-324000, 324000, 1200, 1600},
		112	{-324000, 324000, 1200, 1600},
		113	{-324000, 324000, 1200, 1600},
		114	{-324000, 324000, 1200, 1600},
		115	{-324000, 324000, 1200, 1600}},
		116	{{-324000, 324000, 1200, 1600},
		117	{-324000, 324000, 1200, 1600},
		118	{-324000, 324000, 1200, 1600},
		119	{-324000, 324000, 1200, 1600},
		120	{-324000, 324000, 1200, 1600},
		121	{-324000, 324000, 1200, 1600},
		122	{-324000, 324000, 1200, 1600},
		123	{-324000, 324000, 1200, 1600},
		124	{-324000, 324000, 1200, 1600},
		125	{-324000, 324000, 1200, 1600},
		126	{-324000, 324000, 1200, 1600},
		127	{-324000, 324000, 1200, 1600},
		128	{-324000, 324000, 1200, 1600},
		129	{-324000, 324000, 1200, 1600},
		130	{-324000, 324000, 1200, 1600},
		131	{-324000, 324000, 1200, 1600}},
		132	{{-324000, 324000, 1200, 1600},
		133	{-324000, 324000, 1200, 1600},
		134	{-324000, 324000, 1200, 1600},
		135	{-324000, 324000, 1200, 1600},
		136	{-324000, 324000, 1200, 1600},
		137	{-324000, 324000, 1200, 1600},
		138	{-324000, 324000, 1200, 1600},
		139	{-324000, 324000, 1200, 1600},
		140	{-324000, 324000, 1200, 1600},
		141	{-324000, 324000, 1200, 1600},
		142	{-324000, 324000, 1200, 1600},
		143	{-324000, 324000, 1200, 1600},
		144	{-324000, 324000, 1200, 1600},
		145	{-324000, 324000, 1200, 1600},
		146	{-324000, 324000, 1200, 1600},
		147	{-324000, 324000, 1200, 1600}}};
290	giacomo	148
281	giacomo	149	int timer_expired = 0;
283	giacomo	150	int timeout_event;
281	giacomo	151
286	giacomo	152	const unsigned com_base[] = {0x03F8,0x02F8,0x03E8,0x02E8};
		153
		154	const int BaudTable[] = {
		155	1200,
		156	2400,
		157	4800,
		158	9600,
		159	14400,
		160	19200,
		161	38400,
		162	57600,
		163	115200,
		164	-1};
		165
282	giacomo	166	void set_timer_expired(void *arg)
281	giacomo	167	{
283	giacomo	168	timeout_event = NIL;
281	giacomo	169	timer_expired = 1;
		170	}
		171
		172	unsigned com_read(unsigned port,unsigned reg)
		173	{
		174	unsigned b;
		175	if (port > 3 \|\| reg > 7) return(0);
		176	b = ll_in(com_base[port]+reg);
		177	return(b);
		178	}
		179
		180	void com_write(unsigned port,unsigned reg,unsigned value)
		181	{
		182	if (port > 3 \|\| reg > 7) return;
		183	ll_out(com_base[port]+reg,value);
		184	}
		185
		186	int com_send(unsigned port,BYTE b)
		187	{
		188	while ((com_read(port,LSR) & 32) == 0 && !timer_expired)
		189	barrier();
		190	if (!timer_expired) {
292	giacomo	191	#ifdef SERVO_DEBUG
295	giacomo	192	kern_printf("(SERVO WRITE p = %d b = %02x)",port,b);
292	giacomo	193	#endif
281	giacomo	194	com_write(port,THR,b);
		195	return 0;
		196	} else {
292	giacomo	197	#ifdef SERVO_DEBUG
295	giacomo	198	kern_printf("(WRITE TIMEOUT)");
292	giacomo	199	#endif
281	giacomo	200	return -1;
		201	}
		202	}
		203
		204	int com_receive(unsigned port)
		205	{
292	giacomo	206	int b;
		207
281	giacomo	208	while ((com_read(port,LSR) & 1) == 0 && !timer_expired)
		209	barrier();
		210	if (!timer_expired) {
292	giacomo	211	b = (int)(com_read(port,RBR));
		212	#ifdef SERVO_DEBUG
295	giacomo	213	kern_printf("(SERVO READ p = %d b = %02x)",port,b);
292	giacomo	214	#endif
		215	return b;
281	giacomo	216	} else {
292	giacomo	217	#ifdef SERVO_DEBUG
295	giacomo	218	kern_printf("(READ TIMEOUT)");
292	giacomo	219	#endif
281	giacomo	220	return -1;
		221	}
		222	}
		223
286	giacomo	224	int com_open(unsigned port,DWORD speed,BYTE parity,BYTE len,BYTE stop)
		225	{
		226	unsigned long div,b_mask;
		227
		228	/* Now set up the serial link */
		229	b_mask = (parity & 3) * 8 + (stop & 1) * 4 + ((len - 5) & 3);
		230	div = 115200L / speed;
		231	/* Clear serial interrupt enable register */
		232	com_write(port,IER,0);
		233	/* Empty input buffer */
		234	com_read(port,RBR);
		235	/* Activate DLAB bit for speed setting */
		236	com_write(port,LCR,0x80);
		237	/* Load baud divisor */
		238	com_write(port,0,div & 0x00FF);
		239	div >>= 8;
		240	com_write(port,1,div & 0x00FF);
		241	/* Load control word (parity,stop bit,bit len) */
		242	com_write(port,LCR,b_mask);
		243	/* Attiva OUT1 & OUT2 */
		244	com_write(port,MCR,0x0C);
		245
		246	return 0;
295	giacomo	247
286	giacomo	248	}
		249
		250	int com_close(unsigned port)
		251	{
		252
		253	com_write(port,IER,0);
		254	com_read(port,IIR);
		255	com_read(port,RBR);
		256
		257	return 0;
		258
		259	}
		260
358	giacomo	261	int servo_open(int port,int speed)
286	giacomo	262	{
		263	int err;
		264
358	giacomo	265	err = com_open((unsigned)(port), (DWORD)speed, SERVO_PARITY, SERVO_LEN, SERVO_STOP);
		266
286	giacomo	267	return err;
		268
		269	}
		270
358	giacomo	271	int servo_close(int port)
286	giacomo	272	{
		273	int err;
		274
358	giacomo	275	err = com_close((unsigned)(port));
286	giacomo	276
		277	return err;
		278
		279	}
		280
		281	/* 1000.011w:bbbb.bbbb */
358	giacomo	282	int servo_set_RS232_baudrate(int port, int baud)
281	giacomo	283	{
381	giacomo	284	#ifdef SERVO_TIMEOUT_EVENT
		285	struct timespec current_time;
		286	#endif
285	giacomo	287	unsigned char b;
358	giacomo	288	int err, spbrg_temp, i;
286	giacomo	289	unsigned char spbrg, w;
358	giacomo	290	int servo_port = (unsigned)(port);
286	giacomo	291
		292	i = 0;
		293	while(BaudTable[i] != baud && BaudTable[i] != -1) i++;
		294	if (BaudTable[i] == -1) {
		295	kern_printf("SERVO:Error wrong baud rate\n");
		296	return -1;
		297	}
		298
358	giacomo	299	w = 1;
453	giacomo	300	spbrg_temp = (SERVO_CLOCK10 / (16baud)) - 10;
412	giacomo	301	if (spbrg_temp>2550) {
358	giacomo	302	w = 0;
453	giacomo	303	spbrg_temp = (SERVO_CLOCK10 / (64baud)) - 10;
286	giacomo	304	}
412	giacomo	305	spbrg = spbrg_temp / 10;
453	giacomo	306	if (spbrg_temp % 10 > 5) spbrg++;
412	giacomo	307
286	giacomo	308	#ifdef SERVO_DEBUG
		309	kern_printf("(SERVO:SBPRG %d W %d)",spbrg,w);
358	giacomo	310	#endif
		311
285	giacomo	312	timer_expired = 0;
358	giacomo	313
381	giacomo	314	#ifdef SERVO_TIMEOUT_EVENT
		315	kern_gettime(&current_time);
		316	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		317	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		318	#else
		319	timeout_event = NIL;
		320	#endif
		321
286	giacomo	322	b = 0x86 \| (w & 0x01);
289	giacomo	323	err = com_send(servo_port, b);
		324	err = com_receive(servo_port);
285	giacomo	325	if (err != (int)(b)) timer_expired = 1;
281	giacomo	326
286	giacomo	327	b = spbrg;
289	giacomo	328	err = com_send(servo_port, b);
		329	err = com_receive(servo_port);
285	giacomo	330	if (err != (int)(b)) timer_expired = 1;
281	giacomo	331
285	giacomo	332	if (timeout_event != NIL) kern_event_delete(timeout_event);
358	giacomo	333
		334	/*com_close(servo_port);
		335	com_open(servo_port, baud, SERVO_PARITY, SERVO_LEN, SERVO_STOP);*/
		336
285	giacomo	337	if (!timer_expired)
		338	return 0;
		339	else
		340	return -1;
		341
281	giacomo	342	}
		343
285	giacomo	344	/* 1000.0101 */
358	giacomo	345	int servo_get_RS232_baudrate(int port)
281	giacomo	346	{
381	giacomo	347	#ifdef SERVO_TIMEOUT_EVENT
		348	struct timespec current_time;
		349	#endif
285	giacomo	350	unsigned char b;
358	giacomo	351	int err, res, res_w, res_b;
		352	int servo_port = (unsigned)(port);
		353
285	giacomo	354	timer_expired = 0;
381	giacomo	355
		356	#ifdef SERVO_TIMEOUT_EVENT
		357	kern_gettime(&current_time);
		358	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		359	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		360	#else
		361	timeout_event = NIL;
		362	#endif
358	giacomo	363
285	giacomo	364	b = 0x85;
289	giacomo	365	err = com_send(servo_port, b);
		366	err = com_receive(servo_port);
285	giacomo	367	if (err != (int)(b)) timer_expired = 1;
465	giacomo	368
358	giacomo	369	res_w = com_receive(servo_port); /* bit W */
		370	res_b = com_receive(servo_port); /* byte SPBRG */
465	giacomo	371
		372	if (res_w != -1 && res_b != -1) {
		373	if (res_w)
		374	res = SERVO_CLOCK / ( 16 * (res_b + 1) );
		375	else
		376	res = SERVO_CLOCK / ( 64 * (res_b + 1) );
		377	} else {
		378	return -1;
		379	}
358	giacomo	380
285	giacomo	381	if (timeout_event != NIL) kern_event_delete(timeout_event);
358	giacomo	382
285	giacomo	383	if (!timer_expired)
		384	return res;
		385	else
		386	return -1;
281	giacomo	387
		388	}
		389
286	giacomo	390	/* 1000.0100 */
358	giacomo	391	int servo_store_RS232_baudrate(int port)
281	giacomo	392	{
381	giacomo	393	#ifdef SERVO_TIMEOUT_EVENT
		394	struct timespec current_time;
		395	#endif
285	giacomo	396	unsigned char b;
		397	int err;
358	giacomo	398	int servo_port = (unsigned)(port);
		399
285	giacomo	400	timer_expired = 0;
381	giacomo	401	#ifdef SERVO_TIMEOUT_EVENT
		402	kern_gettime(&current_time);
		403	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		404	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		405	#else
		406	timeout_event = NIL;
		407	#endif
		408
358	giacomo	409
286	giacomo	410	b = 0x84;
289	giacomo	411	err = com_send(servo_port, b);
		412	err = com_receive(servo_port);
285	giacomo	413	if (err != (int)(b)) timer_expired = 1;
358	giacomo	414
285	giacomo	415	if (timeout_event != NIL) kern_event_delete(timeout_event);
358	giacomo	416
285	giacomo	417	if (!timer_expired)
		418	return 0;
		419	else
358	giacomo	420	return -1;
281	giacomo	421
		422	}
		423
289	giacomo	424	/* 1000.1010:llll.llll */
358	giacomo	425	int servo_set_period(int port, int period)
281	giacomo	426	{
381	giacomo	427	#ifdef SERVO_TIMEOUT_EVENT
		428	struct timespec current_time;
		429	#endif
285	giacomo	430	unsigned char b;
		431	int err;
358	giacomo	432	int servo_port = (unsigned)(port);
		433
285	giacomo	434	timer_expired = 0;
381	giacomo	435	#ifdef SERVO_TIMEOUT_EVENT
		436	kern_gettime(&current_time);
		437	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		438	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		439	#else
		440	timeout_event = NIL;
		441	#endif
		442
289	giacomo	443	b = 0x8A;
		444	err = com_send(servo_port, b);
		445	err = com_receive(servo_port);
285	giacomo	446	if (err != (int)(b)) timer_expired = 1;
358	giacomo	447
344	giacomo	448	b = (period*1000)/TICK_LEN_PERIOD/8 & 0xFF;
289	giacomo	449	err = com_send(servo_port, b);
		450	err = com_receive(servo_port);
285	giacomo	451	if (err != (int)(b)) timer_expired = 1;
358	giacomo	452
285	giacomo	453	if (timeout_event != NIL) kern_event_delete(timeout_event);
358	giacomo	454
285	giacomo	455	if (!timer_expired)
		456	return 0;
		457	else
		458	return -1;
281	giacomo	459
		460	}
		461
285	giacomo	462	/* 1000.1001 */
358	giacomo	463	int servo_get_period(int port)
281	giacomo	464	{
381	giacomo	465	#ifdef SERVO_TIMEOUT_EVENT
		466	struct timespec current_time;
		467	#endif
285	giacomo	468	unsigned char b;
		469	int err,res;
358	giacomo	470	int servo_port = (unsigned)(port);
		471
285	giacomo	472	timer_expired = 0;
358	giacomo	473
381	giacomo	474	#ifdef SERVO_TIMEOUT_EVENT
		475	kern_gettime(&current_time);
		476	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		477	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		478	#else
		479	timeout_event = NIL;
		480	#endif
		481
285	giacomo	482	b = 0x89;
289	giacomo	483	err = com_send(servo_port, b);
		484	err = com_receive(servo_port);
285	giacomo	485	if (err != (int)(b)) timer_expired = 1;
358	giacomo	486	res = com_receive(servo_port);
		487
285	giacomo	488	if (timeout_event != NIL) kern_event_delete(timeout_event);
358	giacomo	489
285	giacomo	490	if (!timer_expired)
345	giacomo	491	return (((unsigned char)(res))TICK_LEN_PERIOD/10008);
285	giacomo	492	else
		493	return -1;
281	giacomo	494
		495	}
		496
289	giacomo	497	/* 1000.1000 */
358	giacomo	498	int servo_store_period(int port)
281	giacomo	499	{
381	giacomo	500	#ifdef SERVO_TIMEOUT_EVENT
		501	struct timespec current_time;
		502	#endif
285	giacomo	503	unsigned char b;
		504	int err;
358	giacomo	505	int servo_port = (unsigned)(port);
		506
285	giacomo	507	timer_expired = 0;
358	giacomo	508
381	giacomo	509	#ifdef SERVO_TIMEOUT_EVENT
		510	kern_gettime(&current_time);
		511	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		512	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		513	#else
		514	timeout_event = NIL;
		515	#endif
		516
289	giacomo	517	b = 0x88;
		518	err = com_send(servo_port, b);
		519	err = com_receive(servo_port);
285	giacomo	520	if (err != (int)(b)) timer_expired = 1;
358	giacomo	521
285	giacomo	522	if (timeout_event != NIL) kern_event_delete(timeout_event);
358	giacomo	523
285	giacomo	524	if (!timer_expired)
		525	return 0;
		526	else
358	giacomo	527	return -1;
281	giacomo	528
		529	}
		530
283	giacomo	531	/* 1000.1100 */
358	giacomo	532	int servo_get_setup_switch(int port)
281	giacomo	533	{
381	giacomo	534	#ifdef SERVO_TIMEOUT_EVENT
		535	struct timespec current_time;
		536	#endif
283	giacomo	537	unsigned char b;
358	giacomo	538	int err,res;
		539	int servo_port = (unsigned)(port);
		540
283	giacomo	541	timer_expired = 0;
381	giacomo	542
		543	#ifdef SERVO_TIMEOUT_EVENT
		544	kern_gettime(&current_time);
		545	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		546	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		547	#else
		548	timeout_event = NIL;
		549	#endif
281	giacomo	550
283	giacomo	551	b = 0x8C;
289	giacomo	552	err = com_send(servo_port, b);
		553	err = com_receive(servo_port);
283	giacomo	554	if (err != (int)(b)) timer_expired = 1;
289	giacomo	555	res = com_receive(servo_port);
281	giacomo	556
283	giacomo	557	if (timeout_event != NIL) kern_event_delete(timeout_event);
		558
		559	if (!timer_expired)
		560	return res;
		561	else
		562	return -1;
		563
281	giacomo	564	}
		565
283	giacomo	566	/* 1000.111s */
358	giacomo	567	int servo_set_RC5_switch(int port, int data)
281	giacomo	568	{
381	giacomo	569	#ifdef SERVO_TIMEOUT_EVENT
		570	struct timespec current_time;
		571	#endif
283	giacomo	572	unsigned char b;
		573	int err;
358	giacomo	574	int servo_port = (unsigned)(port);
		575
283	giacomo	576	timer_expired = 0;
281	giacomo	577
381	giacomo	578	#ifdef SERVO_TIMEOUT_EVENT
		579	kern_gettime(&current_time);
		580	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		581	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		582	#else
		583	timeout_event = NIL;
		584	#endif
		585
283	giacomo	586	b = 0x8E \| (data & 0x01);
289	giacomo	587	err = com_send(servo_port, b);
		588	err = com_receive(servo_port);
283	giacomo	589	if (err != (int)(b)) timer_expired = 1;
		590
358	giacomo	591	if (timeout_event != NIL) kern_event_delete(timeout_event);
		592
283	giacomo	593	if (!timer_expired)
		594	return 0;
		595	else
		596	return -1;
		597
281	giacomo	598	}
		599
323	giacomo	600	/* 1000.0000:0000.Mmmm */
358	giacomo	601	int servo_turn_off(int port, int servo)
285	giacomo	602	{
		603
381	giacomo	604	#ifdef SERVO_TIMEOUT_EVENT
		605	struct timespec current_time;
		606	#endif
285	giacomo	607	unsigned char b;
358	giacomo	608	int err;
		609	int servo_port = (unsigned)(port);
		610
		611	if (servo > 15) return -1;
285	giacomo	612	timer_expired = 0;
381	giacomo	613
		614	#ifdef SERVO_TIMEOUT_EVENT
		615	kern_gettime(&current_time);
		616	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		617	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		618	#else
		619	timeout_event = NIL;
		620	#endif
285	giacomo	621
		622	b = 0x80;
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
323	giacomo	627	b = 0x00 \| (servo & 0x0F);
289	giacomo	628	err = com_send(servo_port, b);
		629	err = com_receive(servo_port);
285	giacomo	630	if (err != (int)(b)) timer_expired = 1;
		631
		632	if (timeout_event != NIL) kern_event_delete(timeout_event);
358	giacomo	633
285	giacomo	634	if (!timer_expired)
		635	return 0;
		636	else
		637	return -1;
		638
		639	}
		640
325	giacomo	641	/* 1000.0000:0001.Mmmm */
358	giacomo	642	int servo_turn_on(int port, int servo)
285	giacomo	643	{
358	giacomo	644
381	giacomo	645	#ifdef SERVO_TIMEOUT_EVENT
		646	struct timespec current_time;
		647	#endif
285	giacomo	648	unsigned char b;
		649	int err;
358	giacomo	650	int servo_port = (unsigned)(port);
		651
323	giacomo	652	if (servo > 15) return -1;
358	giacomo	653
285	giacomo	654	timer_expired = 0;
381	giacomo	655
		656	#ifdef SERVO_TIMEOUT_EVENT
		657	kern_gettime(&current_time);
		658	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		659	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		660	#else
		661	timeout_event = NIL;
		662	#endif
358	giacomo	663
285	giacomo	664	b = 0x80;
289	giacomo	665	err = com_send(servo_port, b);
		666	err = com_receive(servo_port);
285	giacomo	667	if (err != (int)(b)) timer_expired = 1;
358	giacomo	668
323	giacomo	669	b = 0x10 \| (servo & 0x0F);
289	giacomo	670	err = com_send(servo_port, b);
		671	err = com_receive(servo_port);
285	giacomo	672	if (err != (int)(b)) timer_expired = 1;
358	giacomo	673
285	giacomo	674	if (timeout_event != NIL) kern_event_delete(timeout_event);
358	giacomo	675
285	giacomo	676	if (!timer_expired)
		677	return 0;
		678	else
		679	return -1;
358	giacomo	680
285	giacomo	681	}
		682
291	giacomo	683	/* 1000.0000:0010.0000 */
358	giacomo	684	int servo_turn_off_all(int port)
323	giacomo	685	{
381	giacomo	686	#ifdef SERVO_TIMEOUT_EVENT
		687	struct timespec current_time;
		688	#endif
285	giacomo	689	unsigned char b;
		690	int err;
358	giacomo	691	int servo_port = (unsigned)(port);
		692
285	giacomo	693	timer_expired = 0;
381	giacomo	694
		695	#ifdef SERVO_TIMEOUT_EVENT
		696	kern_gettime(&current_time);
		697	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		698	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		699	#else
		700	timeout_event = NIL;
		701	#endif
358	giacomo	702
285	giacomo	703	b = 0x80;
289	giacomo	704	err = com_send(servo_port, b);
		705	err = com_receive(servo_port);
285	giacomo	706	if (err != (int)(b)) timer_expired = 1;
358	giacomo	707
291	giacomo	708	b = 0x20;
289	giacomo	709	err = com_send(servo_port, b);
		710	err = com_receive(servo_port);
285	giacomo	711	if (err != (int)(b)) timer_expired = 1;
358	giacomo	712
285	giacomo	713	if (timeout_event != NIL) kern_event_delete(timeout_event);
358	giacomo	714
285	giacomo	715	if (!timer_expired)
		716	return 0;
		717	else
		718	return -1;
358	giacomo	719
285	giacomo	720	}
		721
291	giacomo	722	/* 1000.0000:0010.0001 */
358	giacomo	723	int servo_turn_on_all(int port)
285	giacomo	724	{
381	giacomo	725	#ifdef SERVO_TIMEOUT_EVENT
		726	struct timespec current_time;
		727	#endif
285	giacomo	728	unsigned char b;
		729	int err;
358	giacomo	730	int servo_port = (unsigned)(port);
		731
285	giacomo	732	timer_expired = 0;
358	giacomo	733
381	giacomo	734	#ifdef SERVO_TIMEOUT_EVENT
		735	kern_gettime(&current_time);
		736	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		737	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		738	#else
		739	timeout_event = NIL;
		740	#endif
		741
285	giacomo	742	b = 0x80;
289	giacomo	743	err = com_send(servo_port, b);
		744	err = com_receive(servo_port);
285	giacomo	745	if (err != (int)(b)) timer_expired = 1;
358	giacomo	746
291	giacomo	747	b = 0x21;
289	giacomo	748	err = com_send(servo_port, b);
		749	err = com_receive(servo_port);
285	giacomo	750	if (err != (int)(b)) timer_expired = 1;
358	giacomo	751
285	giacomo	752	if (timeout_event != NIL) kern_event_delete(timeout_event);
358	giacomo	753
285	giacomo	754	if (!timer_expired)
		755	return 0;
		756	else
		757	return -1;
358	giacomo	758
285	giacomo	759	}
		760
323	giacomo	761	/* 1000.0000:0101.000M:mmmm.mmmm */
358	giacomo	762	int servo_set_levels(int port, int bank,int mask)
315	giacomo	763	{
381	giacomo	764	#ifdef SERVO_TIMEOUT_EVENT
		765	struct timespec current_time;
		766	#endif
315	giacomo	767	unsigned char b;
		768	int err;
358	giacomo	769	int servo_port = (unsigned)(port);
		770
315	giacomo	771	timer_expired = 0;
381	giacomo	772
		773	#ifdef SERVO_TIMEOUT_EVENT
		774	kern_gettime(&current_time);
		775	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		776	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		777	#else
		778	timeout_event = NIL;
		779	#endif
358	giacomo	780
315	giacomo	781	b = 0x80;
		782	err = com_send(servo_port, b);
		783	err = com_receive(servo_port);
		784	if (err != (int)(b)) timer_expired = 1;
358	giacomo	785
323	giacomo	786	b = 0x50 \| (0x01 & bank);
315	giacomo	787	err = com_send(servo_port, b);
		788	err = com_receive(servo_port);
		789	if (err != (int)(b)) timer_expired = 1;
358	giacomo	790
315	giacomo	791	b = (unsigned char)(mask & 0xFF);
		792	err = com_send(servo_port, b);
		793	err = com_receive(servo_port);
		794	if (err != (int)(b)) timer_expired = 1;
358	giacomo	795
315	giacomo	796	if (timeout_event != NIL) kern_event_delete(timeout_event);
358	giacomo	797
315	giacomo	798	if (!timer_expired)
		799	return 0;
		800	else
		801	return -1;
358	giacomo	802
315	giacomo	803	}
		804
323	giacomo	805	/* 1000.0000:0100.000M */
358	giacomo	806	int servo_get_levels(int port, int bank)
297	giacomo	807	{
381	giacomo	808	#ifdef SERVO_TIMEOUT_EVENT
		809	struct timespec current_time;
		810	#endif
297	giacomo	811	unsigned char b;
		812	int err;
358	giacomo	813	int servo_port = (unsigned)(port);
		814
297	giacomo	815	timer_expired = 0;
381	giacomo	816
		817	#ifdef SERVO_TIMEOUT_EVENT
		818	kern_gettime(&current_time);
		819	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		820	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		821	#else
		822	timeout_event = NIL;
		823	#endif
358	giacomo	824
297	giacomo	825	b = 0x80;
		826	err = com_send(servo_port, b);
		827	err = com_receive(servo_port);
		828	if (err != (int)(b)) timer_expired = 1;
358	giacomo	829
323	giacomo	830	b = 0x40 \| (0x01 & bank);
297	giacomo	831	err = com_send(servo_port, b);
		832	err = com_receive(servo_port);
		833	if (err != (int)(b)) timer_expired = 1;
358	giacomo	834	err = com_receive(servo_port);
		835
297	giacomo	836	if (timeout_event != NIL) kern_event_delete(timeout_event);
358	giacomo	837
297	giacomo	838	if (!timer_expired)
316	giacomo	839	return err;
297	giacomo	840	else
		841	return -1;
358	giacomo	842
297	giacomo	843	}
		844
		845	/* 1000.0000:1000.0000 */
358	giacomo	846	int servo_store_levels(int port)
297	giacomo	847	{
381	giacomo	848	#ifdef SERVO_TIMEOUT_EVENT
		849	struct timespec current_time;
		850	#endif
297	giacomo	851	unsigned char b;
		852	int err;
358	giacomo	853	int servo_port = (unsigned)(port);
		854
297	giacomo	855	timer_expired = 0;
381	giacomo	856
		857	#ifdef SERVO_TIMEOUT_EVENT
		858	kern_gettime(&current_time);
		859	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		860	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		861	#else
		862	timeout_event = NIL;
		863	#endif
358	giacomo	864
297	giacomo	865	b = 0x80;
		866	err = com_send(servo_port, b);
		867	err = com_receive(servo_port);
		868	if (err != (int)(b)) timer_expired = 1;
358	giacomo	869
297	giacomo	870	b = 0x80;
		871	err = com_send(servo_port, b);
		872	err = com_receive(servo_port);
		873	if (err != (int)(b)) timer_expired = 1;
358	giacomo	874
297	giacomo	875	if (timeout_event != NIL) kern_event_delete(timeout_event);
358	giacomo	876
297	giacomo	877	if (!timer_expired)
		878	return 0;
		879	else
		880	return -1;
358	giacomo	881
297	giacomo	882	}
		883
493	giacomo	884	int servo_set_max_angle_sec(int port, int servo, int angle_sec)
290	giacomo	885	{
		886
381	giacomo	887	servo_table[port][servo].max_angle_sec = angle_sec;
290	giacomo	888	return 0;
		889
		890	}
		891
493	giacomo	892	int servo_set_min_angle_sec(int port, int servo, int angle_sec)
290	giacomo	893	{
		894
381	giacomo	895	servo_table[port][servo].min_angle_sec = angle_sec;
290	giacomo	896	return 0;
		897
		898	}
		899
381	giacomo	900	int servo_set_servo_tick(int port, int servo, int zero_tick, int delta_tick)
		901	{
		902
		903	if (zero_tick != -1) servo_table[port][servo].zero_tick = zero_tick;
		904	if (delta_tick != -1) servo_table[port][servo].delta_tick = delta_tick;
		905	return 0;
		906
		907	}
		908
323	giacomo	909	/* 0000.Pppp:0000.vvvv:vvvv.vvvv */
358	giacomo	910	int servo_set_angle_sec(int port, int servo, int angle_sec)
281	giacomo	911	{
381	giacomo	912	#ifdef SERVO_TIMEOUT_EVENT
		913	struct timespec current_time;
		914	#endif
282	giacomo	915	unsigned char b;
290	giacomo	916	int err, angle_tick;
358	giacomo	917	int servo_port = (unsigned)(port);
282	giacomo	918
323	giacomo	919	if (servo > 15) return -1;
282	giacomo	920
381	giacomo	921	if (angle_sec > servo_table[port][servo].max_angle_sec \|\|
		922	angle_sec < servo_table[port][servo].min_angle_sec) return -1;
290	giacomo	923
381	giacomo	924	angle_tick = (servo_table[port][servo].zero_tick + angle_sec *
		925	servo_table[port][servo].delta_tick /
		926	(servo_table[port][servo].max_angle_sec - servo_table[port][servo].min_angle_sec)) *
		927	1000 / TICK_LEN;
290	giacomo	928
282	giacomo	929	timer_expired = 0;
381	giacomo	930
		931	#ifdef SERVO_TIMEOUT_EVENT
		932	kern_gettime(&current_time);
		933	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		934	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		935	#else
		936	timeout_event = NIL;
		937	#endif
358	giacomo	938
323	giacomo	939	b = 0x00 \| (servo & 0x0F);
289	giacomo	940	err = com_send(servo_port, b);
		941	err = com_receive(servo_port);
283	giacomo	942	if (err != (int)(b)) timer_expired = 1;
282	giacomo	943
290	giacomo	944	b = 0x00 \| ((angle_tick >> 8) & 0x0F);
289	giacomo	945	err = com_send(servo_port, b);
		946	err = com_receive(servo_port);
283	giacomo	947	if (err != (int)(b)) timer_expired = 1;
282	giacomo	948
290	giacomo	949	b = 0x00 \| (angle_tick & 0xFF);
289	giacomo	950	err = com_send(servo_port, b);
		951	err = com_receive(servo_port);
358	giacomo	952	if (err != (int)(b)) timer_expired = 1;
282	giacomo	953
283	giacomo	954	if (timeout_event != NIL) kern_event_delete(timeout_event);
282	giacomo	955
283	giacomo	956	if (!timer_expired)
		957	return 0;
		958	else
		959	return -1;
281	giacomo	960
		961	}
		962
323	giacomo	963	/* 0010.Pppp */
358	giacomo	964	int servo_store_default_position(int port, int servo)
290	giacomo	965	{
381	giacomo	966	#ifdef SERVO_TIMEOUT_EVENT
		967	struct timespec current_time;
		968	#endif
290	giacomo	969	unsigned char b;
		970	int err;
358	giacomo	971	int servo_port = (unsigned)(port);
290	giacomo	972
323	giacomo	973	if (servo > 15) return -1;
358	giacomo	974
290	giacomo	975	timer_expired = 0;
381	giacomo	976
		977	#ifdef SERVO_TIMEOUT_EVENT
		978	kern_gettime(&current_time);
		979	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		980	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		981	#else
		982	timeout_event = NIL;
		983	#endif
		984
323	giacomo	985	b = 0x20 \| (servo & 0x0F);
290	giacomo	986	err = com_send(servo_port, b);
		987	err = com_receive(servo_port);
		988	if (err != (int)(b)) timer_expired = 1;
358	giacomo	989
290	giacomo	990	if (timeout_event != NIL) kern_event_delete(timeout_event);
358	giacomo	991
290	giacomo	992	if (!timer_expired)
		993	return 0;
		994	else
		995	return -1;
		996
		997	}
		998
323	giacomo	999	/* 0001.Pppp */
358	giacomo	1000	int servo_get_angle_sec(int port, int servo)
281	giacomo	1001	{
381	giacomo	1002	#ifdef SERVO_TIMEOUT_EVENT
		1003	struct timespec current_time;
		1004	#endif
282	giacomo	1005	unsigned char b;
290	giacomo	1006	int err,res,data;
358	giacomo	1007	int servo_port = (unsigned)(port);
		1008
323	giacomo	1009	if (servo > 15) return -1;
358	giacomo	1010
282	giacomo	1011	timer_expired = 0;
381	giacomo	1012
		1013	#ifdef SERVO_TIMEOUT_EVENT
		1014	kern_gettime(&current_time);
		1015	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		1016	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		1017	#else
		1018	timeout_event = NIL;
		1019	#endif
281	giacomo	1020
323	giacomo	1021	b = 0x10 \| (servo & 0x0F);
289	giacomo	1022	err = com_send(servo_port, b);
		1023	err = com_receive(servo_port);
283	giacomo	1024	if (err != (int)(b)) timer_expired = 1;
289	giacomo	1025	res = com_receive(servo_port) << 8;
		1026	res \|= com_receive(servo_port);
282	giacomo	1027
283	giacomo	1028	if (timeout_event != NIL) kern_event_delete(timeout_event);
282	giacomo	1029
381	giacomo	1030	data = ((resTICK_LEN/1000) - servo_table[port][servo].zero_tick)
		1031	(servo_table[port][servo].max_angle_sec - servo_table[port][servo].min_angle_sec) /
		1032	servo_table[port][servo].delta_tick;
290	giacomo	1033
282	giacomo	1034	if (!timer_expired)
290	giacomo	1035	return data;
282	giacomo	1036	else
		1037	return -1;
		1038
281	giacomo	1039	}
		1040
282	giacomo	1041	/* 0100:0aaa */
358	giacomo	1042	int servo_get_analog(int port, int adport)
281	giacomo	1043	{
		1044
381	giacomo	1045	#ifdef SERVO_TIMEOUT_EVENT
		1046	struct timespec current_time;
		1047	#endif
282	giacomo	1048	unsigned char b;
283	giacomo	1049	int err,res;
358	giacomo	1050	int servo_port = (unsigned)(port);
		1051
381	giacomo	1052	if (adport > 7) return -1;
358	giacomo	1053
282	giacomo	1054	timer_expired = 0;
381	giacomo	1055
		1056	#ifdef SERVO_TIMEOUT_EVENT
		1057	kern_gettime(&current_time);
		1058	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		1059	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		1060	#else
		1061	timeout_event = NIL;
		1062	#endif
281	giacomo	1063
358	giacomo	1064	b = 0x40 \| (adport & 0x07);
289	giacomo	1065	err = com_send(servo_port, b);
		1066	err = com_receive(servo_port);
283	giacomo	1067	if (err != (int)(b)) timer_expired = 1;
289	giacomo	1068	res = com_receive(servo_port) << 8;
		1069	res \|= com_receive(servo_port);
282	giacomo	1070
283	giacomo	1071	if (timeout_event != NIL) kern_event_delete(timeout_event);
282	giacomo	1072
		1073	if (!timer_expired)
		1074	return res;
		1075	else
		1076	return -1;
		1077
281	giacomo	1078	}
		1079

Subversion Repositories shark

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