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

Subversion Repositories shark

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