WebSVN - shark - Blame - Rev 733 - /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
282	giacomo	39	#include "kernel/kern.h"
281	giacomo	40
717	giacomo	41	#include "drivers/scom.h"
		42	#include "drivers/scomirq.h"
281	giacomo	43
717	giacomo	44	#include "servo.h"
286	giacomo	45
358	giacomo	46	//#define SERVO_DEBUG
286	giacomo	47
398	giacomo	48	#define SERVO_SPEED 19200
286	giacomo	49	#define SERVO_PARITY NONE
		50	#define SERVO_LEN 8
		51	#define SERVO_STOP 1
282	giacomo	52
358	giacomo	53	#define SERVO_CLOCK 20000000 /* 20MHz */
		54
315	giacomo	55	#define TICK_LEN 1600 /* ns */
316	giacomo	56	#define TICK_LEN_PERIOD 51200 /* ns */
290	giacomo	57
		58	struct servo_data {
		59	int min_angle_sec;
		60	int max_angle_sec;
		61	int delta_tick;
		62	int zero_tick;
		63	};
		64
381	giacomo	65	struct servo_data servo_table[4][16] = {
		66	{{-324000, 324000, 1200, 1600},
316	giacomo	67	{-324000, 324000, 1200, 1600},
		68	{-324000, 324000, 1200, 1600},
		69	{-324000, 324000, 1200, 1600},
		70	{-324000, 324000, 1200, 1600},
		71	{-324000, 324000, 1200, 1600},
		72	{-324000, 324000, 1200, 1600},
		73	{-324000, 324000, 1200, 1600},
335	giacomo	74	{-324000, 324000, 1200, 1600},
		75	{-324000, 324000, 1200, 1600},
		76	{-324000, 324000, 1200, 1600},
		77	{-324000, 324000, 1200, 1600},
		78	{-324000, 324000, 1200, 1600},
		79	{-324000, 324000, 1200, 1600},
		80	{-324000, 324000, 1200, 1600},
381	giacomo	81	{-324000, 324000, 1200, 1600}},
		82	{{-324000, 324000, 1200, 1600},
335	giacomo	83	{-324000, 324000, 1200, 1600},
381	giacomo	84	{-324000, 324000, 1200, 1600},
		85	{-324000, 324000, 1200, 1600},
		86	{-324000, 324000, 1200, 1600},
		87	{-324000, 324000, 1200, 1600},
		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}}};
290	giacomo	130
733	giacomo	131	#define SERVO_TIMEOUT 10000
730	giacomo	132
717	giacomo	133	#define RXTX_BUFF_MAX 100
726	giacomo	134	volatile static BYTE RXTX_buff[4][RXTX_BUFF_MAX];
		135	volatile static BYTE *RXTX_addr[4][RXTX_BUFF_MAX];
725	giacomo	136	volatile unsigned int RX_position[4] = {0,0,0,0};
		137	volatile unsigned int TX_position[4] = {0,0,0,0};
730	giacomo	138	volatile unsigned int RX_cycle[4] = {0,0,0,0};
		139	volatile unsigned int TX_cycle[4] = {0,0,0,0};
728	giacomo	140	volatile unsigned int lock_write[4] = {0,0,0,0};
286	giacomo	141
733	giacomo	142	#define SERVO_LOCK 0
		143	#define SERVO_UNLOCK 1
		144	#define SERVO_SKIP (BYTE *)(0xFFFFFFFF)
		145
286	giacomo	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
721	giacomo	158	void servo_rx_error(unsigned port, unsigned type) {
292	giacomo	159
721	giacomo	160	kern_printf("(SERVO: BUFFER ERROR PORT:%d ERR:%d)",port,type);
717	giacomo	161
281	giacomo	162	}
		163
