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

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

Subversion Repositories shark

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