WebSVN - shark - Blame - Rev 717 - /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>
381	giacomo	10	* Mauro Marinoni <mauro.marinoni@unipv.it>
281	giacomo	11	* (see the web pages for full authors list)
		12	*
		13	* ReTiS Lab (Scuola Superiore S.Anna - Pisa - Italy)
		14	*
		15	* http://www.sssup.it
		16	* http://retis.sssup.it
		17	* http://shark.sssup.it
		18	*/
		19
		20	/*
		21	* Copyright (C) 2002 Paolo Gai
		22	*
		23	* This program is free software; you can redistribute it and/or modify
		24	* it under the terms of the GNU General Public License as published by
		25	* the Free Software Foundation; either version 2 of the License, or
		26	* (at your option) any later version.
		27	*
		28	* This program is distributed in the hope that it will be useful,
		29	* but WITHOUT ANY WARRANTY; without even the implied warranty of
		30	* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
		31	* GNU General Public License for more details.
		32	*
		33	* You should have received a copy of the GNU General Public License
		34	* along with this program; if not, write to the Free Software
		35	* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
		36	*
		37	*/
		38
		39	#include <ll/sys/ll/ll-instr.h>
282	giacomo	40	#include "kernel/kern.h"
281	giacomo	41
717	giacomo	42	#include "drivers/scom.h"
		43	#include "drivers/scomirq.h"
281	giacomo	44
717	giacomo	45	#include "servo.h"
286	giacomo	46
358	giacomo	47	//#define SERVO_DEBUG
493	giacomo	48	//#define SERVO_TIMEOUT_EVENT
286	giacomo	49
358	giacomo	50	#define SERVO_TIMEOUT 200000 /* us */
281	giacomo	51
398	giacomo	52	#define SERVO_SPEED 19200
286	giacomo	53	#define SERVO_PARITY NONE
		54	#define SERVO_LEN 8
		55	#define SERVO_STOP 1
282	giacomo	56
358	giacomo	57	#define SERVO_CLOCK 20000000 /* 20MHz */
		58
315	giacomo	59	#define TICK_LEN 1600 /* ns */
316	giacomo	60	#define TICK_LEN_PERIOD 51200 /* ns */
290	giacomo	61
		62	struct servo_data {
		63	int min_angle_sec;
		64	int max_angle_sec;
		65	int delta_tick;
		66	int zero_tick;
		67	};
		68
381	giacomo	69	struct servo_data servo_table[4][16] = {
		70	{{-324000, 324000, 1200, 1600},
316	giacomo	71	{-324000, 324000, 1200, 1600},
		72	{-324000, 324000, 1200, 1600},
		73	{-324000, 324000, 1200, 1600},
		74	{-324000, 324000, 1200, 1600},
		75	{-324000, 324000, 1200, 1600},
		76	{-324000, 324000, 1200, 1600},
		77	{-324000, 324000, 1200, 1600},
335	giacomo	78	{-324000, 324000, 1200, 1600},
		79	{-324000, 324000, 1200, 1600},
		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},
381	giacomo	85	{-324000, 324000, 1200, 1600}},
		86	{{-324000, 324000, 1200, 1600},
335	giacomo	87	{-324000, 324000, 1200, 1600},
381	giacomo	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},
		95	{-324000, 324000, 1200, 1600},
		96	{-324000, 324000, 1200, 1600},
		97	{-324000, 324000, 1200, 1600},
		98	{-324000, 324000, 1200, 1600},
		99	{-324000, 324000, 1200, 1600},
		100	{-324000, 324000, 1200, 1600},
		101	{-324000, 324000, 1200, 1600}},
		102	{{-324000, 324000, 1200, 1600},
		103	{-324000, 324000, 1200, 1600},
		104	{-324000, 324000, 1200, 1600},
		105	{-324000, 324000, 1200, 1600},
		106	{-324000, 324000, 1200, 1600},
		107	{-324000, 324000, 1200, 1600},
		108	{-324000, 324000, 1200, 1600},
		109	{-324000, 324000, 1200, 1600},
		110	{-324000, 324000, 1200, 1600},
		111	{-324000, 324000, 1200, 1600},
		112	{-324000, 324000, 1200, 1600},
		113	{-324000, 324000, 1200, 1600},
		114	{-324000, 324000, 1200, 1600},
		115	{-324000, 324000, 1200, 1600},
		116	{-324000, 324000, 1200, 1600},
		117	{-324000, 324000, 1200, 1600}},
		118	{{-324000, 324000, 1200, 1600},
		119	{-324000, 324000, 1200, 1600},
		120	{-324000, 324000, 1200, 1600},
		121	{-324000, 324000, 1200, 1600},
		122	{-324000, 324000, 1200, 1600},
		123	{-324000, 324000, 1200, 1600},
		124	{-324000, 324000, 1200, 1600},
		125	{-324000, 324000, 1200, 1600},
		126	{-324000, 324000, 1200, 1600},
		127	{-324000, 324000, 1200, 1600},
		128	{-324000, 324000, 1200, 1600},
		129	{-324000, 324000, 1200, 1600},
		130	{-324000, 324000, 1200, 1600},
		131	{-324000, 324000, 1200, 1600},
		132	{-324000, 324000, 1200, 1600},
		133	{-324000, 324000, 1200, 1600}}};