717	giacomo	164	void servo_indication(unsigned port, BYTE data) {
727	giacomo	165
717	giacomo	166	if (RXTX_addr[port][RX_position[port]] == NULL) {
		167	if (data != RXTX_buff[port][RX_position[port]])
721	giacomo	168	servo_rx_error(port,1);
726	giacomo	169
733	giacomo	170	RXTX_addr[port][RX_position[port]] = SERVO_SKIP;
		171
717	giacomo	172	RX_position[port]++;
721	giacomo	173
		174	if (RX_position[port] >= RXTX_BUFF_MAX) {
730	giacomo	175	RX_cycle[port]++;
721	giacomo	176	RX_position[port] = 0;
		177	}
730	giacomo	178	if ((RX_cycle[port] == TX_cycle[port] && RX_position[port] > TX_position[port]) \|\|
		179	(RX_cycle[port] > TX_cycle[port]))
721	giacomo	180	servo_rx_error(port,2);
		181
717	giacomo	182	} else {
726	giacomo	183
733	giacomo	184	if (RXTX_addr[port][RX_position[port]] != SERVO_SKIP) {
721	giacomo	185
733	giacomo	186	*RXTX_addr[port][RX_position[port]] = data;
		187	RXTX_buff[port][RX_position[port]] = SERVO_UNLOCK; //Unlock the data
		188
		189	RXTX_addr[port][RX_position[port]] = SERVO_SKIP;
		190
		191	RX_position[port]++;
		192
		193	if (RX_position[port] >= RXTX_BUFF_MAX) {
730	giacomo	194	RX_cycle[port]++;
721	giacomo	195	RX_position[port] = 0;
733	giacomo	196	}
		197	if ((RX_cycle[port] == TX_cycle[port] && RX_position[port] > TX_position[port]) \|\|
		198	(RX_cycle[port] > TX_cycle[port]))
		199	servo_rx_error(port,3);
		200
		201	} else {
		202
		203	RX_position[port]++;
		204
		205	if (RX_position[port] >= RXTX_BUFF_MAX) {
		206	RX_cycle[port]++;
		207	RX_position[port] = 0;
		208	}
		209	if ((RX_cycle[port] == TX_cycle[port] && RX_position[port] > TX_position[port]) \|\|
		210	(RX_cycle[port] > TX_cycle[port]))
		211	servo_rx_error(port,3);
		212
		213	servo_indication(port, data);
		214
721	giacomo	215	}
726	giacomo	216
717	giacomo	217	}
295	giacomo	218
286	giacomo	219	}
		220
730	giacomo	221	extern unsigned SCom_Error[4];
727	giacomo	222
717	giacomo	223	void servo_confirm(unsigned port, BYTE msg_status) {
286	giacomo	224
727	giacomo	225	if (msg_status == COM_ERROR) {
730	giacomo	226	kern_printf("(SERVO: PORT:%d ERROR:%d)",port,SCom_Error[port]);
727	giacomo	227	}
286	giacomo	228
730	giacomo	229	lock_write[port] = 0;
727	giacomo	230
717	giacomo	231	}
		232
		233	int servo_send_msg(unsigned port, BYTE msg, unsigned len_msg, BYTE res, unsigned len_res) {
		234
726	giacomo	235	int i = 0, old, oldcycle, RX = 0;
733	giacomo	236	volatile int start = kern_gettime(NULL), timeout = 0;
721	giacomo	237	SYS_FLAGS f;
		238
733	giacomo	239	while(lock_write[port] == 1 && timeout == 0)
		240	if ((start + SERVO_TIMEOUT) < kern_gettime(NULL)) timeout = 1;
		241
		242	if (timeout == 1) {
		243	lock_write[port] = 0;
		244	return -1;
		245	}
		246
728	giacomo	247	lock_write[port] = 1;
727	giacomo	248
721	giacomo	249	f = kern_fsave();
		250
717	giacomo	251	old = TX_position[port];
730	giacomo	252	oldcycle = TX_cycle[port];
717	giacomo	253
		254	while(i < len_msg) {
		255	RXTX_buff[port][TX_position[port]] = msg[i];
		256	RXTX_addr[port][TX_position[port]] = NULL;
		257	TX_position[port]++;
721	giacomo	258	if (TX_position[port] >= RXTX_BUFF_MAX) {
730	giacomo	259	TX_cycle[port]++;
721	giacomo	260	TX_position[port] = 0;
		261	}
717	giacomo	262	if (TX_position[port] == RX_position[port]) {
		263	TX_position[port] = old;
730	giacomo	264	TX_cycle[port] = oldcycle;
		265	lock_write[port] = 0;
733	giacomo	266
		267	kern_printf("(SERVO: BUFFER_FULL !!)");
		268
721	giacomo	269	kern_frestore(f);
717	giacomo	270	return -1;
		271	}
		272	i++;
		273	}
		274
		275	i = 0;
		276	while(i < len_res) {
733	giacomo	277	RXTX_buff[port][TX_position[port]] = SERVO_LOCK; //Lock the data
717	giacomo	278	RXTX_addr[port][TX_position[port]] = res+i;
726	giacomo	279	RX = TX_position[port];
717	giacomo	280	TX_position[port]++;
721	giacomo	281	if (TX_position[port] >= RXTX_BUFF_MAX) {
730	giacomo	282	TX_cycle[port]++;
721	giacomo	283	TX_position[port] = 0;
		284	}
717	giacomo	285	if (TX_position[port] == RX_position[port]) {
		286	TX_position[port] = old;
730	giacomo	287	TX_cycle[port] = oldcycle;
		288	lock_write[port] = 0;
733	giacomo	289
		290	kern_printf("(SERVO: BUFFER_FULL !!)");
		291
721	giacomo	292	kern_frestore(f);
717	giacomo	293	return -1;
		294	}
		295	i++;
		296	}
		297
721	giacomo	298	kern_frestore(f);
		299
717	giacomo	300	com_irq_send(port, len_msg, msg);
		301
725	giacomo	302	return RX;
		303
		304	}
		305
