WebSVN - shark - Blame - Rev 285 - /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
		53	#define barrier() __asm__ __volatile__("" ::: "memory");
		54
282	giacomo	55	#define SERVO_TIMEOUT 100000 /* us */
281	giacomo	56
282	giacomo	57	#define SERVO_PORT 1
		58
281	giacomo	59	int timer_expired = 0;
283	giacomo	60	int timeout_event;
281	giacomo	61	unsigned com_base[] = {0x03F8,0x02F8,0x03E8,0x02E8};
		62
282	giacomo	63	void set_timer_expired(void *arg)
281	giacomo	64	{
283	giacomo	65	timeout_event = NIL;
281	giacomo	66	timer_expired = 1;
		67	}
		68
		69	unsigned com_read(unsigned port,unsigned reg)
		70	{
		71	unsigned b;
		72	if (port > 3 \|\| reg > 7) return(0);
		73	b = ll_in(com_base[port]+reg);
		74	return(b);
		75	}
		76
		77	void com_write(unsigned port,unsigned reg,unsigned value)
		78	{
		79	if (port > 3 \|\| reg > 7) return;
		80	ll_out(com_base[port]+reg,value);
		81	}
		82
		83	int com_send(unsigned port,BYTE b)
		84	{
		85	while ((com_read(port,LSR) & 32) == 0 && !timer_expired)
		86	barrier();
		87	if (!timer_expired) {
		88	com_write(port,THR,b);
		89	return 0;
		90	} else {
		91	return -1;
		92	}
		93	}
		94
		95	int com_receive(unsigned port)
		96	{
		97	while ((com_read(port,LSR) & 1) == 0 && !timer_expired)
		98	barrier();
		99	if (!timer_expired) {
		100	return((int)(com_read(port,RBR)));
		101	} else {
		102	return -1;
		103	}
		104	}
		105
285	giacomo	106	/* 1000.0100:bbbb.bbbb */
281	giacomo	107	int servo_set_RS232_baudrate(int baud)
		108	{
285	giacomo	109	struct timespec current_time;
		110	unsigned char b;
		111	int err;
		112
		113	timer_expired = 0;
		114	kern_gettime(&current_time);
		115	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		116	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		117
		118	b = 0x84;
		119	err = com_send(SERVO_PORT, b);
		120	err = com_receive(SERVO_PORT);
		121	if (err != (int)(b)) timer_expired = 1;
281	giacomo	122
285	giacomo	123	b = baud & 0xFF;
		124	err = com_send(SERVO_PORT, b);
		125	err = com_receive(SERVO_PORT);
		126	if (err != (int)(b)) timer_expired = 1;
281	giacomo	127
285	giacomo	128	if (timeout_event != NIL) kern_event_delete(timeout_event);
		129
		130	if (!timer_expired)
		131	return 0;
		132	else
		133	return -1;
		134
281	giacomo	135	}
		136
285	giacomo	137	/* 1000.0101 */
281	giacomo	138	int servo_get_RS232_baudrate(void)
		139	{
285	giacomo	140	struct timespec current_time;
		141	unsigned char b;
		142	int err,res;
		143
		144	timer_expired = 0;
		145	kern_gettime(&current_time);
		146	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		147	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		148
		149	b = 0x85;
		150	err = com_send(SERVO_PORT, b);
		151	err = com_receive(SERVO_PORT);
		152	if (err != (int)(b)) timer_expired = 1;
		153	res = com_receive(SERVO_PORT);
		154
		155	if (timeout_event != NIL) kern_event_delete(timeout_event);
		156
		157	if (!timer_expired)
		158	return res;
		159	else
		160	return -1;
281	giacomo	161
		162	}
		163
