WebSVN - shark - Blame - Rev 286 - /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_PORT 2
		65	#define SERVO_SPEED 38400
		66	#define SERVO_PARITY NONE
		67	#define SERVO_LEN 8
		68	#define SERVO_STOP 1
282	giacomo	69
281	giacomo	70	int timer_expired = 0;
283	giacomo	71	int timeout_event;
281	giacomo	72
286	giacomo	73	const unsigned com_base[] = {0x03F8,0x02F8,0x03E8,0x02E8};
		74
		75	const int BaudTable[] = {
		76	1200,
		77	2400,
		78	4800,
		79	9600,
		80	14400,
		81	19200,
		82	38400,
		83	57600,
		84	115200,
		85	230400,
		86	-1};
		87
		88	/* 20MHz: SPBRG, BAUD BRGH=0, BAUD BRGH=1 */
		89	const int Baud20MhzData[] = {
		90	255, 1221, 4882,
		91	254, 1225, 4902,
		92	253, 1230, 4921,
		93	252, 1235, 4941,
		94	133, 2332, 9328,
		95	132, 2350, 9398,
		96	131, 2367, 9470,
		97	130, 2385, 9542,
		98	129, 2404, 9615,
		99	128, 2422, 9690,
		100	127, 2441, 9766,
		101	126, 2461, 9842,
		102	88, 0, 14045,
		103	87, 0, 14204,
		104	86, 0, 14368,
		105	85, 0, 14535,
		106	84, 0, 14706,
		107	66, 4664, 18657,
		108	65, 4735, 18939,
		109	64, 4808, 19231,
		110	63, 4883, 19531,
		111	32, 9470, 37879,
		112	31, 9766, 39062,
		113	21, 14205, 56818,
		114	15, 19531, 0,
		115	10, 0, 113636,
		116	7, 39062, 0,
		117	-1, -1, -1};
		118
282	giacomo	119	void set_timer_expired(void *arg)
281	giacomo	120	{
283	giacomo	121	timeout_event = NIL;
281	giacomo	122	timer_expired = 1;
		123	}
		124
		125	unsigned com_read(unsigned port,unsigned reg)
		126	{
		127	unsigned b;
		128	if (port > 3 \|\| reg > 7) return(0);
		129	b = ll_in(com_base[port]+reg);
		130	return(b);
		131	}
		132
		133	void com_write(unsigned port,unsigned reg,unsigned value)
		134	{
		135	if (port > 3 \|\| reg > 7) return;
		136	ll_out(com_base[port]+reg,value);
		137	}
		138
		139	int com_send(unsigned port,BYTE b)
		140	{
		141	while ((com_read(port,LSR) & 32) == 0 && !timer_expired)
		142	barrier();
		143	if (!timer_expired) {
		144	com_write(port,THR,b);
		145	return 0;
		146	} else {
		147	return -1;
		148	}
		149	}
		150
		151	int com_receive(unsigned port)
		152	{
		153	while ((com_read(port,LSR) & 1) == 0 && !timer_expired)
		154	barrier();
		155	if (!timer_expired) {
		156	return((int)(com_read(port,RBR)));
		157	} else {
		158	return -1;
		159	}
		160	}
		161
286	giacomo	162	int com_open(unsigned port,DWORD speed,BYTE parity,BYTE len,BYTE stop)
		163	{
		164	unsigned long div,b_mask;
		165
		166	/* Now set up the serial link */
		167	b_mask = (parity & 3) * 8 + (stop & 1) * 4 + ((len - 5) & 3);
		168	div = 115200L / speed;
		169	/* Clear serial interrupt enable register */
		170	com_write(port,IER,0);
		171	/* Empty input buffer */
		172	com_read(port,RBR);
		173	/* Activate DLAB bit for speed setting */
		174	com_write(port,LCR,0x80);
		175	/* Load baud divisor */
		176	com_write(port,0,div & 0x00FF);
		177	div >>= 8;
		178	com_write(port,1,div & 0x00FF);
		179	/* Load control word (parity,stop bit,bit len) */
		180	com_write(port,LCR,b_mask);
		181	/* Attiva OUT1 & OUT2 */
		182	com_write(port,MCR,0x0C);
		183
		184	return 0;
		185	}
		186
		187	int com_close(unsigned port)
		188	{
		189
		190	com_write(port,IER,0);
		191	com_read(port,IIR);
		192	com_read(port,RBR);
		193
		194	return 0;
		195
		196	}
		197
		198	int servo_open(void)
		199	{
		200	int err;
		201
		202	err = com_open(SERVO_PORT, SERVO_SPEED, SERVO_PARITY, SERVO_LEN, SERVO_STOP);
		203
		204	return err;
		205
		206	}
		207
		208	int servo_close(void)
		209	{
		210	int err;
		211
		212	err = com_close(SERVO_PORT);
		213
		214	return err;
		215
		216	}
		217
		218	/* 1000.011w:bbbb.bbbb */
