WebSVN - shark - Blame - Rev 412 - /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
381	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;
412	giacomo	300	spbrg_temp = (SERVO_CLOCK10 / (160baud)) - 10;
		301	if (spbrg_temp>2550) {
358	giacomo	302	w = 0;
412	giacomo	303	spbrg_temp = (SERVO_CLOCK10 / (640baud)) - 10;
286	giacomo	304	}
412	giacomo	305	spbrg = spbrg_temp / 10;
		306	if (spbrg_temp%10 > 5) spbrg++;
		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;
358	giacomo	368	res_w = com_receive(servo_port); /* bit W */
		369	res_b = com_receive(servo_port); /* byte SPBRG */
		370	if (res_w)
		371	res = SERVO_CLOCK / ( 16 * (res_b + 1) );
		372	else
		373	res = SERVO_CLOCK / ( 64 * (res_b + 1) );
		374
285	giacomo	375	if (timeout_event != NIL) kern_event_delete(timeout_event);
358	giacomo	376
285	giacomo	377	if (!timer_expired)
		378	return res;
		379	else
		380	return -1;
281	giacomo	381
		382	}
		383
286	giacomo	384	/* 1000.0100 */
358	giacomo	385	int servo_store_RS232_baudrate(int port)
281	giacomo	386	{
381	giacomo	387	#ifdef SERVO_TIMEOUT_EVENT
		388	struct timespec current_time;
		389	#endif
285	giacomo	390	unsigned char b;
		391	int err;
358	giacomo	392	int servo_port = (unsigned)(port);
		393
285	giacomo	394	timer_expired = 0;
381	giacomo	395	#ifdef SERVO_TIMEOUT_EVENT
		396	kern_gettime(&current_time);
		397	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		398	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		399	#else
		400	timeout_event = NIL;
		401	#endif
		402
358	giacomo	403
286	giacomo	404	b = 0x84;
289	giacomo	405	err = com_send(servo_port, b);
		406	err = com_receive(servo_port);
285	giacomo	407	if (err != (int)(b)) timer_expired = 1;
358	giacomo	408
285	giacomo	409	if (timeout_event != NIL) kern_event_delete(timeout_event);
358	giacomo	410
285	giacomo	411	if (!timer_expired)
		412	return 0;
		413	else
358	giacomo	414	return -1;
281	giacomo	415
		416	}
		417
289	giacomo	418	/* 1000.1010:llll.llll */
358	giacomo	419	int servo_set_period(int port, int period)
281	giacomo	420	{
381	giacomo	421	#ifdef SERVO_TIMEOUT_EVENT
		422	struct timespec current_time;
		423	#endif
285	giacomo	424	unsigned char b;
		425	int err;
358	giacomo	426	int servo_port = (unsigned)(port);
		427
285	giacomo	428	timer_expired = 0;
381	giacomo	429	#ifdef SERVO_TIMEOUT_EVENT
		430	kern_gettime(&current_time);
		431	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		432	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		433	#else
		434	timeout_event = NIL;
		435	#endif
		436
289	giacomo	437	b = 0x8A;
		438	err = com_send(servo_port, b);
		439	err = com_receive(servo_port);
285	giacomo	440	if (err != (int)(b)) timer_expired = 1;
358	giacomo	441
344	giacomo	442	b = (period*1000)/TICK_LEN_PERIOD/8 & 0xFF;
289	giacomo	443	err = com_send(servo_port, b);
		444	err = com_receive(servo_port);
285	giacomo	445	if (err != (int)(b)) timer_expired = 1;
358	giacomo	446
285	giacomo	447	if (timeout_event != NIL) kern_event_delete(timeout_event);
358	giacomo	448
285	giacomo	449	if (!timer_expired)
		450	return 0;
		451	else
		452	return -1;
281	giacomo	453
		454	}
		455