733	giacomo	306	int servo_wait(unsigned port, int RX) {
730	giacomo	307
733	giacomo	308	volatile int start = kern_gettime(NULL), timeout = 0, temp;
730	giacomo	309
733	giacomo	310	/* Active wait until number bytes received */
		311	while(RXTX_buff[port][RX] == 0 && timeout == 0)
		312	if ((start + SERVO_TIMEOUT) < kern_gettime(NULL)) timeout = 1;
730	giacomo	313
733	giacomo	314	if (timeout == 1) {
		315	SYS_FLAGS f;
730	giacomo	316
733	giacomo	317	f = kern_fsave();
725	giacomo	318
733	giacomo	319	temp = RX;
727	giacomo	320
733	giacomo	321	while(RXTX_addr[port][temp] != SERVO_SKIP) {
		322	RXTX_addr[port][temp] = SERVO_SKIP;
		323	temp--;
		324	}
730	giacomo	325
733	giacomo	326	kern_frestore(f);
725	giacomo	327
733	giacomo	328	}
730	giacomo	329
733	giacomo	330	return timeout;
286	giacomo	331
		332	}
		333
358	giacomo	334	int servo_open(int port,int speed)
286	giacomo	335	{
		336	int err;
		337
358	giacomo	338	err = com_open((unsigned)(port), (DWORD)speed, SERVO_PARITY, SERVO_LEN, SERVO_STOP);
		339
717	giacomo	340	com_init_irq((unsigned)(port));
		341
		342	com_set_functions(servo_confirm,servo_indication);
		343
721	giacomo	344	com_irq_enable((unsigned)(port),ALL_IRQ);
		345
286	giacomo	346	return err;
		347
		348	}
		349
358	giacomo	350	int servo_close(int port)
286	giacomo	351	{
		352	int err;
		353
724	giacomo	354	com_irq_disable((unsigned)(port),ALL_IRQ);
		355
717	giacomo	356	com_close_irq((unsigned)(port));
		357
358	giacomo	358	err = com_close((unsigned)(port));
286	giacomo	359
		360	return err;
		361
		362	}
		363
		364	/* 1000.011w:bbbb.bbbb */