281	giacomo	219	int servo_set_RS232_baudrate(int baud)
		220	{
285	giacomo	221	struct timespec current_time;
		222	unsigned char b;
286	giacomo	223	int err,diff,mindiff,i;
		224	unsigned char spbrg, w;
		225
		226	i = 0;
		227	while(BaudTable[i] != baud && BaudTable[i] != -1) i++;
		228	if (BaudTable[i] == -1) {
		229	kern_printf("SERVO:Error wrong baud rate\n");
		230	return -1;
		231	}
		232
		233	i = 0;
		234	mindiff = 0xFFFFFF;
		235	spbrg = 0;
		236	w = 0;
		237	while(Baud20MhzData[i] != -1) {
		238	diff = abs(Baud20MhzData[i+1] - baud);
		239	if (diff < mindiff) {
		240	mindiff = diff;
		241	spbrg = (unsigned char)(Baud20MhzData[i+1]);
		242	w = 0;
		243	}
		244	diff = abs(Baud20MhzData[i+2] - baud);
		245	if (diff < mindiff) {
		246	mindiff = diff;
		247	spbrg = (unsigned char)(Baud20MhzData[i+2]);
		248	w = 1;
		249	}
		250	i += 3;
		251	}
		252
		253	#ifdef SERVO_DEBUG
		254	kern_printf("(SERVO:SBPRG %d W %d)",spbrg,w);
		255	#endif
		256
285	giacomo	257	timer_expired = 0;
		258	kern_gettime(&current_time);
		259	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		260	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		261
286	giacomo	262	b = 0x86 \| (w & 0x01);
285	giacomo	263	err = com_send(SERVO_PORT, b);
		264	err = com_receive(SERVO_PORT);
		265	if (err != (int)(b)) timer_expired = 1;
281	giacomo	266
286	giacomo	267	b = spbrg;
285	giacomo	268	err = com_send(SERVO_PORT, b);
		269	err = com_receive(SERVO_PORT);
		270	if (err != (int)(b)) timer_expired = 1;
281	giacomo	271
285	giacomo	272	if (timeout_event != NIL) kern_event_delete(timeout_event);
286	giacomo	273
		274	com_close(SERVO_PORT);
		275	com_open(SERVO_PORT, baud, SERVO_PARITY, SERVO_LEN, SERVO_STOP);
		276
285	giacomo	277	if (!timer_expired)
		278	return 0;
		279	else
		280	return -1;
		281
281	giacomo	282	}
		283
285	giacomo	284	/* 1000.0101 */
281	giacomo	285	int servo_get_RS232_baudrate(void)
		286	{
285	giacomo	287	struct timespec current_time;
		288	unsigned char b;
		289	int err,res;
		290
		291	timer_expired = 0;
		292	kern_gettime(&current_time);
		293	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		294	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		295
		296	b = 0x85;
		297	err = com_send(SERVO_PORT, b);
		298	err = com_receive(SERVO_PORT);
		299	if (err != (int)(b)) timer_expired = 1;
		300	res = com_receive(SERVO_PORT);
		301
		302	if (timeout_event != NIL) kern_event_delete(timeout_event);
		303
		304	if (!timer_expired)
		305	return res;
		306	else
		307	return -1;
281	giacomo	308
		309	}
		310