285	giacomo	456	/* 1000.1001 */
358	giacomo	457	int servo_get_period(int port)
281	giacomo	458	{
381	giacomo	459	#ifdef SERVO_TIMEOUT_EVENT
		460	struct timespec current_time;
		461	#endif
285	giacomo	462	unsigned char b;
		463	int err,res;
358	giacomo	464	int servo_port = (unsigned)(port);
		465
285	giacomo	466	timer_expired = 0;
358	giacomo	467
381	giacomo	468	#ifdef SERVO_TIMEOUT_EVENT
		469	kern_gettime(&current_time);
		470	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		471	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		472	#else
		473	timeout_event = NIL;
		474	#endif
		475
285	giacomo	476	b = 0x89;
289	giacomo	477	err = com_send(servo_port, b);
		478	err = com_receive(servo_port);
285	giacomo	479	if (err != (int)(b)) timer_expired = 1;
358	giacomo	480	res = com_receive(servo_port);
		481
285	giacomo	482	if (timeout_event != NIL) kern_event_delete(timeout_event);
358	giacomo	483
285	giacomo	484	if (!timer_expired)
345	giacomo	485	return (((unsigned char)(res))TICK_LEN_PERIOD/10008);
285	giacomo	486	else
		487	return -1;
281	giacomo	488
		489	}
		490
289	giacomo	491	/* 1000.1000 */
358	giacomo	492	int servo_store_period(int port)
281	giacomo	493	{
381	giacomo	494	#ifdef SERVO_TIMEOUT_EVENT
		495	struct timespec current_time;
		496	#endif
285	giacomo	497	unsigned char b;
		498	int err;
358	giacomo	499	int servo_port = (unsigned)(port);
		500
285	giacomo	501	timer_expired = 0;
358	giacomo	502
381	giacomo	503	#ifdef SERVO_TIMEOUT_EVENT
		504	kern_gettime(&current_time);
		505	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		506	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		507	#else
		508	timeout_event = NIL;
		509	#endif
		510
289	giacomo	511	b = 0x88;
		512	err = com_send(servo_port, b);
		513	err = com_receive(servo_port);
285	giacomo	514	if (err != (int)(b)) timer_expired = 1;
358	giacomo	515
285	giacomo	516	if (timeout_event != NIL) kern_event_delete(timeout_event);
358	giacomo	517
285	giacomo	518	if (!timer_expired)
		519	return 0;
		520	else
358	giacomo	521	return -1;
281	giacomo	522
		523	}
		524
283	giacomo	525	/* 1000.1100 */
358	giacomo	526	int servo_get_setup_switch(int port)
281	giacomo	527	{
381	giacomo	528	#ifdef SERVO_TIMEOUT_EVENT
		529	struct timespec current_time;
		530	#endif
283	giacomo	531	unsigned char b;
358	giacomo	532	int err,res;
		533	int servo_port = (unsigned)(port);
		534
283	giacomo	535	timer_expired = 0;
381	giacomo	536
		537	#ifdef SERVO_TIMEOUT_EVENT
		538	kern_gettime(&current_time);
		539	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		540	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		541	#else
		542	timeout_event = NIL;
		543	#endif
281	giacomo	544
283	giacomo	545	b = 0x8C;
289	giacomo	546	err = com_send(servo_port, b);
		547	err = com_receive(servo_port);
283	giacomo	548	if (err != (int)(b)) timer_expired = 1;
289	giacomo	549	res = com_receive(servo_port);
281	giacomo	550
283	giacomo	551	if (timeout_event != NIL) kern_event_delete(timeout_event);
		552
		553	if (!timer_expired)
		554	return res;
		555	else
		556	return -1;
		557
281	giacomo	558	}
		559