358	giacomo	365	int servo_set_RS232_baudrate(int port, int baud)
281	giacomo	366	{
724	giacomo	367	unsigned char b[2];
		368	int spbrg_temp, i;
286	giacomo	369	unsigned char spbrg, w;
358	giacomo	370	int servo_port = (unsigned)(port);
286	giacomo	371
		372	i = 0;
		373	while(BaudTable[i] != baud && BaudTable[i] != -1) i++;
		374	if (BaudTable[i] == -1) {
		375	kern_printf("SERVO:Error wrong baud rate\n");
		376	return -1;
		377	}
		378
358	giacomo	379	w = 1;
453	giacomo	380	spbrg_temp = (SERVO_CLOCK10 / (16baud)) - 10;
412	giacomo	381	if (spbrg_temp>2550) {
358	giacomo	382	w = 0;
453	giacomo	383	spbrg_temp = (SERVO_CLOCK10 / (64baud)) - 10;
286	giacomo	384	}
412	giacomo	385	spbrg = spbrg_temp / 10;
453	giacomo	386	if (spbrg_temp % 10 > 5) spbrg++;
412	giacomo	387
286	giacomo	388	#ifdef SERVO_DEBUG
		389	kern_printf("(SERVO:SBPRG %d W %d)",spbrg,w);
358	giacomo	390	#endif
		391
724	giacomo	392	b[0] = 0x86 \| (w & 0x01);
		393	b[1] = spbrg;
		394	servo_send_msg(servo_port, b, 2, NULL, 0);
358	giacomo	395
724	giacomo	396	return 0;
381	giacomo	397
281	giacomo	398	}
		399
285	giacomo	400	/* 1000.0101 */
358	giacomo	401	int servo_get_RS232_baudrate(int port)
281	giacomo	402	{
724	giacomo	403	unsigned char b[1],r[2];
733	giacomo	404	int res, res_w, res_b, t;
358	giacomo	405	int servo_port = (unsigned)(port);
725	giacomo	406	int RX;
358	giacomo	407
724	giacomo	408	b[0] = 0x85;
725	giacomo	409	RX = servo_send_msg(servo_port, b, 1, r, 2);
358	giacomo	410
732	giacomo	411	if (RX != -1) {
		412
733	giacomo	413	t = servo_wait(servo_port, RX);
732	giacomo	414
733	giacomo	415	if (t == 0) {
725	giacomo	416
732	giacomo	417	res_w = r[0]; /* bit W */
		418	res_b = r[1]; /* byte SPBRG */
465	giacomo	419
732	giacomo	420	if (res_w != -1 && res_b != -1) {
		421	if (res_w)
		422	res = SERVO_CLOCK / ( 16 * (res_b + 1) );
		423	else
		424	res = SERVO_CLOCK / ( 64 * (res_b + 1) );
		425	} else {
		426	return -1;
		427	}
358	giacomo	428
732	giacomo	429	return res;
358	giacomo	430
732	giacomo	431	} else return -1;
		432
		433	} else return -1;
		434
281	giacomo	435	}
		436
286	giacomo	437	/* 1000.0100 */
358	giacomo	438	int servo_store_RS232_baudrate(int port)
281	giacomo	439	{
724	giacomo	440	unsigned char b[1];
358	giacomo	441	int servo_port = (unsigned)(port);
		442
724	giacomo	443	b[0] = 0x84;
		444	servo_send_msg(servo_port, b, 1, NULL, 0);
		445
		446	return 0;
358	giacomo	447
281	giacomo	448	}
		449
289	giacomo	450	/* 1000.1010:llll.llll */
358	giacomo	451	int servo_set_period(int port, int period)
281	giacomo	452	{
724	giacomo	453	unsigned char b[2];
358	giacomo	454	int servo_port = (unsigned)(port);
		455
724	giacomo	456	b[0] = 0x8A;
		457	b[1] = (period*1000)/TICK_LEN_PERIOD/8 & 0xFF;
		458	servo_send_msg(servo_port, b, 2, NULL, 0);
381	giacomo	459
724	giacomo	460	return 0;
358	giacomo	461
281	giacomo	462	}
		463
