WebSVN - shark - Blame - Rev 323 - /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
323	giacomo	509	/* 1000.0000:0000.Mmmm */
285	giacomo	510	int servo_turn_off(int servo)
		511	{
		512
		513	struct timespec current_time;
		514	unsigned char b;
323	giacomo	515	int err;
		516	if (servo > 15) return -1;
285	giacomo	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;
289	giacomo	523	err = com_send(servo_port, b);
		524	err = com_receive(servo_port);
285	giacomo	525	if (err != (int)(b)) timer_expired = 1;
		526
323	giacomo	527	b = 0x00 \| (servo & 0x0F);
289	giacomo	528	err = com_send(servo_port, b);
		529	err = com_receive(servo_port);
285	giacomo	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
291	giacomo	541	/* 1000.0000:0001.0mmm */
285	giacomo	542	int servo_turn_on(int servo)
		543	{
		544
		545	struct timespec current_time;
		546	unsigned char b;
		547	int err;
		548
323	giacomo	549	if (servo > 15) return -1;
285	giacomo	550
		551	timer_expired = 0;
		552	kern_gettime(&current_time);
		553	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		554	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		555
		556	b = 0x80;
289	giacomo	557	err = com_send(servo_port, b);
		558	err = com_receive(servo_port);
285	giacomo	559	if (err != (int)(b)) timer_expired = 1;
		560
323	giacomo	561	b = 0x10 \| (servo & 0x0F);
289	giacomo	562	err = com_send(servo_port, b);
		563	err = com_receive(servo_port);
285	giacomo	564	if (err != (int)(b)) timer_expired = 1;
		565
		566	if (timeout_event != NIL) kern_event_delete(timeout_event);
		567
		568	if (!timer_expired)
		569	return 0;
		570	else
		571	return -1;
		572
		573	}
		574
291	giacomo	575	/* 1000.0000:0010.0000 */
285	giacomo	576	int servo_turn_off_all(void)
323	giacomo	577	{
		578	struct timespec current_time;
285	giacomo	579	unsigned char b;
		580	int err;
		581
		582	timer_expired = 0;
		583	kern_gettime(&current_time);
		584	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		585	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		586
		587	b = 0x80;
289	giacomo	588	err = com_send(servo_port, b);
		589	err = com_receive(servo_port);
285	giacomo	590	if (err != (int)(b)) timer_expired = 1;
		591
291	giacomo	592	b = 0x20;
289	giacomo	593	err = com_send(servo_port, b);
		594	err = com_receive(servo_port);
285	giacomo	595	if (err != (int)(b)) timer_expired = 1;
		596
		597	if (timeout_event != NIL) kern_event_delete(timeout_event);
		598
		599	if (!timer_expired)
		600	return 0;
		601	else
		602	return -1;
		603
		604	}
		605
291	giacomo	606	/* 1000.0000:0010.0001 */
285	giacomo	607	int servo_turn_on_all(void)
		608	{
		609	struct timespec current_time;
		610	unsigned char b;
		611	int err;
		612
		613	timer_expired = 0;
		614	kern_gettime(&current_time);
		615	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		616	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		617
		618	b = 0x80;
289	giacomo	619	err = com_send(servo_port, b);
		620	err = com_receive(servo_port);
285	giacomo	621	if (err != (int)(b)) timer_expired = 1;
		622
291	giacomo	623	b = 0x21;
289	giacomo	624	err = com_send(servo_port, b);
		625	err = com_receive(servo_port);
285	giacomo	626	if (err != (int)(b)) timer_expired = 1;
		627
		628	if (timeout_event != NIL) kern_event_delete(timeout_event);
		629
		630	if (!timer_expired)
		631	return 0;
		632	else
		633	return -1;
		634
		635	}
		636
323	giacomo	637	/* 1000.0000:0101.000M:mmmm.mmmm */
		638	int servo_set_levels(int bank,int mask)
315	giacomo	639	{
		640	struct timespec current_time;
		641	unsigned char b;
		642	int err;
		643
		644	timer_expired = 0;
		645	kern_gettime(&current_time);
		646	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		647	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		648
		649	b = 0x80;
		650	err = com_send(servo_port, b);
		651	err = com_receive(servo_port);
		652	if (err != (int)(b)) timer_expired = 1;
		653
323	giacomo	654	b = 0x50 \| (0x01 & bank);
315	giacomo	655	err = com_send(servo_port, b);
		656	err = com_receive(servo_port);
		657	if (err != (int)(b)) timer_expired = 1;
		658
		659	b = (unsigned char)(mask & 0xFF);
		660	err = com_send(servo_port, b);
		661	err = com_receive(servo_port);
		662	if (err != (int)(b)) timer_expired = 1;
		663
		664	if (timeout_event != NIL) kern_event_delete(timeout_event);
		665
		666	if (!timer_expired)
		667	return 0;
		668	else
		669	return -1;
		670
		671	}
		672
323	giacomo	673	/* 1000.0000:0100.000M */
		674	int servo_get_levels(int bank)
297	giacomo	675	{
		676	struct timespec current_time;
		677	unsigned char b;
		678	int err;
		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);
		684
		685	b = 0x80;
		686	err = com_send(servo_port, b);
		687	err = com_receive(servo_port);
		688	if (err != (int)(b)) timer_expired = 1;
		689
323	giacomo	690	b = 0x40 \| (0x01 & bank);
297	giacomo	691	err = com_send(servo_port, b);
		692	err = com_receive(servo_port);
		693	if (err != (int)(b)) timer_expired = 1;
		694	err = com_receive(servo_port);
		695
		696	if (timeout_event != NIL) kern_event_delete(timeout_event);
		697
		698	if (!timer_expired)
316	giacomo	699	return err;
297	giacomo	700	else
		701	return -1;
		702
		703	}
		704
		705	/* 1000.0000:1000.0000 */
		706	int servo_store_levels(void)
		707	{
		708	struct timespec current_time;
		709	unsigned char b;
		710	int err;
		711
		712	timer_expired = 0;
		713	kern_gettime(&current_time);
		714	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		715	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		716
		717	b = 0x80;
		718	err = com_send(servo_port, b);
		719	err = com_receive(servo_port);
		720	if (err != (int)(b)) timer_expired = 1;
		721
		722	b = 0x80;
		723	err = com_send(servo_port, b);
		724	err = com_receive(servo_port);
		725	if (err != (int)(b)) timer_expired = 1;
		726
		727	if (timeout_event != NIL) kern_event_delete(timeout_event);
		728
		729	if (!timer_expired)
		730	return 0;
		731	else
		732	return -1;
		733
		734	}
		735