283	giacomo	560	/* 1000.111s */
358	giacomo	561	int servo_set_RC5_switch(int port, int data)
281	giacomo	562	{
381	giacomo	563	#ifdef SERVO_TIMEOUT_EVENT
		564	struct timespec current_time;
		565	#endif
283	giacomo	566	unsigned char b;
		567	int err;
358	giacomo	568	int servo_port = (unsigned)(port);
		569
283	giacomo	570	timer_expired = 0;
281	giacomo	571
381	giacomo	572	#ifdef SERVO_TIMEOUT_EVENT
		573	kern_gettime(&current_time);
		574	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		575	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		576	#else
		577	timeout_event = NIL;
		578	#endif
		579
283	giacomo	580	b = 0x8E \| (data & 0x01);
289	giacomo	581	err = com_send(servo_port, b);
		582	err = com_receive(servo_port);
283	giacomo	583	if (err != (int)(b)) timer_expired = 1;
		584
358	giacomo	585	if (timeout_event != NIL) kern_event_delete(timeout_event);
		586
283	giacomo	587	if (!timer_expired)
		588	return 0;
		589	else
		590	return -1;
		591
281	giacomo	592	}
		593
323	giacomo	594	/* 1000.0000:0000.Mmmm */
358	giacomo	595	int servo_turn_off(int port, int servo)
285	giacomo	596	{
		597
381	giacomo	598	#ifdef SERVO_TIMEOUT_EVENT
		599	struct timespec current_time;
		600	#endif
285	giacomo	601	unsigned char b;
358	giacomo	602	int err;
		603	int servo_port = (unsigned)(port);
		604
		605	if (servo > 15) return -1;
285	giacomo	606	timer_expired = 0;
381	giacomo	607
		608	#ifdef SERVO_TIMEOUT_EVENT
		609	kern_gettime(&current_time);
		610	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		611	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		612	#else
		613	timeout_event = NIL;
		614	#endif
285	giacomo	615
		616	b = 0x80;
289	giacomo	617	err = com_send(servo_port, b);
		618	err = com_receive(servo_port);
285	giacomo	619	if (err != (int)(b)) timer_expired = 1;
		620
323	giacomo	621	b = 0x00 \| (servo & 0x0F);
289	giacomo	622	err = com_send(servo_port, b);
		623	err = com_receive(servo_port);
285	giacomo	624	if (err != (int)(b)) timer_expired = 1;
		625
		626	if (timeout_event != NIL) kern_event_delete(timeout_event);
358	giacomo	627
285	giacomo	628	if (!timer_expired)
		629	return 0;
		630	else
		631	return -1;
		632
		633	}
		634
325	giacomo	635	/* 1000.0000:0001.Mmmm */
358	giacomo	636	int servo_turn_on(int port, int servo)
285	giacomo	637	{
358	giacomo	638
381	giacomo	639	#ifdef SERVO_TIMEOUT_EVENT
		640	struct timespec current_time;
		641	#endif
285	giacomo	642	unsigned char b;
		643	int err;
358	giacomo	644	int servo_port = (unsigned)(port);
		645
323	giacomo	646	if (servo > 15) return -1;
358	giacomo	647
285	giacomo	648	timer_expired = 0;
381	giacomo	649
		650	#ifdef SERVO_TIMEOUT_EVENT
		651	kern_gettime(&current_time);
		652	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		653	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		654	#else
		655	timeout_event = NIL;
		656	#endif
358	giacomo	657
285	giacomo	658	b = 0x80;
289	giacomo	659	err = com_send(servo_port, b);
		660	err = com_receive(servo_port);
285	giacomo	661	if (err != (int)(b)) timer_expired = 1;
358	giacomo	662
323	giacomo	663	b = 0x10 \| (servo & 0x0F);
289	giacomo	664	err = com_send(servo_port, b);
		665	err = com_receive(servo_port);
285	giacomo	666	if (err != (int)(b)) timer_expired = 1;
358	giacomo	667
285	giacomo	668	if (timeout_event != NIL) kern_event_delete(timeout_event);
358	giacomo	669
285	giacomo	670	if (!timer_expired)
		671	return 0;
		672	else
		673	return -1;
358	giacomo	674
285	giacomo	675	}
		676