290	giacomo	134
281	giacomo	135	int timer_expired = 0;
283	giacomo	136	int timeout_event;
281	giacomo	137
717	giacomo	138	#define RXTX_BUFF_MAX 100
		139	static BYTE RXTX_buff[4][RXTX_BUFF_MAX];
		140	static BYTE *RXTX_addr[4][RXTX_BUFF_MAX];
		141	unsigned int RX_position[4] = {0,0,0,0};
		142	unsigned int TX_position[4] = {0,0,0,0};
286	giacomo	143
		144	const int BaudTable[] = {
		145	1200,
		146	2400,
		147	4800,
		148	9600,
		149	14400,
		150	19200,
		151	38400,
		152	57600,
		153	115200,
		154	-1};
		155
282	giacomo	156	void set_timer_expired(void *arg)
281	giacomo	157	{
283	giacomo	158	timeout_event = NIL;
281	giacomo	159	timer_expired = 1;
		160	}
		161
717	giacomo	162	void servo_rx_error(unsigned port) {
292	giacomo	163
717	giacomo	164	kern_printf("SERVO: RX ERROR COM%d\n",port);
		165
281	giacomo	166	}
		167
717	giacomo	168	void servo_indication(unsigned port, BYTE data) {
286	giacomo	169
717	giacomo	170	if (RXTX_addr[port][RX_position[port]] == NULL) {
		171	if (data != RXTX_buff[port][RX_position[port]])
		172	servo_rx_error(port);
		173	RX_position[port]++;
		174	if (RX_position[port] >= RXTX_BUFF_MAX) RX_position[port] = 0;
		175	if (TX_position[port] == RX_position[port])
		176	servo_rx_error(port);
		177	} else {
		178	*RXTX_addr[port][RX_position[port]] = data;
		179	RX_position[port]++;
		180	if (RX_position[port] >= RXTX_BUFF_MAX) RX_position[port] = 0;
		181	if (TX_position[port] == RX_position[port])
		182	servo_rx_error(port);
		183	}
295	giacomo	184
717	giacomo	185	cprintf("(TXRX:%d:%d)",TX_position[port],RX_position[port]);
		186
286	giacomo	187	}
		188
717	giacomo	189	void servo_confirm(unsigned port, BYTE msg_status) {
286	giacomo	190
717	giacomo	191	#ifdef SERVO_DEBUG
		192	if (msg_status == COM_ERROR)
		193	kern_printf("(SERVO:COM%d ERROR)",port);
		194	#endif
286	giacomo	195
717	giacomo	196	}
		197
		198	int servo_send_msg(unsigned port, BYTE msg, unsigned len_msg, BYTE res, unsigned len_res) {
		199
		200	int i = 0, old;
		201
		202	old = TX_position[port];
		203
		204	cprintf("(SEND:%d:%d)",len_msg,len_res);
		205
		206	while(i < len_msg) {
		207	RXTX_buff[port][TX_position[port]] = msg[i];
		208	RXTX_addr[port][TX_position[port]] = NULL;
		209	TX_position[port]++;
		210	if (TX_position[port] >= RXTX_BUFF_MAX) TX_position[port] = 0;
		211	if (TX_position[port] == RX_position[port]) {
		212	TX_position[port] = old;
		213	cprintf("Error\n");
		214	return -1;
		215	}
		216	i++;
		217	}
		218
		219	i = 0;
		220	while(i < len_res) {
		221	RXTX_buff[port][TX_position[port]] = 0;
		222	RXTX_addr[port][TX_position[port]] = res+i;
		223	TX_position[port]++;
		224	if (TX_position[port] >= RXTX_BUFF_MAX) TX_position[port] = 0;
		225	if (TX_position[port] == RX_position[port]) {
		226	TX_position[port] = old;
		227	cprintf("Error\n");
		228	return -1;
		229	}
		230	i++;
		231	}
		232
		233	com_irq_send(port, len_msg, msg);
		234
		235	cprintf("(TXRX:%d:%d)",TX_position[port],RX_position[port]);
		236
286	giacomo	237	return 0;
		238
		239	}
		240
358	giacomo	241	int servo_open(int port,int speed)
286	giacomo	242	{
		243	int err;
		244
358	giacomo	245	err = com_open((unsigned)(port), (DWORD)speed, SERVO_PARITY, SERVO_LEN, SERVO_STOP);
		246
717	giacomo	247	com_init_irq((unsigned)(port));
		248
		249	com_set_functions(servo_confirm,servo_indication);
		250
286	giacomo	251	return err;
		252
		253	}
		254
