WebSVN - shark - Blame - Rev 379 - /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
358	giacomo	60	//#define SERVO_DEBUG
286	giacomo	61
358	giacomo	62	#define SERVO_TIMEOUT 200000 /* 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
358	giacomo	69	#define SERVO_CLOCK 20000000 /* 20MHz */
		70
315	giacomo	71	#define TICK_LEN 1600 /* ns */
316	giacomo	72	#define TICK_LEN_PERIOD 51200 /* ns */
290	giacomo	73
		74	struct servo_data {
		75	int min_angle_sec;
		76	int max_angle_sec;
		77	int delta_tick;
		78	int zero_tick;
		79	};
		80
		81	struct servo_data servo_table[] = {
316	giacomo	82	{-324000, 324000, 1200, 1600},
		83	{-324000, 324000, 1200, 1600},
		84	{-324000, 324000, 1200, 1600},
		85	{-324000, 324000, 1200, 1600},
		86	{-324000, 324000, 1200, 1600},
		87	{-324000, 324000, 1200, 1600},
		88	{-324000, 324000, 1200, 1600},
335	giacomo	89	{-324000, 324000, 1200, 1600},
		90	{-324000, 324000, 1200, 1600},
		91	{-324000, 324000, 1200, 1600},
		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},
316	giacomo	97	{-324000, 324000, 1200, 1600}};
290	giacomo	98
281	giacomo	99	int timer_expired = 0;
283	giacomo	100	int timeout_event;
281	giacomo	101
286	giacomo	102	const unsigned com_base[] = {0x03F8,0x02F8,0x03E8,0x02E8};
		103
		104	const int BaudTable[] = {
		105	1200,
		106	2400,
		107	4800,
		108	9600,
		109	14400,
		110	19200,
		111	38400,
		112	57600,
		113	115200,
		114	-1};
		115
282	giacomo	116	void set_timer_expired(void *arg)
281	giacomo	117	{
283	giacomo	118	timeout_event = NIL;
281	giacomo	119	timer_expired = 1;
		120	}
		121
		122	unsigned com_read(unsigned port,unsigned reg)
		123	{
		124	unsigned b;
		125	if (port > 3 \|\| reg > 7) return(0);
		126	b = ll_in(com_base[port]+reg);
		127	return(b);
		128	}
		129
		130	void com_write(unsigned port,unsigned reg,unsigned value)
		131	{
		132	if (port > 3 \|\| reg > 7) return;
		133	ll_out(com_base[port]+reg,value);
		134	}
		135
		136	int com_send(unsigned port,BYTE b)
		137	{
		138	while ((com_read(port,LSR) & 32) == 0 && !timer_expired)
		139	barrier();
		140	if (!timer_expired) {
292	giacomo	141	#ifdef SERVO_DEBUG
295	giacomo	142	kern_printf("(SERVO WRITE p = %d b = %02x)",port,b);
292	giacomo	143	#endif
281	giacomo	144	com_write(port,THR,b);
		145	return 0;
		146	} else {
292	giacomo	147	#ifdef SERVO_DEBUG
295	giacomo	148	kern_printf("(WRITE TIMEOUT)");
292	giacomo	149	#endif
281	giacomo	150	return -1;
		151	}
		152	}
		153
		154	int com_receive(unsigned port)
		155	{
292	giacomo	156	int b;
		157
281	giacomo	158	while ((com_read(port,LSR) & 1) == 0 && !timer_expired)
		159	barrier();
		160	if (!timer_expired) {
292	giacomo	161	b = (int)(com_read(port,RBR));
		162	#ifdef SERVO_DEBUG
295	giacomo	163	kern_printf("(SERVO READ p = %d b = %02x)",port,b);
292	giacomo	164	#endif
		165	return b;
281	giacomo	166	} else {
292	giacomo	167	#ifdef SERVO_DEBUG
295	giacomo	168	kern_printf("(READ TIMEOUT)");
292	giacomo	169	#endif
281	giacomo	170	return -1;
		171	}
		172	}
		173