291	giacomo	677	/* 1000.0000:0010.0000 */
358	giacomo	678	int servo_turn_off_all(int port)
323	giacomo	679	{
381	giacomo	680	#ifdef SERVO_TIMEOUT_EVENT
		681	struct timespec current_time;
		682	#endif
285	giacomo	683	unsigned char b;
		684	int err;
358	giacomo	685	int servo_port = (unsigned)(port);
		686
285	giacomo	687	timer_expired = 0;
381	giacomo	688
		689	#ifdef SERVO_TIMEOUT_EVENT
		690	kern_gettime(&current_time);
		691	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		692	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		693	#else
		694	timeout_event = NIL;
		695	#endif
358	giacomo	696
285	giacomo	697	b = 0x80;
289	giacomo	698	err = com_send(servo_port, b);
		699	err = com_receive(servo_port);
285	giacomo	700	if (err != (int)(b)) timer_expired = 1;
358	giacomo	701
291	giacomo	702	b = 0x20;
289	giacomo	703	err = com_send(servo_port, b);
		704	err = com_receive(servo_port);
285	giacomo	705	if (err != (int)(b)) timer_expired = 1;
358	giacomo	706
285	giacomo	707	if (timeout_event != NIL) kern_event_delete(timeout_event);
358	giacomo	708
285	giacomo	709	if (!timer_expired)
		710	return 0;
		711	else
		712	return -1;
358	giacomo	713
285	giacomo	714	}
		715
291	giacomo	716	/* 1000.0000:0010.0001 */
358	giacomo	717	int servo_turn_on_all(int port)
285	giacomo	718	{
381	giacomo	719	#ifdef SERVO_TIMEOUT_EVENT
		720	struct timespec current_time;
		721	#endif
285	giacomo	722	unsigned char b;
		723	int err;
358	giacomo	724	int servo_port = (unsigned)(port);
		725
285	giacomo	726	timer_expired = 0;
358	giacomo	727
381	giacomo	728	#ifdef SERVO_TIMEOUT_EVENT
		729	kern_gettime(&current_time);
		730	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		731	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		732	#else
		733	timeout_event = NIL;
		734	#endif
		735
285	giacomo	736	b = 0x80;
289	giacomo	737	err = com_send(servo_port, b);
		738	err = com_receive(servo_port);
285	giacomo	739	if (err != (int)(b)) timer_expired = 1;
358	giacomo	740
291	giacomo	741	b = 0x21;
289	giacomo	742	err = com_send(servo_port, b);
		743	err = com_receive(servo_port);
285	giacomo	744	if (err != (int)(b)) timer_expired = 1;
358	giacomo	745
285	giacomo	746	if (timeout_event != NIL) kern_event_delete(timeout_event);
358	giacomo	747
285	giacomo	748	if (!timer_expired)
		749	return 0;
		750	else
		751	return -1;
358	giacomo	752
285	giacomo	753	}
		754
323	giacomo	755	/* 1000.0000:0101.000M:mmmm.mmmm */
358	giacomo	756	int servo_set_levels(int port, int bank,int mask)
315	giacomo	757	{
381	giacomo	758	#ifdef SERVO_TIMEOUT_EVENT
		759	struct timespec current_time;
		760	#endif
315	giacomo	761	unsigned char b;
		762	int err;
358	giacomo	763	int servo_port = (unsigned)(port);
		764
315	giacomo	765	timer_expired = 0;
381	giacomo	766
		767	#ifdef SERVO_TIMEOUT_EVENT
		768	kern_gettime(&current_time);
		769	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		770	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		771	#else
		772	timeout_event = NIL;
		773	#endif
358	giacomo	774
315	giacomo	775	b = 0x80;
		776	err = com_send(servo_port, b);
		777	err = com_receive(servo_port);
		778	if (err != (int)(b)) timer_expired = 1;
358	giacomo	779
323	giacomo	780	b = 0x50 \| (0x01 & bank);
315	giacomo	781	err = com_send(servo_port, b);
		782	err = com_receive(servo_port);
		783	if (err != (int)(b)) timer_expired = 1;
358	giacomo	784
315	giacomo	785	b = (unsigned char)(mask & 0xFF);
		786	err = com_send(servo_port, b);
		787	err = com_receive(servo_port);
		788	if (err != (int)(b)) timer_expired = 1;
358	giacomo	789
315	giacomo	790	if (timeout_event != NIL) kern_event_delete(timeout_event);
358	giacomo	791
315	giacomo	792	if (!timer_expired)
		793	return 0;
		794	else
		795	return -1;
358	giacomo	796
315	giacomo	797	}
		798