358	giacomo	255	int servo_close(int port)
286	giacomo	256	{
		257	int err;
		258
717	giacomo	259	com_close_irq((unsigned)(port));
		260
358	giacomo	261	err = com_close((unsigned)(port));
286	giacomo	262
		263	return err;
		264
		265	}
		266
		267	/* 1000.011w:bbbb.bbbb */
358	giacomo	268	int servo_set_RS232_baudrate(int port, int baud)
281	giacomo	269	{
381	giacomo	270	#ifdef SERVO_TIMEOUT_EVENT
		271	struct timespec current_time;
		272	#endif
285	giacomo	273	unsigned char b;
358	giacomo	274	int err, spbrg_temp, i;
286	giacomo	275	unsigned char spbrg, w;
358	giacomo	276	int servo_port = (unsigned)(port);
286	giacomo	277
		278	i = 0;
		279	while(BaudTable[i] != baud && BaudTable[i] != -1) i++;
		280	if (BaudTable[i] == -1) {
		281	kern_printf("SERVO:Error wrong baud rate\n");
		282	return -1;
		283	}
		284
358	giacomo	285	w = 1;
453	giacomo	286	spbrg_temp = (SERVO_CLOCK10 / (16baud)) - 10;
412	giacomo	287	if (spbrg_temp>2550) {
358	giacomo	288	w = 0;
453	giacomo	289	spbrg_temp = (SERVO_CLOCK10 / (64baud)) - 10;
286	giacomo	290	}
412	giacomo	291	spbrg = spbrg_temp / 10;
453	giacomo	292	if (spbrg_temp % 10 > 5) spbrg++;
412	giacomo	293
286	giacomo	294	#ifdef SERVO_DEBUG
		295	kern_printf("(SERVO:SBPRG %d W %d)",spbrg,w);
358	giacomo	296	#endif
		297
285	giacomo	298	timer_expired = 0;
358	giacomo	299
381	giacomo	300	#ifdef SERVO_TIMEOUT_EVENT
		301	kern_gettime(&current_time);
		302	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		303	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		304	#else
		305	timeout_event = NIL;
		306	#endif
		307
286	giacomo	308	b = 0x86 \| (w & 0x01);
289	giacomo	309	err = com_send(servo_port, b);
		310	err = com_receive(servo_port);
285	giacomo	311	if (err != (int)(b)) timer_expired = 1;
281	giacomo	312
286	giacomo	313	b = spbrg;
289	giacomo	314	err = com_send(servo_port, b);
		315	err = com_receive(servo_port);
285	giacomo	316	if (err != (int)(b)) timer_expired = 1;
281	giacomo	317
285	giacomo	318	if (timeout_event != NIL) kern_event_delete(timeout_event);
358	giacomo	319
		320	/*com_close(servo_port);
		321	com_open(servo_port, baud, SERVO_PARITY, SERVO_LEN, SERVO_STOP);*/
		322
285	giacomo	323	if (!timer_expired)
		324	return 0;
		325	else
		326	return -1;
		327
281	giacomo	328	}
		329
285	giacomo	330	/* 1000.0101 */
358	giacomo	331	int servo_get_RS232_baudrate(int port)
281	giacomo	332	{
381	giacomo	333	#ifdef SERVO_TIMEOUT_EVENT
		334	struct timespec current_time;
		335	#endif
285	giacomo	336	unsigned char b;
358	giacomo	337	int err, res, res_w, res_b;
		338	int servo_port = (unsigned)(port);
		339
285	giacomo	340	timer_expired = 0;
381	giacomo	341
		342	#ifdef SERVO_TIMEOUT_EVENT
		343	kern_gettime(&current_time);
		344	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		345	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		346	#else
		347	timeout_event = NIL;
		348	#endif
358	giacomo	349
285	giacomo	350	b = 0x85;
289	giacomo	351	err = com_send(servo_port, b);
		352	err = com_receive(servo_port);
285	giacomo	353	if (err != (int)(b)) timer_expired = 1;
465	giacomo	354
358	giacomo	355	res_w = com_receive(servo_port); /* bit W */
		356	res_b = com_receive(servo_port); /* byte SPBRG */
465	giacomo	357
		358	if (res_w != -1 && res_b != -1) {
		359	if (res_w)
		360	res = SERVO_CLOCK / ( 16 * (res_b + 1) );
		361	else
		362	res = SERVO_CLOCK / ( 64 * (res_b + 1) );
		363	} else {
		364	return -1;
		365	}
358	giacomo	366
285	giacomo	367	if (timeout_event != NIL) kern_event_delete(timeout_event);
358	giacomo	368
285	giacomo	369	if (!timer_expired)
		370	return res;
		371	else
		372	return -1;
281	giacomo	373
		374	}
		375
