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

Subversion Repositories shark

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