323	giacomo	799	/* 1000.0000:0100.000M */
358	giacomo	800	int servo_get_levels(int port, int bank)
297	giacomo	801	{
381	giacomo	802	#ifdef SERVO_TIMEOUT_EVENT
		803	struct timespec current_time;
		804	#endif
297	giacomo	805	unsigned char b;
		806	int err;
358	giacomo	807	int servo_port = (unsigned)(port);
		808
297	giacomo	809	timer_expired = 0;
381	giacomo	810
		811	#ifdef SERVO_TIMEOUT_EVENT
		812	kern_gettime(&current_time);
		813	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		814	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		815	#else
		816	timeout_event = NIL;
		817	#endif
358	giacomo	818
297	giacomo	819	b = 0x80;
		820	err = com_send(servo_port, b);
		821	err = com_receive(servo_port);
		822	if (err != (int)(b)) timer_expired = 1;
358	giacomo	823
323	giacomo	824	b = 0x40 \| (0x01 & bank);
297	giacomo	825	err = com_send(servo_port, b);
		826	err = com_receive(servo_port);
		827	if (err != (int)(b)) timer_expired = 1;
358	giacomo	828	err = com_receive(servo_port);
		829
297	giacomo	830	if (timeout_event != NIL) kern_event_delete(timeout_event);
358	giacomo	831
297	giacomo	832	if (!timer_expired)
316	giacomo	833	return err;
297	giacomo	834	else
		835	return -1;
358	giacomo	836
297	giacomo	837	}
		838
		839	/* 1000.0000:1000.0000 */
358	giacomo	840	int servo_store_levels(int port)
297	giacomo	841	{
381	giacomo	842	#ifdef SERVO_TIMEOUT_EVENT
		843	struct timespec current_time;
		844	#endif
297	giacomo	845	unsigned char b;
		846	int err;
358	giacomo	847	int servo_port = (unsigned)(port);
		848
297	giacomo	849	timer_expired = 0;
381	giacomo	850
		851	#ifdef SERVO_TIMEOUT_EVENT
		852	kern_gettime(&current_time);
		853	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		854	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		855	#else
		856	timeout_event = NIL;
		857	#endif
358	giacomo	858
297	giacomo	859	b = 0x80;
		860	err = com_send(servo_port, b);
		861	err = com_receive(servo_port);
		862	if (err != (int)(b)) timer_expired = 1;
358	giacomo	863
297	giacomo	864	b = 0x80;
		865	err = com_send(servo_port, b);
		866	err = com_receive(servo_port);
		867	if (err != (int)(b)) timer_expired = 1;
358	giacomo	868
297	giacomo	869	if (timeout_event != NIL) kern_event_delete(timeout_event);
358	giacomo	870
297	giacomo	871	if (!timer_expired)
		872	return 0;
		873	else
		874	return -1;
358	giacomo	875
297	giacomo	876	}
		877
358	giacomo	878	int servo_set_max_angle(int port, int servo, int angle_sec)
290	giacomo	879	{
		880
381	giacomo	881	servo_table[port][servo].max_angle_sec = angle_sec;
290	giacomo	882	return 0;
		883
		884	}
		885