286	giacomo	376	/* 1000.0100 */
358	giacomo	377	int servo_store_RS232_baudrate(int port)
281	giacomo	378	{
381	giacomo	379	#ifdef SERVO_TIMEOUT_EVENT
		380	struct timespec current_time;
		381	#endif
285	giacomo	382	unsigned char b;
		383	int err;
358	giacomo	384	int servo_port = (unsigned)(port);
		385
285	giacomo	386	timer_expired = 0;
381	giacomo	387	#ifdef SERVO_TIMEOUT_EVENT
		388	kern_gettime(&current_time);
		389	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		390	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		391	#else
		392	timeout_event = NIL;
		393	#endif
		394
358	giacomo	395
286	giacomo	396	b = 0x84;
289	giacomo	397	err = com_send(servo_port, b);
		398	err = com_receive(servo_port);
285	giacomo	399	if (err != (int)(b)) timer_expired = 1;
358	giacomo	400
285	giacomo	401	if (timeout_event != NIL) kern_event_delete(timeout_event);
358	giacomo	402
285	giacomo	403	if (!timer_expired)
		404	return 0;
		405	else
358	giacomo	406	return -1;
281	giacomo	407
		408	}
		409
289	giacomo	410	/* 1000.1010:llll.llll */
358	giacomo	411	int servo_set_period(int port, int period)
281	giacomo	412	{
381	giacomo	413	#ifdef SERVO_TIMEOUT_EVENT
		414	struct timespec current_time;
		415	#endif
285	giacomo	416	unsigned char b;
		417	int err;
358	giacomo	418	int servo_port = (unsigned)(port);
		419
285	giacomo	420	timer_expired = 0;
381	giacomo	421	#ifdef SERVO_TIMEOUT_EVENT
		422	kern_gettime(&current_time);
		423	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		424	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		425	#else
		426	timeout_event = NIL;
		427	#endif
		428
289	giacomo	429	b = 0x8A;
		430	err = com_send(servo_port, b);
		431	err = com_receive(servo_port);
285	giacomo	432	if (err != (int)(b)) timer_expired = 1;
358	giacomo	433
344	giacomo	434	b = (period*1000)/TICK_LEN_PERIOD/8 & 0xFF;
289	giacomo	435	err = com_send(servo_port, b);
		436	err = com_receive(servo_port);
285	giacomo	437	if (err != (int)(b)) timer_expired = 1;
358	giacomo	438
285	giacomo	439	if (timeout_event != NIL) kern_event_delete(timeout_event);
358	giacomo	440
285	giacomo	441	if (!timer_expired)
		442	return 0;
		443	else
		444	return -1;
281	giacomo	445
		446	}
		447
285	giacomo	448	/* 1000.1001 */
358	giacomo	449	int servo_get_period(int port)
281	giacomo	450	{
381	giacomo	451	#ifdef SERVO_TIMEOUT_EVENT
		452	struct timespec current_time;
		453	#endif
285	giacomo	454	unsigned char b;
		455	int err,res;
358	giacomo	456	int servo_port = (unsigned)(port);
		457
285	giacomo	458	timer_expired = 0;
358	giacomo	459
381	giacomo	460	#ifdef SERVO_TIMEOUT_EVENT
		461	kern_gettime(&current_time);
		462	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		463	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		464	#else
		465	timeout_event = NIL;
		466	#endif
		467
285	giacomo	468	b = 0x89;
289	giacomo	469	err = com_send(servo_port, b);
		470	err = com_receive(servo_port);
285	giacomo	471	if (err != (int)(b)) timer_expired = 1;
358	giacomo	472	res = com_receive(servo_port);
		473
285	giacomo	474	if (timeout_event != NIL) kern_event_delete(timeout_event);
358	giacomo	475
285	giacomo	476	if (!timer_expired)
345	giacomo	477	return (((unsigned char)(res))TICK_LEN_PERIOD/10008);
285	giacomo	478	else
		479	return -1;
281	giacomo	480
		481	}
		482
289	giacomo	483	/* 1000.1000 */
358	giacomo	484	int servo_store_period(int port)
281	giacomo	485	{
381	giacomo	486	#ifdef SERVO_TIMEOUT_EVENT
		487	struct timespec current_time;
		488	#endif
285	giacomo	489	unsigned char b;
		490	int err;
358	giacomo	491	int servo_port = (unsigned)(port);
		492
285	giacomo	493	timer_expired = 0;
358	giacomo	494
381	giacomo	495	#ifdef SERVO_TIMEOUT_EVENT
		496	kern_gettime(&current_time);
		497	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		498	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		499	#else
		500	timeout_event = NIL;
		501	#endif
		502
289	giacomo	503	b = 0x88;
		504	err = com_send(servo_port, b);
		505	err = com_receive(servo_port);
285	giacomo	506	if (err != (int)(b)) timer_expired = 1;
358	giacomo	507
285	giacomo	508	if (timeout_event != NIL) kern_event_delete(timeout_event);
358	giacomo	509
285	giacomo	510	if (!timer_expired)
		511	return 0;
		512	else
358	giacomo	513	return -1;
281	giacomo	514
		515	}
		516