286	giacomo	174	int com_open(unsigned port,DWORD speed,BYTE parity,BYTE len,BYTE stop)
		175	{
		176	unsigned long div,b_mask;
		177
		178	/* Now set up the serial link */
		179	b_mask = (parity & 3) * 8 + (stop & 1) * 4 + ((len - 5) & 3);
		180	div = 115200L / speed;
		181	/* Clear serial interrupt enable register */
		182	com_write(port,IER,0);
		183	/* Empty input buffer */
		184	com_read(port,RBR);
		185	/* Activate DLAB bit for speed setting */
		186	com_write(port,LCR,0x80);
		187	/* Load baud divisor */
		188	com_write(port,0,div & 0x00FF);
		189	div >>= 8;
		190	com_write(port,1,div & 0x00FF);
		191	/* Load control word (parity,stop bit,bit len) */
		192	com_write(port,LCR,b_mask);
		193	/* Attiva OUT1 & OUT2 */
		194	com_write(port,MCR,0x0C);
		195
		196	return 0;
295	giacomo	197
286	giacomo	198	}
		199
		200	int com_close(unsigned port)
		201	{
		202
		203	com_write(port,IER,0);
		204	com_read(port,IIR);
		205	com_read(port,RBR);
		206
		207	return 0;
		208
		209	}
		210
358	giacomo	211	int servo_open(int port,int speed)
286	giacomo	212	{
		213	int err;
		214
358	giacomo	215	err = com_open((unsigned)(port), (DWORD)speed, SERVO_PARITY, SERVO_LEN, SERVO_STOP);
		216
286	giacomo	217	return err;
		218
		219	}
		220
358	giacomo	221	int servo_close(int port)
286	giacomo	222	{
		223	int err;
		224
358	giacomo	225	err = com_close((unsigned)(port));
286	giacomo	226
		227	return err;
		228
		229	}
		230
		231	/* 1000.011w:bbbb.bbbb */
358	giacomo	232	int servo_set_RS232_baudrate(int port, int baud)
281	giacomo	233	{
285	giacomo	234	struct timespec current_time;
		235	unsigned char b;
358	giacomo	236	int err, spbrg_temp, i;
286	giacomo	237	unsigned char spbrg, w;
358	giacomo	238	int servo_port = (unsigned)(port);
286	giacomo	239
		240	i = 0;
		241	while(BaudTable[i] != baud && BaudTable[i] != -1) i++;
		242	if (BaudTable[i] == -1) {
		243	kern_printf("SERVO:Error wrong baud rate\n");
		244	return -1;
		245	}
		246
358	giacomo	247	w = 1;
		248	spbrg_temp = (SERVO_CLOCK / (16*baud)) - 1;
		249	if (spbrg_temp>255) {
		250	w = 0;
		251	spbrg = (SERVO_CLOCK / (64*baud)) - 1;
		252	} else {
		253	spbrg = spbrg_temp;
286	giacomo	254	}
358	giacomo	255
286	giacomo	256	#ifdef SERVO_DEBUG
		257	kern_printf("(SERVO:SBPRG %d W %d)",spbrg,w);
358	giacomo	258	#endif
		259
285	giacomo	260	timer_expired = 0;
		261	kern_gettime(&current_time);
		262	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		263	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
358	giacomo	264
286	giacomo	265	b = 0x86 \| (w & 0x01);
289	giacomo	266	err = com_send(servo_port, b);
		267	err = com_receive(servo_port);
285	giacomo	268	if (err != (int)(b)) timer_expired = 1;
281	giacomo	269
286	giacomo	270	b = spbrg;
289	giacomo	271	err = com_send(servo_port, b);
		272	err = com_receive(servo_port);
285	giacomo	273	if (err != (int)(b)) timer_expired = 1;
281	giacomo	274
285	giacomo	275	if (timeout_event != NIL) kern_event_delete(timeout_event);
358	giacomo	276
		277	/*com_close(servo_port);
		278	com_open(servo_port, baud, SERVO_PARITY, SERVO_LEN, SERVO_STOP);*/
		279
285	giacomo	280	if (!timer_expired)
		281	return 0;
		282	else
		283	return -1;
		284
281	giacomo	285	}
		286