285	giacomo	464	/* 1000.1001 */
358	giacomo	465	int servo_get_period(int port)
281	giacomo	466	{
724	giacomo	467	unsigned char b[1],r[1];
733	giacomo	468	int res, RX, t;
358	giacomo	469	int servo_port = (unsigned)(port);
		470
724	giacomo	471	b[0] = 0x89;
725	giacomo	472	RX = servo_send_msg(servo_port, b, 1, r, 1);
358	giacomo	473
732	giacomo	474	if (RX != -1) {
		475
733	giacomo	476	t = servo_wait(servo_port, RX);
732	giacomo	477
733	giacomo	478	if (t == 0) {
725	giacomo	479
732	giacomo	480	res = r[0];
381	giacomo	481
732	giacomo	482	return (((unsigned char)(res))TICK_LEN_PERIOD/10008);
358	giacomo	483
732	giacomo	484	} else return -1;
		485
		486	} else return -1;
		487
281	giacomo	488	}
		489
289	giacomo	490	/* 1000.1000 */
358	giacomo	491	int servo_store_period(int port)
281	giacomo	492	{
724	giacomo	493	unsigned char b[1];
358	giacomo	494	int servo_port = (unsigned)(port);
		495
724	giacomo	496	b[0] = 0x88;
		497	servo_send_msg(servo_port, b, 1, NULL, 0);
358	giacomo	498
724	giacomo	499	return 0;
381	giacomo	500
281	giacomo	501	}
		502
283	giacomo	503	/* 1000.1100 */
358	giacomo	504	int servo_get_setup_switch(int port)
281	giacomo	505	{
724	giacomo	506	unsigned char b[1],r[1];
733	giacomo	507	int res, RX, t;
358	giacomo	508	int servo_port = (unsigned)(port);
		509
724	giacomo	510	b[0] = 0x8C;
725	giacomo	511	RX = servo_send_msg(servo_port, b, 1, r, 1);
281	giacomo	512
732	giacomo	513	if (RX != -1) {
		514
733	giacomo	515	t = servo_wait(servo_port, RX);
732	giacomo	516
733	giacomo	517	if (t == 0) {
725	giacomo	518
732	giacomo	519	res = r[0];
281	giacomo	520
732	giacomo	521	return res;
283	giacomo	522
732	giacomo	523	} else return -1;
		524
		525	} else return -1;
		526
281	giacomo	527	}
		528
283	giacomo	529	/* 1000.111s */
358	giacomo	530	int servo_set_RC5_switch(int port, int data)
281	giacomo	531	{
724	giacomo	532	unsigned char b[1];
358	giacomo	533	int servo_port = (unsigned)(port);
		534
724	giacomo	535	b[0] = 0x8E \| (data & 0x01);
		536	servo_send_msg(servo_port, b, 1, NULL, 0);
281	giacomo	537
724	giacomo	538	return 0;
381	giacomo	539
281	giacomo	540	}
		541
323	giacomo	542	/* 1000.0000:0000.Mmmm */
358	giacomo	543	int servo_turn_off(int port, int servo)
285	giacomo	544	{
724	giacomo	545	unsigned char b[2];
358	giacomo	546	int servo_port = (unsigned)(port);
		547
		548	if (servo > 15) return -1;
724	giacomo	549
		550	b[0] = 0x80;
		551	b[1] = 0x00 \| (servo & 0x0F);
		552	servo_send_msg(servo_port, b, 2, NULL, 0);
381	giacomo	553
724	giacomo	554	return 0;
285	giacomo	555
		556	}
		557
325	giacomo	558	/* 1000.0000:0001.Mmmm */
358	giacomo	559	int servo_turn_on(int port, int servo)
285	giacomo	560	{
721	giacomo	561	unsigned char b[2];
358	giacomo	562	int servo_port = (unsigned)(port);
		563
323	giacomo	564	if (servo > 15) return -1;
358	giacomo	565
721	giacomo	566	b[0] = 0x80;
		567	b[1] = 0x10 \| (servo & 0x0F);
		568	servo_send_msg(servo_port, b, 2, NULL, 0);
358	giacomo	569
721	giacomo	570	return 0;
358	giacomo	571
285	giacomo	572	}
		573