283	giacomo	517	/* 1000.1100 */
358	giacomo	518	int servo_get_setup_switch(int port)
281	giacomo	519	{
381	giacomo	520	#ifdef SERVO_TIMEOUT_EVENT
		521	struct timespec current_time;
		522	#endif
283	giacomo	523	unsigned char b;
358	giacomo	524	int err,res;
		525	int servo_port = (unsigned)(port);
		526
283	giacomo	527	timer_expired = 0;
381	giacomo	528
		529	#ifdef SERVO_TIMEOUT_EVENT
		530	kern_gettime(&current_time);
		531	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		532	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		533	#else
		534	timeout_event = NIL;
		535	#endif
281	giacomo	536
283	giacomo	537	b = 0x8C;
289	giacomo	538	err = com_send(servo_port, b);
		539	err = com_receive(servo_port);
283	giacomo	540	if (err != (int)(b)) timer_expired = 1;
289	giacomo	541	res = com_receive(servo_port);
281	giacomo	542
283	giacomo	543	if (timeout_event != NIL) kern_event_delete(timeout_event);
		544
		545	if (!timer_expired)
		546	return res;
		547	else
		548	return -1;
		549
281	giacomo	550	}
		551
283	giacomo	552	/* 1000.111s */
358	giacomo	553	int servo_set_RC5_switch(int port, int data)
281	giacomo	554	{
381	giacomo	555	#ifdef SERVO_TIMEOUT_EVENT
		556	struct timespec current_time;
		557	#endif
283	giacomo	558	unsigned char b;
		559	int err;
358	giacomo	560	int servo_port = (unsigned)(port);
		561
283	giacomo	562	timer_expired = 0;
281	giacomo	563
381	giacomo	564	#ifdef SERVO_TIMEOUT_EVENT
		565	kern_gettime(&current_time);
		566	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		567	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		568	#else
		569	timeout_event = NIL;
		570	#endif
		571
283	giacomo	572	b = 0x8E \| (data & 0x01);
289	giacomo	573	err = com_send(servo_port, b);
		574	err = com_receive(servo_port);
283	giacomo	575	if (err != (int)(b)) timer_expired = 1;
		576
358	giacomo	577	if (timeout_event != NIL) kern_event_delete(timeout_event);
		578
283	giacomo	579	if (!timer_expired)
		580	return 0;
		581	else
		582	return -1;
		583
281	giacomo	584	}
		585
323	giacomo	586	/* 1000.0000:0000.Mmmm */
358	giacomo	587	int servo_turn_off(int port, int servo)
285	giacomo	588	{
		589
381	giacomo	590	#ifdef SERVO_TIMEOUT_EVENT
		591	struct timespec current_time;
		592	#endif
285	giacomo	593	unsigned char b;
358	giacomo	594	int err;
		595	int servo_port = (unsigned)(port);
		596
		597	if (servo > 15) return -1;
285	giacomo	598	timer_expired = 0;
381	giacomo	599
		600	#ifdef SERVO_TIMEOUT_EVENT
		601	kern_gettime(&current_time);
		602	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		603	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		604	#else
		605	timeout_event = NIL;
		606	#endif
285	giacomo	607
		608	b = 0x80;
289	giacomo	609	err = com_send(servo_port, b);
		610	err = com_receive(servo_port);
285	giacomo	611	if (err != (int)(b)) timer_expired = 1;
		612
323	giacomo	613	b = 0x00 \| (servo & 0x0F);
289	giacomo	614	err = com_send(servo_port, b);
		615	err = com_receive(servo_port);
285	giacomo	616	if (err != (int)(b)) timer_expired = 1;
		617
		618	if (timeout_event != NIL) kern_event_delete(timeout_event);
358	giacomo	619
285	giacomo	620	if (!timer_expired)
		621	return 0;
		622	else
		623	return -1;
		624
		625	}
		626
325	giacomo	627	/* 1000.0000:0001.Mmmm */
358	giacomo	628	int servo_turn_on(int port, int servo)
285	giacomo	629	{
358	giacomo	630
381	giacomo	631	#ifdef SERVO_TIMEOUT_EVENT
		632	struct timespec current_time;
		633	#endif
285	giacomo	634	unsigned char b;
		635	int err;
358	giacomo	636	int servo_port = (unsigned)(port);
		637
323	giacomo	638	if (servo > 15) return -1;
358	giacomo	639
285	giacomo	640	timer_expired = 0;
381	giacomo	641
		642	#ifdef SERVO_TIMEOUT_EVENT
		643	kern_gettime(&current_time);
		644	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		645	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		646	#else
		647	timeout_event = NIL;
		648	#endif
358	giacomo	649
285	giacomo	650	b = 0x80;
289	giacomo	651	err = com_send(servo_port, b);
		652	err = com_receive(servo_port);
285	giacomo	653	if (err != (int)(b)) timer_expired = 1;
358	giacomo	654
323	giacomo	655	b = 0x10 \| (servo & 0x0F);
289	giacomo	656	err = com_send(servo_port, b);
		657	err = com_receive(servo_port);
285	giacomo	658	if (err != (int)(b)) timer_expired = 1;
358	giacomo	659
285	giacomo	660	if (timeout_event != NIL) kern_event_delete(timeout_event);
358	giacomo	661
285	giacomo	662	if (!timer_expired)
		663	return 0;
		664	else
		665	return -1;
358	giacomo	666
285	giacomo	667	}
		668