285	giacomo	164	/* 1000.0110 */
281	giacomo	165	int servo_store_RS232_buadrate(void)
		166	{
285	giacomo	167	struct timespec current_time;
		168	unsigned char b;
		169	int err;
		170
		171	timer_expired = 0;
		172	kern_gettime(&current_time);
		173	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		174	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		175
		176	b = 0x86;
		177	err = com_send(SERVO_PORT, b);
		178	err = com_receive(SERVO_PORT);
		179	if (err != (int)(b)) timer_expired = 1;
		180
		181	if (timeout_event != NIL) kern_event_delete(timeout_event);
		182
		183	if (!timer_expired)
		184	return 0;
		185	else
		186	return -1;
281	giacomo	187
		188	}
		189
285	giacomo	190	/* 1000.1000:llll.llll */
281	giacomo	191	int servo_set_period(int period)
		192	{
285	giacomo	193	struct timespec current_time;
		194	unsigned char b;
		195	int err;
		196
		197	timer_expired = 0;
		198	kern_gettime(&current_time);
		199	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		200	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		201
		202	b = 0x88;
		203	err = com_send(SERVO_PORT, b);
		204	err = com_receive(SERVO_PORT);
		205	if (err != (int)(b)) timer_expired = 1;
		206
		207	b = period/8 & 0xFF;
		208	err = com_send(SERVO_PORT, b);
		209	err = com_receive(SERVO_PORT);
		210	if (err != (int)(b)) timer_expired = 1;
		211
		212	if (timeout_event != NIL) kern_event_delete(timeout_event);
		213
		214	if (!timer_expired)
		215	return 0;
		216	else
		217	return -1;
281	giacomo	218
		219	}
		220
285	giacomo	221	/* 1000.1001 */
281	giacomo	222	int servo_get_period(void)
		223	{
285	giacomo	224	struct timespec current_time;
		225	unsigned char b;
		226	int err,res;
		227
		228	timer_expired = 0;
		229	kern_gettime(&current_time);
		230	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		231	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		232
		233	b = 0x89;
		234	err = com_send(SERVO_PORT, b);
		235	err = com_receive(SERVO_PORT);
		236	if (err != (int)(b)) timer_expired = 1;
		237	res = com_receive(SERVO_PORT);
		238
		239	if (timeout_event != NIL) kern_event_delete(timeout_event);
		240
		241	if (!timer_expired)
		242	return res*8;
		243	else
		244	return -1;
281	giacomo	245
		246	}
		247
285	giacomo	248	/* 1000.1010 */
281	giacomo	249	int servo_store_period(void)
		250	{
285	giacomo	251	struct timespec current_time;
		252	unsigned char b;
		253	int err;
		254
		255	timer_expired = 0;
		256	kern_gettime(&current_time);
		257	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		258	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		259
		260	b = 0x8A;
		261	err = com_send(SERVO_PORT, b);
		262	err = com_receive(SERVO_PORT);
		263	if (err != (int)(b)) timer_expired = 1;
		264
		265	if (timeout_event != NIL) kern_event_delete(timeout_event);
		266
		267	if (!timer_expired)
		268	return 0;
		269	else
		270	return -1;
281	giacomo	271
		272	}
		273
283	giacomo	274	/* 1000.1100 */
281	giacomo	275	int servo_get_setup_switch(void)
		276	{
283	giacomo	277	struct timespec current_time;
		278	unsigned char b;
		279	int err,res;
		280	timer_expired = 0;
		281	kern_gettime(&current_time);
		282	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		283	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
281	giacomo	284
283	giacomo	285	b = 0x8C;
		286	err = com_send(SERVO_PORT, b);
		287	err = com_receive(SERVO_PORT);
		288	if (err != (int)(b)) timer_expired = 1;
		289	res = com_receive(SERVO_PORT);
281	giacomo	290
283	giacomo	291	if (timeout_event != NIL) kern_event_delete(timeout_event);
		292
		293	if (!timer_expired)
		294	return res;
		295	else
		296	return -1;
		297
281	giacomo	298	}
		299