285	giacomo	287	/* 1000.0101 */
358	giacomo	288	int servo_get_RS232_baudrate(int port)
281	giacomo	289	{
285	giacomo	290	struct timespec current_time;
		291	unsigned char b;
358	giacomo	292	int err, res, res_w, res_b;
		293	int servo_port = (unsigned)(port);
		294
285	giacomo	295	timer_expired = 0;
		296	kern_gettime(&current_time);
		297	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		298	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
358	giacomo	299
285	giacomo	300	b = 0x85;
289	giacomo	301	err = com_send(servo_port, b);
		302	err = com_receive(servo_port);
285	giacomo	303	if (err != (int)(b)) timer_expired = 1;
358	giacomo	304	res_w = com_receive(servo_port); /* bit W */
		305	res_b = com_receive(servo_port); /* byte SPBRG */
		306	if (res_w)
		307	res = SERVO_CLOCK / ( 16 * (res_b + 1) );
		308	else
		309	res = SERVO_CLOCK / ( 64 * (res_b + 1) );
		310
285	giacomo	311	if (timeout_event != NIL) kern_event_delete(timeout_event);
358	giacomo	312
285	giacomo	313	if (!timer_expired)
		314	return res;
		315	else
		316	return -1;
281	giacomo	317
		318	}
		319
286	giacomo	320	/* 1000.0100 */
358	giacomo	321	int servo_store_RS232_baudrate(int port)
281	giacomo	322	{
285	giacomo	323	struct timespec current_time;
		324	unsigned char b;
		325	int err;
358	giacomo	326	int servo_port = (unsigned)(port);
		327
285	giacomo	328	timer_expired = 0;
		329	kern_gettime(&current_time);
		330	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		331	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
358	giacomo	332
286	giacomo	333	b = 0x84;
289	giacomo	334	err = com_send(servo_port, b);
		335	err = com_receive(servo_port);
285	giacomo	336	if (err != (int)(b)) timer_expired = 1;
358	giacomo	337
285	giacomo	338	if (timeout_event != NIL) kern_event_delete(timeout_event);
358	giacomo	339
285	giacomo	340	if (!timer_expired)
		341	return 0;
		342	else
358	giacomo	343	return -1;
281	giacomo	344
		345	}
		346
289	giacomo	347	/* 1000.1010:llll.llll */
358	giacomo	348	int servo_set_period(int port, int period)
281	giacomo	349	{
285	giacomo	350	struct timespec current_time;
		351	unsigned char b;
		352	int err;
358	giacomo	353	int servo_port = (unsigned)(port);
		354
285	giacomo	355	timer_expired = 0;
		356	kern_gettime(&current_time);
		357	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		358	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
358	giacomo	359
289	giacomo	360	b = 0x8A;
		361	err = com_send(servo_port, b);
		362	err = com_receive(servo_port);
285	giacomo	363	if (err != (int)(b)) timer_expired = 1;
358	giacomo	364
344	giacomo	365	b = (period*1000)/TICK_LEN_PERIOD/8 & 0xFF;
289	giacomo	366	err = com_send(servo_port, b);
		367	err = com_receive(servo_port);
285	giacomo	368	if (err != (int)(b)) timer_expired = 1;
358	giacomo	369
285	giacomo	370	if (timeout_event != NIL) kern_event_delete(timeout_event);
358	giacomo	371
285	giacomo	372	if (!timer_expired)
		373	return 0;
		374	else
		375	return -1;
281	giacomo	376
		377	}
		378
285	giacomo	379	/* 1000.1001 */
358	giacomo	380	int servo_get_period(int port)
281	giacomo	381	{
285	giacomo	382	struct timespec current_time;
		383	unsigned char b;
		384	int err,res;
358	giacomo	385	int servo_port = (unsigned)(port);
		386
285	giacomo	387	timer_expired = 0;
		388	kern_gettime(&current_time);
		389	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		390	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
358	giacomo	391
285	giacomo	392	b = 0x89;
289	giacomo	393	err = com_send(servo_port, b);
		394	err = com_receive(servo_port);
285	giacomo	395	if (err != (int)(b)) timer_expired = 1;
358	giacomo	396	res = com_receive(servo_port);
		397
285	giacomo	398	if (timeout_event != NIL) kern_event_delete(timeout_event);
358	giacomo	399
285	giacomo	400	if (!timer_expired)
345	giacomo	401	return (((unsigned char)(res))TICK_LEN_PERIOD/10008);
285	giacomo	402	else
		403	return -1;
281	giacomo	404
		405	}
		406