286	giacomo	311	/* 1000.0100 */
281	giacomo	312	int servo_store_RS232_buadrate(void)
		313	{
285	giacomo	314	struct timespec current_time;
		315	unsigned char b;
		316	int err;
		317
		318	timer_expired = 0;
		319	kern_gettime(&current_time);
		320	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		321	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		322
286	giacomo	323	b = 0x84;
285	giacomo	324	err = com_send(SERVO_PORT, b);
		325	err = com_receive(SERVO_PORT);
		326	if (err != (int)(b)) timer_expired = 1;
		327
		328	if (timeout_event != NIL) kern_event_delete(timeout_event);
		329
		330	if (!timer_expired)
		331	return 0;
		332	else
		333	return -1;
281	giacomo	334
		335	}
		336
285	giacomo	337	/* 1000.1000:llll.llll */
281	giacomo	338	int servo_set_period(int period)
		339	{
285	giacomo	340	struct timespec current_time;
		341	unsigned char b;
		342	int err;
		343
		344	timer_expired = 0;
		345	kern_gettime(&current_time);
		346	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		347	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		348
		349	b = 0x88;
		350	err = com_send(SERVO_PORT, b);
		351	err = com_receive(SERVO_PORT);
		352	if (err != (int)(b)) timer_expired = 1;
		353
		354	b = period/8 & 0xFF;
		355	err = com_send(SERVO_PORT, b);
		356	err = com_receive(SERVO_PORT);
		357	if (err != (int)(b)) timer_expired = 1;
		358
		359	if (timeout_event != NIL) kern_event_delete(timeout_event);
		360
		361	if (!timer_expired)
		362	return 0;
		363	else
		364	return -1;
281	giacomo	365
		366	}
		367
285	giacomo	368	/* 1000.1001 */
281	giacomo	369	int servo_get_period(void)
		370	{
285	giacomo	371	struct timespec current_time;
		372	unsigned char b;
		373	int err,res;
		374
		375	timer_expired = 0;
		376	kern_gettime(&current_time);
		377	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		378	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		379
		380	b = 0x89;
		381	err = com_send(SERVO_PORT, b);
		382	err = com_receive(SERVO_PORT);
		383	if (err != (int)(b)) timer_expired = 1;
		384	res = com_receive(SERVO_PORT);
		385
		386	if (timeout_event != NIL) kern_event_delete(timeout_event);
		387
		388	if (!timer_expired)
		389	return res*8;
		390	else
		391	return -1;
281	giacomo	392
		393	}
		394
285	giacomo	395	/* 1000.1010 */
281	giacomo	396	int servo_store_period(void)
		397	{
285	giacomo	398	struct timespec current_time;
		399	unsigned char b;
		400	int err;
		401
		402	timer_expired = 0;
		403	kern_gettime(&current_time);
		404	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		405	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		406
		407	b = 0x8A;
		408	err = com_send(SERVO_PORT, b);
		409	err = com_receive(SERVO_PORT);
		410	if (err != (int)(b)) timer_expired = 1;
		411
		412	if (timeout_event != NIL) kern_event_delete(timeout_event);
		413
		414	if (!timer_expired)
		415	return 0;
		416	else
		417	return -1;
281	giacomo	418
		419	}
		420
283	giacomo	421	/* 1000.1100 */
281	giacomo	422	int servo_get_setup_switch(void)
		423	{
283	giacomo	424	struct timespec current_time;
		425	unsigned char b;
		426	int err,res;
		427	timer_expired = 0;
		428	kern_gettime(&current_time);
		429	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		430	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
281	giacomo	431
283	giacomo	432	b = 0x8C;
		433	err = com_send(SERVO_PORT, b);
		434	err = com_receive(SERVO_PORT);
		435	if (err != (int)(b)) timer_expired = 1;
		436	res = com_receive(SERVO_PORT);
281	giacomo	437
283	giacomo	438	if (timeout_event != NIL) kern_event_delete(timeout_event);
		439
		440	if (!timer_expired)
		441	return res;
		442	else
		443	return -1;
		444
281	giacomo	445	}
		446
