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

Subversion Repositories shark

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