289	giacomo	407	/* 1000.1000 */
358	giacomo	408	int servo_store_period(int port)
281	giacomo	409	{
285	giacomo	410	struct timespec current_time;
		411	unsigned char b;
		412	int err;
358	giacomo	413	int servo_port = (unsigned)(port);
		414
285	giacomo	415	timer_expired = 0;
		416	kern_gettime(&current_time);
		417	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		418	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
358	giacomo	419
289	giacomo	420	b = 0x88;
		421	err = com_send(servo_port, b);
		422	err = com_receive(servo_port);
285	giacomo	423	if (err != (int)(b)) timer_expired = 1;
358	giacomo	424
285	giacomo	425	if (timeout_event != NIL) kern_event_delete(timeout_event);
358	giacomo	426
285	giacomo	427	if (!timer_expired)
		428	return 0;
		429	else
358	giacomo	430	return -1;
281	giacomo	431
		432	}
		433
283	giacomo	434	/* 1000.1100 */
358	giacomo	435	int servo_get_setup_switch(int port)
281	giacomo	436	{
283	giacomo	437	struct timespec current_time;
		438	unsigned char b;
358	giacomo	439	int err,res;
		440	int servo_port = (unsigned)(port);
		441
283	giacomo	442	timer_expired = 0;
		443	kern_gettime(&current_time);
		444	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
358	giacomo	445	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
281	giacomo	446
283	giacomo	447	b = 0x8C;
289	giacomo	448	err = com_send(servo_port, b);
		449	err = com_receive(servo_port);
283	giacomo	450	if (err != (int)(b)) timer_expired = 1;
289	giacomo	451	res = com_receive(servo_port);
281	giacomo	452
283	giacomo	453	if (timeout_event != NIL) kern_event_delete(timeout_event);
		454
		455	if (!timer_expired)
		456	return res;
		457	else
		458	return -1;
		459
281	giacomo	460	}
		461
283	giacomo	462	/* 1000.111s */
358	giacomo	463	int servo_set_RC5_switch(int port, int data)
281	giacomo	464	{
283	giacomo	465	struct timespec current_time;
		466	unsigned char b;
		467	int err;
358	giacomo	468	int servo_port = (unsigned)(port);
		469
283	giacomo	470	timer_expired = 0;
		471	kern_gettime(&current_time);
		472	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		473	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
281	giacomo	474
283	giacomo	475	b = 0x8E \| (data & 0x01);
289	giacomo	476	err = com_send(servo_port, b);
		477	err = com_receive(servo_port);
283	giacomo	478	if (err != (int)(b)) timer_expired = 1;
		479
358	giacomo	480	if (timeout_event != NIL) kern_event_delete(timeout_event);
		481
283	giacomo	482	if (!timer_expired)
		483	return 0;
		484	else
		485	return -1;
		486
281	giacomo	487	}
		488
323	giacomo	489	/* 1000.0000:0000.Mmmm */
358	giacomo	490	int servo_turn_off(int port, int servo)
285	giacomo	491	{
		492
		493	struct timespec current_time;
		494	unsigned char b;
358	giacomo	495	int err;
		496	int servo_port = (unsigned)(port);
		497
		498	if (servo > 15) return -1;
285	giacomo	499	timer_expired = 0;
		500	kern_gettime(&current_time);
		501	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		502	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		503
		504	b = 0x80;
289	giacomo	505	err = com_send(servo_port, b);
		506	err = com_receive(servo_port);
285	giacomo	507	if (err != (int)(b)) timer_expired = 1;
		508
323	giacomo	509	b = 0x00 \| (servo & 0x0F);
289	giacomo	510	err = com_send(servo_port, b);
		511	err = com_receive(servo_port);
285	giacomo	512	if (err != (int)(b)) timer_expired = 1;
		513
		514	if (timeout_event != NIL) kern_event_delete(timeout_event);
358	giacomo	515
285	giacomo	516	if (!timer_expired)
		517	return 0;
		518	else
		519	return -1;
		520
		521	}
		522
325	giacomo	523	/* 1000.0000:0001.Mmmm */
358	giacomo	524	int servo_turn_on(int port, int servo)
285	giacomo	525	{
358	giacomo	526
285	giacomo	527	struct timespec current_time;
		528	unsigned char b;
		529	int err;
358	giacomo	530	int servo_port = (unsigned)(port);
		531
323	giacomo	532	if (servo > 15) return -1;
358	giacomo	533
285	giacomo	534	timer_expired = 0;
		535	kern_gettime(&current_time);
		536	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		537	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
358	giacomo	538
285	giacomo	539	b = 0x80;
289	giacomo	540	err = com_send(servo_port, b);
		541	err = com_receive(servo_port);
285	giacomo	542	if (err != (int)(b)) timer_expired = 1;
358	giacomo	543
323	giacomo	544	b = 0x10 \| (servo & 0x0F);
289	giacomo	545	err = com_send(servo_port, b);
		546	err = com_receive(servo_port);
285	giacomo	547	if (err != (int)(b)) timer_expired = 1;
358	giacomo	548
285	giacomo	549	if (timeout_event != NIL) kern_event_delete(timeout_event);
358	giacomo	550
285	giacomo	551	if (!timer_expired)
		552	return 0;
		553	else
		554	return -1;
358	giacomo	555
285	giacomo	556	}
		557