381	giacomo	886	int servo_set_min_angle(int port, int servo, int angle_sec)
290	giacomo	887	{
		888
381	giacomo	889	servo_table[port][servo].min_angle_sec = angle_sec;
290	giacomo	890	return 0;
		891
		892	}
		893
381	giacomo	894	int servo_set_servo_tick(int port, int servo, int zero_tick, int delta_tick)
		895	{
		896
		897	if (zero_tick != -1) servo_table[port][servo].zero_tick = zero_tick;
		898	if (delta_tick != -1) servo_table[port][servo].delta_tick = delta_tick;
		899	return 0;
		900
		901	}
		902
323	giacomo	903	/* 0000.Pppp:0000.vvvv:vvvv.vvvv */
358	giacomo	904	int servo_set_angle_sec(int port, int servo, int angle_sec)
281	giacomo	905	{
381	giacomo	906	#ifdef SERVO_TIMEOUT_EVENT
		907	struct timespec current_time;
		908	#endif
282	giacomo	909	unsigned char b;
290	giacomo	910	int err, angle_tick;
358	giacomo	911	int servo_port = (unsigned)(port);
282	giacomo	912
323	giacomo	913	if (servo > 15) return -1;
282	giacomo	914
381	giacomo	915	if (angle_sec > servo_table[port][servo].max_angle_sec \|\|
		916	angle_sec < servo_table[port][servo].min_angle_sec) return -1;
290	giacomo	917
381	giacomo	918	angle_tick = (servo_table[port][servo].zero_tick + angle_sec *
		919	servo_table[port][servo].delta_tick /
		920	(servo_table[port][servo].max_angle_sec - servo_table[port][servo].min_angle_sec)) *
		921	1000 / TICK_LEN;
290	giacomo	922
282	giacomo	923	timer_expired = 0;
381	giacomo	924
		925	#ifdef SERVO_TIMEOUT_EVENT
		926	kern_gettime(&current_time);
		927	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		928	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		929	#else
		930	timeout_event = NIL;
		931	#endif
358	giacomo	932
323	giacomo	933	b = 0x00 \| (servo & 0x0F);
289	giacomo	934	err = com_send(servo_port, b);
		935	err = com_receive(servo_port);
283	giacomo	936	if (err != (int)(b)) timer_expired = 1;
282	giacomo	937
290	giacomo	938	b = 0x00 \| ((angle_tick >> 8) & 0x0F);
289	giacomo	939	err = com_send(servo_port, b);
		940	err = com_receive(servo_port);
283	giacomo	941	if (err != (int)(b)) timer_expired = 1;
282	giacomo	942
290	giacomo	943	b = 0x00 \| (angle_tick & 0xFF);
289	giacomo	944	err = com_send(servo_port, b);
		945	err = com_receive(servo_port);
358	giacomo	946	if (err != (int)(b)) timer_expired = 1;
282	giacomo	947
283	giacomo	948	if (timeout_event != NIL) kern_event_delete(timeout_event);
282	giacomo	949
283	giacomo	950	if (!timer_expired)
		951	return 0;
		952	else
		953	return -1;
281	giacomo	954
		955	}
		956
323	giacomo	957	/* 0010.Pppp */
358	giacomo	958	int servo_store_default_position(int port, int servo)
290	giacomo	959	{
381	giacomo	960	#ifdef SERVO_TIMEOUT_EVENT
		961	struct timespec current_time;
		962	#endif
290	giacomo	963	unsigned char b;
		964	int err;
358	giacomo	965	int servo_port = (unsigned)(port);
290	giacomo	966
323	giacomo	967	if (servo > 15) return -1;
358	giacomo	968
290	giacomo	969	timer_expired = 0;
381	giacomo	970
		971	#ifdef SERVO_TIMEOUT_EVENT
		972	kern_gettime(&current_time);
		973	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		974	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		975	#else
		976	timeout_event = NIL;
		977	#endif
		978
323	giacomo	979	b = 0x20 \| (servo & 0x0F);
290	giacomo	980	err = com_send(servo_port, b);
		981	err = com_receive(servo_port);
		982	if (err != (int)(b)) timer_expired = 1;
358	giacomo	983
290	giacomo	984	if (timeout_event != NIL) kern_event_delete(timeout_event);
358	giacomo	985
290	giacomo	986	if (!timer_expired)
		987	return 0;
		988	else
		989	return -1;
		990
		991	}
		992