290	giacomo	736	int servo_set_max_angle(int servo, int angle_sec)
		737	{
		738
		739	servo_table[servo].max_angle_sec = angle_sec;
		740	return 0;
		741
		742	}
		743
		744	int servo_set_min_angle(int servo, int angle_sec)
		745	{
		746
		747	servo_table[servo].min_angle_sec = angle_sec;
		748	return 0;
		749
		750	}
		751
323	giacomo	752	/* 0000.Pppp:0000.vvvv:vvvv.vvvv */
290	giacomo	753	int servo_set_angle_sec(int servo, int angle_sec)
281	giacomo	754	{
282	giacomo	755	struct timespec current_time;
		756	unsigned char b;
290	giacomo	757	int err, angle_tick;
282	giacomo	758
323	giacomo	759	if (servo > 15) return -1;
282	giacomo	760
290	giacomo	761	if (angle_sec > servo_table[servo].max_angle_sec \|\|
		762	angle_sec < servo_table[servo].min_angle_sec) return -1;
		763
316	giacomo	764	angle_tick = (servo_table[servo].zero_tick + angle_sec * servo_table[servo].delta_tick /
		765	(servo_table[servo].max_angle_sec - servo_table[servo].min_angle_sec)) * 1000 / TICK_LEN;
290	giacomo	766
282	giacomo	767	timer_expired = 0;
		768	kern_gettime(&current_time);
		769	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		770	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		771
323	giacomo	772	b = 0x00 \| (servo & 0x0F);
289	giacomo	773	err = com_send(servo_port, b);
		774	err = com_receive(servo_port);
283	giacomo	775	if (err != (int)(b)) timer_expired = 1;
282	giacomo	776
290	giacomo	777	b = 0x00 \| ((angle_tick >> 8) & 0x0F);
289	giacomo	778	err = com_send(servo_port, b);
		779	err = com_receive(servo_port);
283	giacomo	780	if (err != (int)(b)) timer_expired = 1;
282	giacomo	781
290	giacomo	782	b = 0x00 \| (angle_tick & 0xFF);
289	giacomo	783	err = com_send(servo_port, b);
		784	err = com_receive(servo_port);
283	giacomo	785	if (err != (int)(b)) timer_expired = 1;
282	giacomo	786
283	giacomo	787	if (timeout_event != NIL) kern_event_delete(timeout_event);
282	giacomo	788
283	giacomo	789	if (!timer_expired)
		790	return 0;
		791	else
		792	return -1;
281	giacomo	793
		794	}
		795