291	giacomo	558	/* 1000.0000:0010.0000 */
358	giacomo	559	int servo_turn_off_all(int port)
323	giacomo	560	{
		561	struct timespec current_time;
285	giacomo	562	unsigned char b;
		563	int err;
358	giacomo	564	int servo_port = (unsigned)(port);
		565
285	giacomo	566	timer_expired = 0;
		567	kern_gettime(&current_time);
		568	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		569	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
358	giacomo	570
285	giacomo	571	b = 0x80;
289	giacomo	572	err = com_send(servo_port, b);
		573	err = com_receive(servo_port);
285	giacomo	574	if (err != (int)(b)) timer_expired = 1;
358	giacomo	575
291	giacomo	576	b = 0x20;
289	giacomo	577	err = com_send(servo_port, b);
		578	err = com_receive(servo_port);
285	giacomo	579	if (err != (int)(b)) timer_expired = 1;
358	giacomo	580
285	giacomo	581	if (timeout_event != NIL) kern_event_delete(timeout_event);
358	giacomo	582
285	giacomo	583	if (!timer_expired)
		584	return 0;
		585	else
		586	return -1;
358	giacomo	587
285	giacomo	588	}
		589
291	giacomo	590	/* 1000.0000:0010.0001 */
358	giacomo	591	int servo_turn_on_all(int port)
285	giacomo	592	{
		593	struct timespec current_time;
		594	unsigned char b;
		595	int err;
358	giacomo	596	int servo_port = (unsigned)(port);
		597
285	giacomo	598	timer_expired = 0;
		599	kern_gettime(&current_time);
		600	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		601	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
358	giacomo	602
285	giacomo	603	b = 0x80;
289	giacomo	604	err = com_send(servo_port, b);
		605	err = com_receive(servo_port);
285	giacomo	606	if (err != (int)(b)) timer_expired = 1;
358	giacomo	607
291	giacomo	608	b = 0x21;
289	giacomo	609	err = com_send(servo_port, b);
		610	err = com_receive(servo_port);
285	giacomo	611	if (err != (int)(b)) timer_expired = 1;
358	giacomo	612
285	giacomo	613	if (timeout_event != NIL) kern_event_delete(timeout_event);
358	giacomo	614
285	giacomo	615	if (!timer_expired)
		616	return 0;
		617	else
		618	return -1;
358	giacomo	619
285	giacomo	620	}
		621
323	giacomo	622	/* 1000.0000:0101.000M:mmmm.mmmm */
358	giacomo	623	int servo_set_levels(int port, int bank,int mask)
315	giacomo	624	{
		625	struct timespec current_time;
		626	unsigned char b;
		627	int err;
358	giacomo	628	int servo_port = (unsigned)(port);
		629
315	giacomo	630	timer_expired = 0;
		631	kern_gettime(&current_time);
		632	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		633	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
358	giacomo	634
315	giacomo	635	b = 0x80;
		636	err = com_send(servo_port, b);
		637	err = com_receive(servo_port);
		638	if (err != (int)(b)) timer_expired = 1;
358	giacomo	639
323	giacomo	640	b = 0x50 \| (0x01 & bank);
315	giacomo	641	err = com_send(servo_port, b);
		642	err = com_receive(servo_port);
		643	if (err != (int)(b)) timer_expired = 1;
358	giacomo	644
315	giacomo	645	b = (unsigned char)(mask & 0xFF);
		646	err = com_send(servo_port, b);
		647	err = com_receive(servo_port);
		648	if (err != (int)(b)) timer_expired = 1;
358	giacomo	649
315	giacomo	650	if (timeout_event != NIL) kern_event_delete(timeout_event);
358	giacomo	651
315	giacomo	652	if (!timer_expired)
		653	return 0;
		654	else
		655	return -1;
358	giacomo	656
315	giacomo	657	}
		658