291	giacomo	574	/* 1000.0000:0010.0000 */
358	giacomo	575	int servo_turn_off_all(int port)
323	giacomo	576	{
724	giacomo	577	unsigned char b[2];
358	giacomo	578	int servo_port = (unsigned)(port);
		579
724	giacomo	580	b[0] = 0x80;
		581	b[1] = 0x20;
		582	servo_send_msg(servo_port, b, 2, NULL, 0);
358	giacomo	583
724	giacomo	584	return 0;
358	giacomo	585
285	giacomo	586	}
		587
291	giacomo	588	/* 1000.0000:0010.0001 */
358	giacomo	589	int servo_turn_on_all(int port)
285	giacomo	590	{
724	giacomo	591	unsigned char b[2];
358	giacomo	592	int servo_port = (unsigned)(port);
		593
724	giacomo	594	b[0] = 0x80;
		595	b[1] = 0x21;
		596	servo_send_msg(servo_port, b, 2, NULL, 0);
		597
		598	return 0;
358	giacomo	599
285	giacomo	600	}
		601
323	giacomo	602	/* 1000.0000:0101.000M:mmmm.mmmm */
724	giacomo	603	int servo_set_levels(int port, int bank, int mask)
315	giacomo	604	{
724	giacomo	605	unsigned char b[3];
358	giacomo	606	int servo_port = (unsigned)(port);
		607
724	giacomo	608	b[0] = 0x80;
		609	b[1] = 0x50 \| (0x01 & bank);
		610	b[2] = (unsigned char)(mask & 0xFF);
		611	servo_send_msg(servo_port, b, 3, NULL, 0);
358	giacomo	612
724	giacomo	613	return 0;
358	giacomo	614
315	giacomo	615	}
		616
323	giacomo	617	/* 1000.0000:0100.000M */
358	giacomo	618	int servo_get_levels(int port, int bank)
297	giacomo	619	{
724	giacomo	620	unsigned char b[2],r[1];
733	giacomo	621	int res, RX, t;
358	giacomo	622	int servo_port = (unsigned)(port);
		623
724	giacomo	624	b[0] = 0x80;
		625	b[1] = 0x40 \| (0x01 & bank);
725	giacomo	626	RX = servo_send_msg(servo_port, b, 2, r, 1);
		627
732	giacomo	628	if (RX != -1) {
		629
733	giacomo	630	t = servo_wait(servo_port, RX);
732	giacomo	631
733	giacomo	632	if (t == 0) {
358	giacomo	633
732	giacomo	634	res = r[0];
358	giacomo	635
732	giacomo	636	return res;
		637
		638	} else return -1;
		639
		640	} else return -1;
		641
297	giacomo	642	}
		643
		644	/* 1000.0000:1000.0000 */
358	giacomo	645	int servo_store_levels(int port)
297	giacomo	646	{
724	giacomo	647	unsigned char b[2];
358	giacomo	648	int servo_port = (unsigned)(port);
		649
724	giacomo	650	b[0] = 0x80;
		651	b[1] = 0x80;
		652	servo_send_msg(servo_port, b, 2, NULL, 0);
381	giacomo	653
724	giacomo	654	return 0;
358	giacomo	655
297	giacomo	656	}
		657
493	giacomo	658	int servo_set_max_angle_sec(int port, int servo, int angle_sec)
290	giacomo	659	{
		660
381	giacomo	661	servo_table[port][servo].max_angle_sec = angle_sec;
290	giacomo	662	return 0;
		663
		664	}
		665
493	giacomo	666	int servo_set_min_angle_sec(int port, int servo, int angle_sec)
290	giacomo	667	{
		668
381	giacomo	669	servo_table[port][servo].min_angle_sec = angle_sec;
290	giacomo	670	return 0;
		671
		672	}
		673
381	giacomo	674	int servo_set_servo_tick(int port, int servo, int zero_tick, int delta_tick)
		675	{
		676
		677	if (zero_tick != -1) servo_table[port][servo].zero_tick = zero_tick;
		678	if (delta_tick != -1) servo_table[port][servo].delta_tick = delta_tick;
		679	return 0;
		680
		681	}
		682