323	giacomo	796	/* 0010.Pppp */
290	giacomo	797	int servo_store_default_position(int servo)
		798	{
		799	struct timespec current_time;
		800	unsigned char b;
		801	int err;
		802
323	giacomo	803	if (servo > 15) return -1;
290	giacomo	804
		805	timer_expired = 0;
		806	kern_gettime(&current_time);
		807	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		808	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		809
323	giacomo	810	b = 0x20 \| (servo & 0x0F);
290	giacomo	811	err = com_send(servo_port, b);
		812	err = com_receive(servo_port);
		813	if (err != (int)(b)) timer_expired = 1;
		814
		815	if (timeout_event != NIL) kern_event_delete(timeout_event);
		816
		817	if (!timer_expired)
		818	return 0;
		819	else
		820	return -1;
		821
		822	}
		823
323	giacomo	824	/* 0001.Pppp */
290	giacomo	825	int servo_get_angle_sec(int servo)
281	giacomo	826	{
282	giacomo	827	struct timespec current_time;
		828	unsigned char b;
290	giacomo	829	int err,res,data;
282	giacomo	830
323	giacomo	831	if (servo > 15) return -1;
282	giacomo	832
		833	timer_expired = 0;
		834	kern_gettime(&current_time);
		835	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		836	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
281	giacomo	837
323	giacomo	838	b = 0x10 \| (servo & 0x0F);
289	giacomo	839	err = com_send(servo_port, b);
		840	err = com_receive(servo_port);
283	giacomo	841	if (err != (int)(b)) timer_expired = 1;
289	giacomo	842	res = com_receive(servo_port) << 8;
		843	res \|= com_receive(servo_port);
282	giacomo	844
283	giacomo	845	if (timeout_event != NIL) kern_event_delete(timeout_event);
282	giacomo	846
316	giacomo	847	data = ((resTICK_LEN/1000) - servo_table[servo].zero_tick)
290	giacomo	848	(servo_table[servo].max_angle_sec - servo_table[servo].min_angle_sec) /
		849	servo_table[servo].delta_tick;
		850
282	giacomo	851	if (!timer_expired)
290	giacomo	852	return data;
282	giacomo	853	else
		854	return -1;
		855
281	giacomo	856	}
		857
282	giacomo	858	/* 0100:0aaa */
281	giacomo	859	int servo_get_analog(int port)
		860	{
		861
282	giacomo	862	struct timespec current_time;
		863	unsigned char b;
283	giacomo	864	int err,res;
282	giacomo	865
323	giacomo	866	if (port > 7) return -1;
282	giacomo	867
		868	timer_expired = 0;
		869	kern_gettime(&current_time);
		870	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		871	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
281	giacomo	872
282	giacomo	873	b = 0x40 \| (port & 7);
289	giacomo	874	err = com_send(servo_port, b);
		875	err = com_receive(servo_port);
283	giacomo	876	if (err != (int)(b)) timer_expired = 1;
289	giacomo	877	res = com_receive(servo_port) << 8;
		878	res \|= com_receive(servo_port);
282	giacomo	879
283	giacomo	880	if (timeout_event != NIL) kern_event_delete(timeout_event);
282	giacomo	881
		882	if (!timer_expired)
		883	return res;
		884	else
		885	return -1;
		886
281	giacomo	887	}
		888

Subversion Repositories shark

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