323	giacomo	659	/* 1000.0000:0100.000M */
358	giacomo	660	int servo_get_levels(int port, int bank)
297	giacomo	661	{
		662	struct timespec current_time;
		663	unsigned char b;
		664	int err;
358	giacomo	665	int servo_port = (unsigned)(port);
		666
297	giacomo	667	timer_expired = 0;
		668	kern_gettime(&current_time);
		669	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		670	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
358	giacomo	671
297	giacomo	672	b = 0x80;
		673	err = com_send(servo_port, b);
		674	err = com_receive(servo_port);
		675	if (err != (int)(b)) timer_expired = 1;
358	giacomo	676
323	giacomo	677	b = 0x40 \| (0x01 & bank);
297	giacomo	678	err = com_send(servo_port, b);
		679	err = com_receive(servo_port);
		680	if (err != (int)(b)) timer_expired = 1;
358	giacomo	681	err = com_receive(servo_port);
		682
297	giacomo	683	if (timeout_event != NIL) kern_event_delete(timeout_event);
358	giacomo	684
297	giacomo	685	if (!timer_expired)
316	giacomo	686	return err;
297	giacomo	687	else
		688	return -1;
358	giacomo	689
297	giacomo	690	}
		691
		692	/* 1000.0000:1000.0000 */
358	giacomo	693	int servo_store_levels(int port)
297	giacomo	694	{
		695	struct timespec current_time;
		696	unsigned char b;
		697	int err;
358	giacomo	698	int servo_port = (unsigned)(port);
		699
297	giacomo	700	timer_expired = 0;
		701	kern_gettime(&current_time);
		702	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		703	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
358	giacomo	704
297	giacomo	705	b = 0x80;
		706	err = com_send(servo_port, b);
		707	err = com_receive(servo_port);
		708	if (err != (int)(b)) timer_expired = 1;
358	giacomo	709
297	giacomo	710	b = 0x80;
		711	err = com_send(servo_port, b);
		712	err = com_receive(servo_port);
		713	if (err != (int)(b)) timer_expired = 1;
358	giacomo	714
297	giacomo	715	if (timeout_event != NIL) kern_event_delete(timeout_event);
358	giacomo	716
297	giacomo	717	if (!timer_expired)
		718	return 0;
		719	else
		720	return -1;
358	giacomo	721
297	giacomo	722	}
		723
358	giacomo	724	int servo_set_max_angle(int port, int servo, int angle_sec)
290	giacomo	725	{
		726
		727	servo_table[servo].max_angle_sec = angle_sec;
		728	return 0;
		729
		730	}
		731
		732	int servo_set_min_angle(int servo, int angle_sec)
		733	{
		734
		735	servo_table[servo].min_angle_sec = angle_sec;
		736	return 0;
		737
		738	}
		739
323	giacomo	740	/* 0000.Pppp:0000.vvvv:vvvv.vvvv */
358	giacomo	741	int servo_set_angle_sec(int port, int servo, int angle_sec)
281	giacomo	742	{
282	giacomo	743	struct timespec current_time;
		744	unsigned char b;
290	giacomo	745	int err, angle_tick;
358	giacomo	746	int servo_port = (unsigned)(port);
282	giacomo	747
323	giacomo	748	if (servo > 15) return -1;
282	giacomo	749
358	giacomo	750	if (angle_sec > servo_table[servo].max_angle_sec \|\|
290	giacomo	751	angle_sec < servo_table[servo].min_angle_sec) return -1;
		752
358	giacomo	753	angle_tick = (servo_table[servo].zero_tick + angle_sec * servo_table[servo].delta_tick /
316	giacomo	754	(servo_table[servo].max_angle_sec - servo_table[servo].min_angle_sec)) * 1000 / TICK_LEN;
290	giacomo	755
282	giacomo	756	timer_expired = 0;
		757	kern_gettime(&current_time);
		758	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		759	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
358	giacomo	760
323	giacomo	761	b = 0x00 \| (servo & 0x0F);
289	giacomo	762	err = com_send(servo_port, b);
		763	err = com_receive(servo_port);
283	giacomo	764	if (err != (int)(b)) timer_expired = 1;
282	giacomo	765
290	giacomo	766	b = 0x00 \| ((angle_tick >> 8) & 0x0F);
289	giacomo	767	err = com_send(servo_port, b);
		768	err = com_receive(servo_port);
283	giacomo	769	if (err != (int)(b)) timer_expired = 1;
282	giacomo	770
290	giacomo	771	b = 0x00 \| (angle_tick & 0xFF);
289	giacomo	772	err = com_send(servo_port, b);
		773	err = com_receive(servo_port);
358	giacomo	774	if (err != (int)(b)) timer_expired = 1;
282	giacomo	775
283	giacomo	776	if (timeout_event != NIL) kern_event_delete(timeout_event);
282	giacomo	777
283	giacomo	778	if (!timer_expired)
		779	return 0;
		780	else
		781	return -1;
281	giacomo	782
		783	}
		784