291	giacomo	669	/* 1000.0000:0010.0000 */
358	giacomo	670	int servo_turn_off_all(int port)
323	giacomo	671	{
381	giacomo	672	#ifdef SERVO_TIMEOUT_EVENT
		673	struct timespec current_time;
		674	#endif
285	giacomo	675	unsigned char b;
		676	int err;
358	giacomo	677	int servo_port = (unsigned)(port);
		678
285	giacomo	679	timer_expired = 0;
381	giacomo	680
		681	#ifdef SERVO_TIMEOUT_EVENT
		682	kern_gettime(&current_time);
		683	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		684	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		685	#else
		686	timeout_event = NIL;
		687	#endif
358	giacomo	688
285	giacomo	689	b = 0x80;
289	giacomo	690	err = com_send(servo_port, b);
		691	err = com_receive(servo_port);
285	giacomo	692	if (err != (int)(b)) timer_expired = 1;
358	giacomo	693
291	giacomo	694	b = 0x20;
289	giacomo	695	err = com_send(servo_port, b);
		696	err = com_receive(servo_port);
285	giacomo	697	if (err != (int)(b)) timer_expired = 1;
358	giacomo	698
285	giacomo	699	if (timeout_event != NIL) kern_event_delete(timeout_event);
358	giacomo	700
285	giacomo	701	if (!timer_expired)
		702	return 0;
		703	else
		704	return -1;
358	giacomo	705
285	giacomo	706	}
		707
291	giacomo	708	/* 1000.0000:0010.0001 */
358	giacomo	709	int servo_turn_on_all(int port)
285	giacomo	710	{
381	giacomo	711	#ifdef SERVO_TIMEOUT_EVENT
		712	struct timespec current_time;
		713	#endif
285	giacomo	714	unsigned char b;
		715	int err;
358	giacomo	716	int servo_port = (unsigned)(port);
		717
285	giacomo	718	timer_expired = 0;
358	giacomo	719
381	giacomo	720	#ifdef SERVO_TIMEOUT_EVENT
		721	kern_gettime(&current_time);
		722	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		723	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		724	#else
		725	timeout_event = NIL;
		726	#endif
		727
285	giacomo	728	b = 0x80;
289	giacomo	729	err = com_send(servo_port, b);
		730	err = com_receive(servo_port);
285	giacomo	731	if (err != (int)(b)) timer_expired = 1;
358	giacomo	732
291	giacomo	733	b = 0x21;
289	giacomo	734	err = com_send(servo_port, b);
		735	err = com_receive(servo_port);
285	giacomo	736	if (err != (int)(b)) timer_expired = 1;
358	giacomo	737
285	giacomo	738	if (timeout_event != NIL) kern_event_delete(timeout_event);
358	giacomo	739
285	giacomo	740	if (!timer_expired)
		741	return 0;
		742	else
		743	return -1;
358	giacomo	744
285	giacomo	745	}
		746
323	giacomo	747	/* 1000.0000:0101.000M:mmmm.mmmm */
358	giacomo	748	int servo_set_levels(int port, int bank,int mask)
315	giacomo	749	{
381	giacomo	750	#ifdef SERVO_TIMEOUT_EVENT
		751	struct timespec current_time;
		752	#endif
315	giacomo	753	unsigned char b;
		754	int err;
358	giacomo	755	int servo_port = (unsigned)(port);
		756
315	giacomo	757	timer_expired = 0;
381	giacomo	758
		759	#ifdef SERVO_TIMEOUT_EVENT
		760	kern_gettime(&current_time);
		761	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		762	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		763	#else
		764	timeout_event = NIL;
		765	#endif
358	giacomo	766
315	giacomo	767	b = 0x80;
		768	err = com_send(servo_port, b);
		769	err = com_receive(servo_port);
		770	if (err != (int)(b)) timer_expired = 1;
358	giacomo	771
323	giacomo	772	b = 0x50 \| (0x01 & bank);
315	giacomo	773	err = com_send(servo_port, b);
		774	err = com_receive(servo_port);
		775	if (err != (int)(b)) timer_expired = 1;
358	giacomo	776
315	giacomo	777	b = (unsigned char)(mask & 0xFF);
		778	err = com_send(servo_port, b);
		779	err = com_receive(servo_port);
		780	if (err != (int)(b)) timer_expired = 1;
358	giacomo	781
315	giacomo	782	if (timeout_event != NIL) kern_event_delete(timeout_event);
358	giacomo	783
315	giacomo	784	if (!timer_expired)
		785	return 0;
		786	else
		787	return -1;
358	giacomo	788
315	giacomo	789	}
		790