323	giacomo	993	/* 0001.Pppp */
358	giacomo	994	int servo_get_angle_sec(int port, int servo)
281	giacomo	995	{
381	giacomo	996	#ifdef SERVO_TIMEOUT_EVENT
		997	struct timespec current_time;
		998	#endif
282	giacomo	999	unsigned char b;
290	giacomo	1000	int err,res,data;
358	giacomo	1001	int servo_port = (unsigned)(port);
		1002
323	giacomo	1003	if (servo > 15) return -1;
358	giacomo	1004
282	giacomo	1005	timer_expired = 0;
381	giacomo	1006
		1007	#ifdef SERVO_TIMEOUT_EVENT
		1008	kern_gettime(&current_time);
		1009	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		1010	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		1011	#else
		1012	timeout_event = NIL;
		1013	#endif
281	giacomo	1014
323	giacomo	1015	b = 0x10 \| (servo & 0x0F);
289	giacomo	1016	err = com_send(servo_port, b);
		1017	err = com_receive(servo_port);
283	giacomo	1018	if (err != (int)(b)) timer_expired = 1;
289	giacomo	1019	res = com_receive(servo_port) << 8;
		1020	res \|= com_receive(servo_port);
282	giacomo	1021
283	giacomo	1022	if (timeout_event != NIL) kern_event_delete(timeout_event);
282	giacomo	1023
381	giacomo	1024	data = ((resTICK_LEN/1000) - servo_table[port][servo].zero_tick)
		1025	(servo_table[port][servo].max_angle_sec - servo_table[port][servo].min_angle_sec) /
		1026	servo_table[port][servo].delta_tick;
290	giacomo	1027
282	giacomo	1028	if (!timer_expired)
290	giacomo	1029	return data;
282	giacomo	1030	else
		1031	return -1;
		1032
281	giacomo	1033	}
		1034
282	giacomo	1035	/* 0100:0aaa */
358	giacomo	1036	int servo_get_analog(int port, int adport)
281	giacomo	1037	{
		1038
381	giacomo	1039	#ifdef SERVO_TIMEOUT_EVENT
		1040	struct timespec current_time;
		1041	#endif
282	giacomo	1042	unsigned char b;
283	giacomo	1043	int err,res;
358	giacomo	1044	int servo_port = (unsigned)(port);
		1045
381	giacomo	1046	if (adport > 7) return -1;
358	giacomo	1047
282	giacomo	1048	timer_expired = 0;
381	giacomo	1049
		1050	#ifdef SERVO_TIMEOUT_EVENT
		1051	kern_gettime(&current_time);
		1052	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		1053	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		1054	#else
		1055	timeout_event = NIL;
		1056	#endif
281	giacomo	1057
358	giacomo	1058	b = 0x40 \| (adport & 0x07);
289	giacomo	1059	err = com_send(servo_port, b);
		1060	err = com_receive(servo_port);
283	giacomo	1061	if (err != (int)(b)) timer_expired = 1;
289	giacomo	1062	res = com_receive(servo_port) << 8;
		1063	res \|= com_receive(servo_port);
282	giacomo	1064
283	giacomo	1065	if (timeout_event != NIL) kern_event_delete(timeout_event);
282	giacomo	1066
		1067	if (!timer_expired)
		1068	return res;
		1069	else
		1070	return -1;
		1071
281	giacomo	1072	}
		1073

Subversion Repositories shark

(root)/shark/trunk/ports/servo/servo.c @ 726 - Rev 412