323	giacomo	785	/* 0010.Pppp */
358	giacomo	786	int servo_store_default_position(int port, int servo)
290	giacomo	787	{
		788	struct timespec current_time;
		789	unsigned char b;
		790	int err;
358	giacomo	791	int servo_port = (unsigned)(port);
290	giacomo	792
323	giacomo	793	if (servo > 15) return -1;
358	giacomo	794
290	giacomo	795	timer_expired = 0;
		796	kern_gettime(&current_time);
		797	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		798	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
358	giacomo	799
323	giacomo	800	b = 0x20 \| (servo & 0x0F);
290	giacomo	801	err = com_send(servo_port, b);
		802	err = com_receive(servo_port);
		803	if (err != (int)(b)) timer_expired = 1;
358	giacomo	804
290	giacomo	805	if (timeout_event != NIL) kern_event_delete(timeout_event);
358	giacomo	806
290	giacomo	807	if (!timer_expired)
		808	return 0;
		809	else
		810	return -1;
		811
		812	}
		813
323	giacomo	814	/* 0001.Pppp */
358	giacomo	815	int servo_get_angle_sec(int port, int servo)
281	giacomo	816	{
282	giacomo	817	struct timespec current_time;
		818	unsigned char b;
290	giacomo	819	int err,res,data;
358	giacomo	820	int servo_port = (unsigned)(port);
		821
323	giacomo	822	if (servo > 15) return -1;
358	giacomo	823
282	giacomo	824	timer_expired = 0;
		825	kern_gettime(&current_time);
		826	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		827	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
281	giacomo	828
323	giacomo	829	b = 0x10 \| (servo & 0x0F);
289	giacomo	830	err = com_send(servo_port, b);
		831	err = com_receive(servo_port);
283	giacomo	832	if (err != (int)(b)) timer_expired = 1;
289	giacomo	833	res = com_receive(servo_port) << 8;
		834	res \|= com_receive(servo_port);
282	giacomo	835
283	giacomo	836	if (timeout_event != NIL) kern_event_delete(timeout_event);
282	giacomo	837
358	giacomo	838	data = ((resTICK_LEN/1000) - servo_table[servo].zero_tick)
290	giacomo	839	(servo_table[servo].max_angle_sec - servo_table[servo].min_angle_sec) /
		840	servo_table[servo].delta_tick;
		841
282	giacomo	842	if (!timer_expired)
290	giacomo	843	return data;
282	giacomo	844	else
		845	return -1;
		846
281	giacomo	847	}
		848
282	giacomo	849	/* 0100:0aaa */
358	giacomo	850	int servo_get_analog(int port, int adport)
281	giacomo	851	{
		852
282	giacomo	853	struct timespec current_time;
		854	unsigned char b;
283	giacomo	855	int err,res;
358	giacomo	856	int servo_port = (unsigned)(port);
		857
323	giacomo	858	if (port > 7) return -1;
358	giacomo	859
282	giacomo	860	timer_expired = 0;
		861	kern_gettime(&current_time);
		862	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		863	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
281	giacomo	864
358	giacomo	865	b = 0x40 \| (adport & 0x07);
289	giacomo	866	err = com_send(servo_port, b);
		867	err = com_receive(servo_port);
283	giacomo	868	if (err != (int)(b)) timer_expired = 1;
289	giacomo	869	res = com_receive(servo_port) << 8;
		870	res \|= com_receive(servo_port);
282	giacomo	871
283	giacomo	872	if (timeout_event != NIL) kern_event_delete(timeout_event);
282	giacomo	873
		874	if (!timer_expired)
		875	return res;
		876	else
		877	return -1;
		878
281	giacomo	879	}
		880

Subversion Repositories shark

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