323	giacomo	791	/* 1000.0000:0100.000M */
358	giacomo	792	int servo_get_levels(int port, int bank)
297	giacomo	793	{
381	giacomo	794	#ifdef SERVO_TIMEOUT_EVENT
		795	struct timespec current_time;
		796	#endif
297	giacomo	797	unsigned char b;
		798	int err;
358	giacomo	799	int servo_port = (unsigned)(port);
		800
297	giacomo	801	timer_expired = 0;
381	giacomo	802
		803	#ifdef SERVO_TIMEOUT_EVENT
		804	kern_gettime(&current_time);
		805	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		806	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		807	#else
		808	timeout_event = NIL;
		809	#endif
358	giacomo	810
297	giacomo	811	b = 0x80;
		812	err = com_send(servo_port, b);
		813	err = com_receive(servo_port);
		814	if (err != (int)(b)) timer_expired = 1;
358	giacomo	815
323	giacomo	816	b = 0x40 \| (0x01 & bank);
297	giacomo	817	err = com_send(servo_port, b);
		818	err = com_receive(servo_port);
		819	if (err != (int)(b)) timer_expired = 1;
358	giacomo	820	err = com_receive(servo_port);
		821
297	giacomo	822	if (timeout_event != NIL) kern_event_delete(timeout_event);
358	giacomo	823
297	giacomo	824	if (!timer_expired)
316	giacomo	825	return err;
297	giacomo	826	else
		827	return -1;
358	giacomo	828
297	giacomo	829	}
		830
		831	/* 1000.0000:1000.0000 */
358	giacomo	832	int servo_store_levels(int port)
297	giacomo	833	{
381	giacomo	834	#ifdef SERVO_TIMEOUT_EVENT
		835	struct timespec current_time;
		836	#endif
297	giacomo	837	unsigned char b;
		838	int err;
358	giacomo	839	int servo_port = (unsigned)(port);
		840
297	giacomo	841	timer_expired = 0;
381	giacomo	842
		843	#ifdef SERVO_TIMEOUT_EVENT
		844	kern_gettime(&current_time);
		845	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		846	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		847	#else
		848	timeout_event = NIL;
		849	#endif
358	giacomo	850
297	giacomo	851	b = 0x80;
		852	err = com_send(servo_port, b);
		853	err = com_receive(servo_port);
		854	if (err != (int)(b)) timer_expired = 1;
358	giacomo	855
297	giacomo	856	b = 0x80;
		857	err = com_send(servo_port, b);
		858	err = com_receive(servo_port);
		859	if (err != (int)(b)) timer_expired = 1;
358	giacomo	860
297	giacomo	861	if (timeout_event != NIL) kern_event_delete(timeout_event);
358	giacomo	862
297	giacomo	863	if (!timer_expired)
		864	return 0;
		865	else
		866	return -1;
358	giacomo	867
297	giacomo	868	}
		869
493	giacomo	870	int servo_set_max_angle_sec(int port, int servo, int angle_sec)
290	giacomo	871	{
		872
381	giacomo	873	servo_table[port][servo].max_angle_sec = angle_sec;
290	giacomo	874	return 0;
		875
		876	}
		877
493	giacomo	878	int servo_set_min_angle_sec(int port, int servo, int angle_sec)
290	giacomo	879	{
		880
381	giacomo	881	servo_table[port][servo].min_angle_sec = angle_sec;
290	giacomo	882	return 0;
		883
		884	}
		885
381	giacomo	886	int servo_set_servo_tick(int port, int servo, int zero_tick, int delta_tick)
		887	{
		888
		889	if (zero_tick != -1) servo_table[port][servo].zero_tick = zero_tick;
		890	if (delta_tick != -1) servo_table[port][servo].delta_tick = delta_tick;
		891	return 0;
		892
		893	}
		894
323	giacomo	895	/* 0000.Pppp:0000.vvvv:vvvv.vvvv */
358	giacomo	896	int servo_set_angle_sec(int port, int servo, int angle_sec)
281	giacomo	897	{
717	giacomo	898	unsigned char b[3];
		899	int angle_tick;
358	giacomo	900	int servo_port = (unsigned)(port);
282	giacomo	901
323	giacomo	902	if (servo > 15) return -1;
282	giacomo	903
381	giacomo	904	angle_tick = (servo_table[port][servo].zero_tick + angle_sec *
		905	servo_table[port][servo].delta_tick /
542	giacomo	906	(servo_table[port][servo].max_angle_sec - servo_table[port][servo].min_angle_sec)) * 1000 / TICK_LEN;
290	giacomo	907
717	giacomo	908	b[0] = 0x00 \| (servo & 0x0F);
		909	b[1] = 0x00 \| ((angle_tick >> 8) & 0x0F);
		910	b[2] = 0x00 \| (angle_tick & 0xFF);
		911	servo_send_msg(servo_port, b, 3, NULL, 0);
358	giacomo	912
717	giacomo	913	return 0;
282	giacomo	914
281	giacomo	915	}
		916