283	giacomo	447	/* 1000.111s */
281	giacomo	448	int servo_set_RC5_switch(int data)
		449	{
283	giacomo	450	struct timespec current_time;
		451	unsigned char b;
		452	int err;
		453
		454	timer_expired = 0;
		455	kern_gettime(&current_time);
		456	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		457	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
281	giacomo	458
283	giacomo	459	b = 0x8E \| (data & 0x01);
		460	err = com_send(SERVO_PORT, b);
		461	err = com_receive(SERVO_PORT);
		462	if (err != (int)(b)) timer_expired = 1;
		463
		464	if (timeout_event != NIL) kern_event_delete(timeout_event);
		465
		466	if (!timer_expired)
		467	return 0;
		468	else
		469	return -1;
		470
281	giacomo	471	}
		472
285	giacomo	473	/* 1000.0000:0000.0mmm */
		474	int servo_turn_off(int servo)
		475	{
		476
		477	struct timespec current_time;
		478	unsigned char b;
		479	int err;
		480
		481	if (servo > 7) return -1;
		482
		483	timer_expired = 0;
		484	kern_gettime(&current_time);
		485	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		486	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		487
		488	b = 0x80;
		489	err = com_send(SERVO_PORT, b);
		490	err = com_receive(SERVO_PORT);
		491	if (err != (int)(b)) timer_expired = 1;
		492
		493	b = 0x00 \| (servo & 0x07);
		494	err = com_send(SERVO_PORT, b);
		495	err = com_receive(SERVO_PORT);
		496	if (err != (int)(b)) timer_expired = 1;
		497
		498	if (timeout_event != NIL) kern_event_delete(timeout_event);
		499
		500	if (!timer_expired)
		501	return 0;
		502	else
		503	return -1;
		504
		505	}
		506
		507	/* 1000.0000:1000.0mmm */
		508	int servo_turn_on(int servo)
		509	{
		510
		511	struct timespec current_time;
		512	unsigned char b;
		513	int err;
		514
		515	if (servo > 7) return -1;
		516
		517	timer_expired = 0;
		518	kern_gettime(&current_time);
		519	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		520	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		521
		522	b = 0x80;
		523	err = com_send(SERVO_PORT, b);
		524	err = com_receive(SERVO_PORT);
		525	if (err != (int)(b)) timer_expired = 1;
		526
		527	b = 0x08 \| (servo & 0x07);
		528	err = com_send(SERVO_PORT, b);
		529	err = com_receive(SERVO_PORT);
		530	if (err != (int)(b)) timer_expired = 1;
		531
		532	if (timeout_event != NIL) kern_event_delete(timeout_event);
		533
		534	if (!timer_expired)
		535	return 0;
		536	else
		537	return -1;
		538
		539	}
		540
		541	/* 1000.0000:0001.0000 */
		542	int servo_turn_off_all(void)
		543	{ struct timespec current_time;
		544	unsigned char b;
		545	int err;
		546
		547	timer_expired = 0;
		548	kern_gettime(&current_time);
		549	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		550	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		551
		552	b = 0x80;
		553	err = com_send(SERVO_PORT, b);
		554	err = com_receive(SERVO_PORT);
		555	if (err != (int)(b)) timer_expired = 1;
		556
		557	b = 0x10;
		558	err = com_send(SERVO_PORT, b);
		559	err = com_receive(SERVO_PORT);
		560	if (err != (int)(b)) timer_expired = 1;
		561
		562	if (timeout_event != NIL) kern_event_delete(timeout_event);
		563
		564	if (!timer_expired)
		565	return 0;
		566	else
		567	return -1;
		568
		569	}
		570
		571	/* 1000.0000:0001.0001 */
		572	int servo_turn_on_all(void)
		573	{
		574	struct timespec current_time;
		575	unsigned char b;
		576	int err;
		577
		578	timer_expired = 0;
		579	kern_gettime(&current_time);
		580	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		581	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		582
		583	b = 0x80;
		584	err = com_send(SERVO_PORT, b);
		585	err = com_receive(SERVO_PORT);
		586	if (err != (int)(b)) timer_expired = 1;
		587
		588	b = 0x11;
		589	err = com_send(SERVO_PORT, b);
		590	err = com_receive(SERVO_PORT);
		591	if (err != (int)(b)) timer_expired = 1;
		592
		593	if (timeout_event != NIL) kern_event_delete(timeout_event);
		594
		595	if (!timer_expired)
		596	return 0;
		597	else
		598	return -1;
		599
		600	}
		601