283	giacomo	300	/* 1000.111s */
281	giacomo	301	int servo_set_RC5_switch(int data)
		302	{
283	giacomo	303	struct timespec current_time;
		304	unsigned char b;
		305	int err;
		306
		307	timer_expired = 0;
		308	kern_gettime(&current_time);
		309	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		310	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
281	giacomo	311
283	giacomo	312	b = 0x8E \| (data & 0x01);
		313	err = com_send(SERVO_PORT, b);
		314	err = com_receive(SERVO_PORT);
		315	if (err != (int)(b)) timer_expired = 1;
		316
		317	if (timeout_event != NIL) kern_event_delete(timeout_event);
		318
		319	if (!timer_expired)
		320	return 0;
		321	else
		322	return -1;
		323
281	giacomo	324	}
		325
285	giacomo	326	/* 1000.0000:0000.0mmm */
		327	int servo_turn_off(int servo)
		328	{
		329
		330	struct timespec current_time;
		331	unsigned char b;
		332	int err;
		333
		334	if (servo > 7) return -1;
		335
		336	timer_expired = 0;
		337	kern_gettime(&current_time);
		338	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		339	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		340
		341	b = 0x80;
		342	err = com_send(SERVO_PORT, b);
		343	err = com_receive(SERVO_PORT);
		344	if (err != (int)(b)) timer_expired = 1;
		345
		346	b = 0x00 \| (servo & 0x07);
		347	err = com_send(SERVO_PORT, b);
		348	err = com_receive(SERVO_PORT);
		349	if (err != (int)(b)) timer_expired = 1;
		350
		351	if (timeout_event != NIL) kern_event_delete(timeout_event);
		352
		353	if (!timer_expired)
		354	return 0;
		355	else
		356	return -1;
		357
		358	}
		359
		360	/* 1000.0000:1000.0mmm */
		361	int servo_turn_on(int servo)
		362	{
		363
		364	struct timespec current_time;
		365	unsigned char b;
		366	int err;
		367
		368	if (servo > 7) return -1;
		369
		370	timer_expired = 0;
		371	kern_gettime(&current_time);
		372	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		373	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		374
		375	b = 0x80;
		376	err = com_send(SERVO_PORT, b);
		377	err = com_receive(SERVO_PORT);
		378	if (err != (int)(b)) timer_expired = 1;
		379
		380	b = 0x08 \| (servo & 0x07);
		381	err = com_send(SERVO_PORT, b);
		382	err = com_receive(SERVO_PORT);
		383	if (err != (int)(b)) timer_expired = 1;
		384
		385	if (timeout_event != NIL) kern_event_delete(timeout_event);
		386
		387	if (!timer_expired)
		388	return 0;
		389	else
		390	return -1;
		391
		392	}
		393
		394	/* 1000.0000:0001.0000 */
		395	int servo_turn_off_all(void)
		396	{ struct timespec current_time;
		397	unsigned char b;
		398	int err;
		399
		400	timer_expired = 0;
		401	kern_gettime(&current_time);
		402	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		403	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		404
		405	b = 0x80;
		406	err = com_send(SERVO_PORT, b);
		407	err = com_receive(SERVO_PORT);
		408	if (err != (int)(b)) timer_expired = 1;
		409
		410	b = 0x10;
		411	err = com_send(SERVO_PORT, b);
		412	err = com_receive(SERVO_PORT);
		413	if (err != (int)(b)) timer_expired = 1;
		414
		415	if (timeout_event != NIL) kern_event_delete(timeout_event);
		416
		417	if (!timer_expired)
		418	return 0;
		419	else
		420	return -1;
		421
		422	}
		423
		424	/* 1000.0000:0001.0001 */
		425	int servo_turn_on_all(void)
		426	{
		427	struct timespec current_time;
		428	unsigned char b;
		429	int err;
		430
		431	timer_expired = 0;
		432	kern_gettime(&current_time);
		433	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		434	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		435
		436	b = 0x80;
		437	err = com_send(SERVO_PORT, b);
		438	err = com_receive(SERVO_PORT);
		439	if (err != (int)(b)) timer_expired = 1;
		440
		441	b = 0x11;
		442	err = com_send(SERVO_PORT, b);
		443	err = com_receive(SERVO_PORT);
		444	if (err != (int)(b)) timer_expired = 1;
		445
		446	if (timeout_event != NIL) kern_event_delete(timeout_event);
		447
		448	if (!timer_expired)
		449	return 0;
		450	else
		451	return -1;
		452
		453	}
		454