323	giacomo	917	/* 0010.Pppp */
358	giacomo	918	int servo_store_default_position(int port, int servo)
290	giacomo	919	{
381	giacomo	920	#ifdef SERVO_TIMEOUT_EVENT
		921	struct timespec current_time;
		922	#endif
290	giacomo	923	unsigned char b;
		924	int err;
358	giacomo	925	int servo_port = (unsigned)(port);
290	giacomo	926
323	giacomo	927	if (servo > 15) return -1;
358	giacomo	928
290	giacomo	929	timer_expired = 0;
381	giacomo	930
		931	#ifdef SERVO_TIMEOUT_EVENT
		932	kern_gettime(&current_time);
		933	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		934	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		935	#else
		936	timeout_event = NIL;
		937	#endif
		938
323	giacomo	939	b = 0x20 \| (servo & 0x0F);
290	giacomo	940	err = com_send(servo_port, b);
		941	err = com_receive(servo_port);
		942	if (err != (int)(b)) timer_expired = 1;
358	giacomo	943
290	giacomo	944	if (timeout_event != NIL) kern_event_delete(timeout_event);
358	giacomo	945
290	giacomo	946	if (!timer_expired)
		947	return 0;
		948	else
		949	return -1;
		950
		951	}
		952
323	giacomo	953	/* 0001.Pppp */
358	giacomo	954	int servo_get_angle_sec(int port, int servo)
281	giacomo	955	{
381	giacomo	956	#ifdef SERVO_TIMEOUT_EVENT
		957	struct timespec current_time;
		958	#endif
282	giacomo	959	unsigned char b;
290	giacomo	960	int err,res,data;
358	giacomo	961	int servo_port = (unsigned)(port);
		962
323	giacomo	963	if (servo > 15) return -1;
358	giacomo	964
282	giacomo	965	timer_expired = 0;
381	giacomo	966
		967	#ifdef SERVO_TIMEOUT_EVENT
		968	kern_gettime(&current_time);
		969	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		970	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		971	#else
		972	timeout_event = NIL;
		973	#endif
281	giacomo	974
323	giacomo	975	b = 0x10 \| (servo & 0x0F);
289	giacomo	976	err = com_send(servo_port, b);
		977	err = com_receive(servo_port);
283	giacomo	978	if (err != (int)(b)) timer_expired = 1;
289	giacomo	979	res = com_receive(servo_port) << 8;
		980	res \|= com_receive(servo_port);
282	giacomo	981
283	giacomo	982	if (timeout_event != NIL) kern_event_delete(timeout_event);
282	giacomo	983
381	giacomo	984	data = ((resTICK_LEN/1000) - servo_table[port][servo].zero_tick)
		985	(servo_table[port][servo].max_angle_sec - servo_table[port][servo].min_angle_sec) /
		986	servo_table[port][servo].delta_tick;
290	giacomo	987
282	giacomo	988	if (!timer_expired)
290	giacomo	989	return data;
282	giacomo	990	else
		991	return -1;
		992
281	giacomo	993	}
		994
282	giacomo	995	/* 0100:0aaa */
358	giacomo	996	int servo_get_analog(int port, int adport)
281	giacomo	997	{
		998
381	giacomo	999	#ifdef SERVO_TIMEOUT_EVENT
		1000	struct timespec current_time;
		1001	#endif
282	giacomo	1002	unsigned char b;
283	giacomo	1003	int err,res;
358	giacomo	1004	int servo_port = (unsigned)(port);
		1005
381	giacomo	1006	if (adport > 7) return -1;
358	giacomo	1007
282	giacomo	1008	timer_expired = 0;
381	giacomo	1009
		1010	#ifdef SERVO_TIMEOUT_EVENT
		1011	kern_gettime(&current_time);
		1012	ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
		1013	timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
		1014	#else
		1015	timeout_event = NIL;
		1016	#endif
281	giacomo	1017
358	giacomo	1018	b = 0x40 \| (adport & 0x07);
289	giacomo	1019	err = com_send(servo_port, b);
		1020	err = com_receive(servo_port);
283	giacomo	1021	if (err != (int)(b)) timer_expired = 1;
289	giacomo	1022	res = com_receive(servo_port) << 8;
		1023	res \|= com_receive(servo_port);
282	giacomo	1024
283	giacomo	1025	if (timeout_event != NIL) kern_event_delete(timeout_event);
282	giacomo	1026
		1027	if (!timer_expired)
		1028	return res;
		1029	else
		1030	return -1;
		1031
281	giacomo	1032	}
		1033

Subversion Repositories shark

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