323	giacomo	683	/* 0000.Pppp:0000.vvvv:vvvv.vvvv */
358	giacomo	684	int servo_set_angle_sec(int port, int servo, int angle_sec)
281	giacomo	685	{
717	giacomo	686	unsigned char b[3];
730	giacomo	687	int angle_tick, res = 0;
358	giacomo	688	int servo_port = (unsigned)(port);
282	giacomo	689
323	giacomo	690	if (servo > 15) return -1;
282	giacomo	691
381	giacomo	692	angle_tick = (servo_table[port][servo].zero_tick + angle_sec *
		693	servo_table[port][servo].delta_tick /
542	giacomo	694	(servo_table[port][servo].max_angle_sec - servo_table[port][servo].min_angle_sec)) * 1000 / TICK_LEN;
290	giacomo	695
717	giacomo	696	b[0] = 0x00 \| (servo & 0x0F);
		697	b[1] = 0x00 \| ((angle_tick >> 8) & 0x0F);
		698	b[2] = 0x00 \| (angle_tick & 0xFF);
730	giacomo	699	res = servo_send_msg(servo_port, b, 3, NULL, 0);
358	giacomo	700
730	giacomo	701	return res;
282	giacomo	702
281	giacomo	703	}
		704
323	giacomo	705	/* 0010.Pppp */
358	giacomo	706	int servo_store_default_position(int port, int servo)
290	giacomo	707	{
724	giacomo	708	unsigned char b[1];
730	giacomo	709	int res = 0;
358	giacomo	710	int servo_port = (unsigned)(port);
290	giacomo	711
323	giacomo	712	if (servo > 15) return -1;
358	giacomo	713
724	giacomo	714	b[0] = 0x20 \| (servo & 0x0F);
730	giacomo	715	res = servo_send_msg(servo_port, b, 1, NULL, 0);
358	giacomo	716
730	giacomo	717	return res;
358	giacomo	718
290	giacomo	719	}
		720
323	giacomo	721	/* 0001.Pppp */
358	giacomo	722	int servo_get_angle_sec(int port, int servo)
281	giacomo	723	{
724	giacomo	724	unsigned char b[1],r[2];
733	giacomo	725	int res,data,RX,t;
358	giacomo	726	int servo_port = (unsigned)(port);
		727
323	giacomo	728	if (servo > 15) return -1;
358	giacomo	729
724	giacomo	730	b[0] = 0x10 \| (servo & 0x0F);
725	giacomo	731	RX = servo_send_msg(servo_port, b, 1, r, 2);
281	giacomo	732
730	giacomo	733	if (RX != -1) {
725	giacomo	734
733	giacomo	735	t = servo_wait(servo_port, RX);
282	giacomo	736
733	giacomo	737	if (t == 0) {
290	giacomo	738
730	giacomo	739	res = (int)(r[0]) << 8;
		740	res \|= r[1];
282	giacomo	741
730	giacomo	742	data = ((resTICK_LEN/1000) - servo_table[port][servo].zero_tick)
		743	(servo_table[port][servo].max_angle_sec - servo_table[port][servo].min_angle_sec) /
		744	servo_table[port][servo].delta_tick;
		745
		746	return data;
		747
		748	} else return -1;
		749
		750	} else return -1;
		751
281	giacomo	752	}
		753
282	giacomo	754	/* 0100:0aaa */
358	giacomo	755	int servo_get_analog(int port, int adport)
281	giacomo	756	{
724	giacomo	757	unsigned char b[1],r[2];
733	giacomo	758	int res, RX, t;
358	giacomo	759	int servo_port = (unsigned)(port);
		760
381	giacomo	761	if (adport > 7) return -1;
358	giacomo	762
724	giacomo	763	b[0] = 0x40 \| (adport & 0x07);
725	giacomo	764	RX = servo_send_msg(servo_port, b, 1, r, 2);
281	giacomo	765
732	giacomo	766	if (RX != -1) {
725	giacomo	767
733	giacomo	768	t = servo_wait(servo_port, RX);
282	giacomo	769
733	giacomo	770	if (t == 0) {
282	giacomo	771
732	giacomo	772	res = (int)r[0] << 8;
		773	res \|= r[1];
		774
		775	return res;
		776
		777	} else return -1;
		778
		779	} else return -1;
		780
281	giacomo	781	}
		782

Subversion Repositories shark

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