282	giacomo	602	/* 0000.0ppp:0000.vvvv:vvvv.vvvv */
281	giacomo	603	int servo_set_angle(int servo, int angle)
		604	{
282	giacomo	605	struct timespec current_time;
		606	unsigned char b;
283	giacomo	607	int err;
282	giacomo	608
		609	if (servo > 7) return -1;
		610
		611	timer_expired = 0;
		612	kern_gettime(&current_time);
		613	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		614	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		615
		616	b = 0x00 \| (servo & 0x07);
		617	err = com_send(SERVO_PORT, b);
		618	err = com_receive(SERVO_PORT);
283	giacomo	619	if (err != (int)(b)) timer_expired = 1;
282	giacomo	620
		621	b = 0x00 \| ((angle >> 8) & 0x0F);
		622	err = com_send(SERVO_PORT, b);
		623	err = com_receive(SERVO_PORT);
283	giacomo	624	if (err != (int)(b)) timer_expired = 1;
282	giacomo	625
		626	b = 0x00 \| (angle & 0xFF);
		627	err = com_send(SERVO_PORT, b);
		628	err = com_receive(SERVO_PORT);
283	giacomo	629	if (err != (int)(b)) timer_expired = 1;
282	giacomo	630
283	giacomo	631	if (timeout_event != NIL) kern_event_delete(timeout_event);
282	giacomo	632
283	giacomo	633	if (!timer_expired)
		634	return 0;
		635	else
		636	return -1;
281	giacomo	637
		638	}
		639
282	giacomo	640	/* 0000.1ppp */
281	giacomo	641	int servo_get_angle(int servo)
		642	{
282	giacomo	643	struct timespec current_time;
		644	unsigned char b;
283	giacomo	645	int err,res;
282	giacomo	646
		647	if (servo > 7) return -1;
		648
		649	timer_expired = 0;
		650	kern_gettime(&current_time);
		651	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		652	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
281	giacomo	653
282	giacomo	654	b = 0x08 \| (servo & 0x07);
		655	err = com_send(SERVO_PORT, b);
		656	err = com_receive(SERVO_PORT);
283	giacomo	657	if (err != (int)(b)) timer_expired = 1;
		658	res = com_receive(SERVO_PORT) << 8;
		659	res \|= com_receive(SERVO_PORT);
282	giacomo	660
283	giacomo	661	if (timeout_event != NIL) kern_event_delete(timeout_event);
282	giacomo	662
		663	if (!timer_expired)
		664	return res;
		665	else
		666	return -1;
		667
281	giacomo	668	}
		669
282	giacomo	670	/* 0100:0aaa */
281	giacomo	671	int servo_get_analog(int port)
		672	{
		673
282	giacomo	674	struct timespec current_time;
		675	unsigned char b;
283	giacomo	676	int err,res;
282	giacomo	677
		678	if (port > 4) return -1;
		679
		680	timer_expired = 0;
		681	kern_gettime(&current_time);
		682	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		683	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
281	giacomo	684
282	giacomo	685	b = 0x40 \| (port & 7);
		686	err = com_send(SERVO_PORT, b);
		687	err = com_receive(SERVO_PORT);
283	giacomo	688	if (err != (int)(b)) timer_expired = 1;
282	giacomo	689	res = com_receive(SERVO_PORT) << 8;
		690	res \|= com_receive(SERVO_PORT);
		691
283	giacomo	692	if (timeout_event != NIL) kern_event_delete(timeout_event);
282	giacomo	693
		694	if (!timer_expired)
		695	return res;
		696	else
		697	return -1;
		698
281	giacomo	699	}
		700

Subversion Repositories shark

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