282	giacomo	455	/* 0000.0ppp:0000.vvvv:vvvv.vvvv */
281	giacomo	456	int servo_set_angle(int servo, int angle)
		457	{
282	giacomo	458	struct timespec current_time;
		459	unsigned char b;
283	giacomo	460	int err;
282	giacomo	461
		462	if (servo > 7) return -1;
		463
		464	timer_expired = 0;
		465	kern_gettime(&current_time);
		466	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		467	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		468
		469	b = 0x00 \| (servo & 0x07);
		470	err = com_send(SERVO_PORT, b);
		471	err = com_receive(SERVO_PORT);
283	giacomo	472	if (err != (int)(b)) timer_expired = 1;
282	giacomo	473
		474	b = 0x00 \| ((angle >> 8) & 0x0F);
		475	err = com_send(SERVO_PORT, b);
		476	err = com_receive(SERVO_PORT);
283	giacomo	477	if (err != (int)(b)) timer_expired = 1;
282	giacomo	478
		479	b = 0x00 \| (angle & 0xFF);
		480	err = com_send(SERVO_PORT, b);
		481	err = com_receive(SERVO_PORT);
283	giacomo	482	if (err != (int)(b)) timer_expired = 1;
282	giacomo	483
283	giacomo	484	if (timeout_event != NIL) kern_event_delete(timeout_event);
282	giacomo	485
283	giacomo	486	if (!timer_expired)
		487	return 0;
		488	else
		489	return -1;
281	giacomo	490
		491	}
		492
282	giacomo	493	/* 0000.1ppp */
281	giacomo	494	int servo_get_angle(int servo)
		495	{
282	giacomo	496	struct timespec current_time;
		497	unsigned char b;
283	giacomo	498	int err,res;
282	giacomo	499
		500	if (servo > 7) return -1;
		501
		502	timer_expired = 0;
		503	kern_gettime(&current_time);
		504	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		505	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
281	giacomo	506
282	giacomo	507	b = 0x08 \| (servo & 0x07);
		508	err = com_send(SERVO_PORT, b);
		509	err = com_receive(SERVO_PORT);
283	giacomo	510	if (err != (int)(b)) timer_expired = 1;
		511	res = com_receive(SERVO_PORT) << 8;
		512	res \|= com_receive(SERVO_PORT);
282	giacomo	513
283	giacomo	514	if (timeout_event != NIL) kern_event_delete(timeout_event);
282	giacomo	515
		516	if (!timer_expired)
		517	return res;
		518	else
		519	return -1;
		520
281	giacomo	521	}
		522
282	giacomo	523	/* 0100:0aaa */
281	giacomo	524	int servo_get_analog(int port)
		525	{
		526
282	giacomo	527	struct timespec current_time;
		528	unsigned char b;
283	giacomo	529	int err,res;
282	giacomo	530
		531	if (port > 4) return -1;
		532
		533	timer_expired = 0;
		534	kern_gettime(&current_time);
		535	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		536	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
281	giacomo	537
282	giacomo	538	b = 0x40 \| (port & 7);
		539	err = com_send(SERVO_PORT, b);
		540	err = com_receive(SERVO_PORT);
283	giacomo	541	if (err != (int)(b)) timer_expired = 1;
282	giacomo	542	res = com_receive(SERVO_PORT) << 8;
		543	res \|= com_receive(SERVO_PORT);
		544
283	giacomo	545	if (timeout_event != NIL) kern_event_delete(timeout_event);
282	giacomo	546
		547	if (!timer_expired)
		548	return res;
		549	else
		550	return -1;
		551
281	giacomo	552	}
		553

Subversion Repositories shark

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