WebSVN - shark - Blame - Rev 1333 - /demos/trunk/chimera/send.c

Rev	Author	Line No.	Line
1331	giacomo	1
		2	/*
		3	* Project: S.Ha.R.K.
		4	*
		5	* Coordinators: Giorgio Buttazzo <giorgio@sssup.it>
		6	*
		7	* Authors : Paolo Gai <pj@hartik.sssup.it>
		8	* (see authors.txt for full list of hartik's authors)
		9	*
		10	* ReTiS Lab (Scuola Superiore S.Anna - Pisa - Italy)
		11	*
		12	* http://www.sssup.it
		13	* http://retis.sssup.it
		14	* http://shark.sssup.it
		15	*/
		16
		17	/*
		18	* Copyright (C) 2000 Paolo Gai
		19	*
		20	* This program is free software; you can redistribute it and/or modify
		21	* it under the terms of the GNU General Public License as published by
		22	* the Free Software Foundation; either version 2 of the License, or
		23	* (at your option) any later version.
		24	*
		25	* This program is distributed in the hope that it will be useful,
		26	* but WITHOUT ANY WARRANTY; without even the implied warranty of
		27	* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
		28	* GNU General Public License for more details.
		29	*
		30	* You should have received a copy of the GNU General Public License
		31	* along with this program; if not, write to the Free Software
		32	* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
		33	*
		34	*/
		35
		36	#include "chimera.h"
		37
		38	//#define DEBUG_SEND /* Print Sent Values */
		39	#define SERIAL_ON /* Send Data using Serial Port */
		40
		41	/* Servo Tasks Constants */
		42	#ifdef DUBUG_SEND
		43	#define SEND_TASK_WCET 6000
		44	#else
1332	giacomo	45	#define SEND_TASK_WCET 6000
1331	giacomo	46	#endif
		47	#define SEND_TASK_MIT 20000
		48
		49	#ifdef DUBUG_SEND
1332	giacomo	50	#define GET_TASK_WCET 6000
1331	giacomo	51	#else
1332	giacomo	52	#define GET_TASK_WCET 6000
1331	giacomo	53	#endif
		54	#define GET_TASK_MIT 20000
		55
		56	#define LEG_A 100.0
		57	#define LEG_B 66.0
		58	#define LEG_C 26.0
		59	#define LEG_D 38.0
		60	#define LEG_CD_2IPO 92.087 /* 2 * sqrt(LEG_C^2 + LEG_D^2) */
		61	#define LEG_CD_ANG 34.380 /* arctg(LEG_C/LEG_D) in gradi */
		62
		63	const float c0 = LEG_C * LEG_C;
		64	const float c1 = LEG_B * LEG_B;
		65	const float c2 = LEG_B * LEG_B - LEG_A * LEG_A;
		66	const float todeg = 180.0 / PI;
		67
		68	sem_t mx_status, mx_servo;
		69	HEXAPOD_STATE status;
		70	ANGLES_STATE angles[6];
		71
1332	giacomo	72	extern struct leg_calibration calibration_table[];
		73
1331	giacomo	74	void print_status(int n){
		75	int i;
		76
		77	if (n>=6) {
		78	for (i=0; i<6;i++)
1333	giacomo	79	cprintf("Leg %1d: ( %3d \| %3d \| %3d ) (%d)\n", i, status.leg[i].x, status.leg[i].y, status.leg[i].z, status.cfg[i].pwm );
1331	giacomo	80	cprintf("Power: %d", status.power);
		81	cprintf("\n");
		82	} else {
1333	giacomo	83	cprintf("Leg %1d: ( %3d \| %3d \| %3d ) (%1d-%1d)\n", n, status.leg[n].x,status.leg[n].y,status.leg[n].z, status.cfg[n].pwm, status.power);
1331	giacomo	84	}
		85	}
		86
		87	int set_leg_position(int n, float px, float py, float pz)
		88	{
		89	float px2 = px * px;
		90	float py2 = py * py;
		91	float pz2 = pz * pz;
		92
		93	float pxz2 = px2 + pz2;
		94
		95	float alfa1,beta1,alfa2,beta2,gamma1;
		96	float m,dsqrt;
		97
		98	float delta_xz = pxz2 - c0;
		99	float s,k,k2,y1,delta_xy;
		100
		101	if (delta_xz < 0.0) return -1;
		102
		103	if (pz >= LEG_C) {
		104	gamma1 = acos((pz * LEG_C + px * sqrt(delta_xz)) / pxz2) * todeg;
		105	} else {
		106	gamma1 = -acos((pz * LEG_C + px * sqrt(delta_xz)) / pxz2) * todeg;
		107	}
		108
		109	if (gamma1 < -90.0 \|\| gamma1 > 90.0) return -1;
		110
		111	m = pxz2 - LEG_CD_2IPO * (sin(gamma1+LEG_CD_ANG)+cos(gamma1+LEG_CD_ANG));
		112
		113	s = m + py2;
		114	k = c2 + s;
		115	k2 = k * k;
		116
		117	delta_xy = py2 * k2 - s * (k2 - 4.0 * m * c1);
		118
		119	if (delta_xy >= 0.0) {
		120	dsqrt = sqrt(delta_xy);
		121	y1 = (py * k + dsqrt) / (2.0 * s);
		122	beta1 = asin(y1/LEG_B) * todeg;
		123	alfa1 = asin((y1 - py)/LEG_A) * todeg + beta1;
		124	y1 = (py * k - dsqrt) / (2.0 * s);
		125	beta2 = asin(y1/LEG_B) * todeg;
		126	alfa2 = asin((y1 - py)/LEG_A) * todeg + beta2;
		127
		128	if ((alfa1 >= 0.0 && alfa1 <= 180.0) && (beta1 >= -90.0 && beta1 <= 90.0)) {
		129	angles[n].a = (int)(alfa1 * 3600.0);
		130	angles[n].b = (int)(beta1 * 3600.0);
		131	angles[n].c = (int)(gamma1 * 3600.0);
		132	#ifdef DEBUG_SEND
		133	cprintf("Leg %d: ( %3d \| %3d \| %3d )\n", n, angles[n].a, angles[n].b, angles[n].c);
		134	#endif
		135	return 0;
		136	} else if ((alfa2 >= 0.0 && alfa2 <= 180.0) && (beta2 >= -90.0 && beta2 <= 90.0)) {
		137	angles[n].a = (int)(alfa2 * 3600.0);
		138	angles[n].b = (int)(beta2 * 3600.0);
		139	angles[n].c = (int)(gamma1 * 3600.0);
		140	#ifdef DEBUG_SEND
		141	cprintf("Leg %d: ( %3d \| %3d \| %3d )\n", n, angles[n].a, angles[n].b, angles[n].c);
		142	#endif
		143	return 0;
		144	} else {
		145	#ifdef DEBUG_SEND
		146	cprintf("No possible !\n");
		147	#endif
		148	return 1;
		149	}
		150	} else
		151	return 1;
		152
		153	return 0;
		154	}
		155
1333	giacomo	156	void update_event_action(void) {
		157
		158	struct timespec t;
		159	struct action_event *e;
		160	int i;
		161
		162	kern_gettime(&t);
		163
		164	while ((e = get_first_old_event(&t)) != NULL) {
		165
		166	if (e->type == EVT_SET_MASK_LEG_POS) {
		167
		168	e->status = EVT_STATUS_DONE;
		169
		170	}
		171
		172	if (e->type == EVT_SET_MASK_LEG_ANGLE) {
		173
		174	for (i=0;i<6;i++)
		175	if ((1 << i) & e->mask) {
		176
		177	status.ang[i].a = angles[i].a = (int )(e->data);
		178	status.ang[i].b = angles[i].b = (int )(e->data+4);
		179	status.ang[i].c = angles[i].c = (int )(e->data+8);
		180	status.cfg[i].pwm = (unsigned char )(e->data+16);
		181
		182	//#ifdef DEBUG_SEND
		183	cprintf("Update leg %d angle\n",i);
		184	//#endif
		185
		186
		187	}
		188
		189	e->status = EVT_STATUS_DONE;
		190
		191	}
		192
		193	}
		194
		195	}
		196
1331	giacomo	197	TASK servo_send()
		198	{
		199	HEXAPOD_STATE old_status;
		200	register char changes, new_pos, new_pwm, new_power;
		201	int n;
		202
		203	for (n=0; n<6;n++) {
		204	old_status.leg[n].x = 150;
1333	giacomo	205	old_status.leg[n].y = 0;
		206	old_status.leg[n].z = 0;
		207	old_status.ang[n].a = 0;
		208	old_status.ang[n].b = 0;
		209	old_status.ang[n].c = 0;
		210	old_status.cfg[n].pwm = 0;
1331	giacomo	211	}
		212	old_status.power = 0;
		213
		214	while (1) {
		215	changes = 0;
1333	giacomo	216
		217	update_event_action();
1331	giacomo	218
		219	for (n=0; n<6; n++){
		220	new_pos = 0;
		221	new_pwm = 0;
		222	new_power = 0;
		223
1333	giacomo	224	sem_wait(&mx_status);
1331	giacomo	225	if ((status.leg[n].x != old_status.leg[n].x) \|\| (status.leg[n].y != old_status.leg[n].y) \|\| (status.leg[n].z != old_status.leg[n].z)) {
		226	if (set_leg_position(n, status.leg[n].x, status.leg[n].y, status.leg[n].z)==0) {
		227	old_status.leg[n].x = status.leg[n].x;
		228	old_status.leg[n].y = status.leg[n].y;
		229	old_status.leg[n].z = status.leg[n].z;
		230	new_pos++;
		231	} else {
		232	#ifdef DEBUG_SEND
		233	cprintf("Position not reachable.\n");
		234	#endif
		235	status.leg[n].x = old_status.leg[n].x;
		236	status.leg[n].y = old_status.leg[n].y;
		237	status.leg[n].z = old_status.leg[n].z;
		238	}
		239	}
1333	giacomo	240
		241	if ((status.ang[n].a != old_status.ang[n].a) \|\|
		242	(status.ang[n].b != old_status.ang[n].b) \|\|
		243	(status.ang[n].c != old_status.ang[n].c)) {
		244
		245	old_status.ang[n].a = status.ang[n].a;
		246	old_status.ang[n].b = status.ang[n].b;
		247	old_status.ang[n].c = status.ang[n].c;
		248	new_pos++;
		249	}
		250
		251	if (status.cfg[n].pwm != old_status.cfg[n].pwm) {
		252	old_status.cfg[n].pwm = status.cfg[n].pwm;
1331	giacomo	253	new_pwm++;
		254	}
		255	if (status.power != old_status.power) {
		256	old_status.power = status.power;
		257	new_power++;
		258	}
1333	giacomo	259	sem_post(&mx_status);
1331	giacomo	260
		261	if (new_pos) {
		262	#ifdef SERIAL_ON
		263	sem_wait(&mx_servo);
1332	giacomo	264	servo_set_angle_sec(COM_PORT, n*3+2, adjust(angles[n].a,n,0));
		265	servo_set_angle_sec(COM_PORT, n*3+1, adjust(angles[n].b,n,1));
		266	servo_set_angle_sec(COM_PORT, n*3 , adjust(angles[n].c,n,2));
1331	giacomo	267	sem_post(&mx_servo);
1332	giacomo	268
		269	printf_xy(3,3,WHITE,"PX = %05d PY = %05d PZ = %05d",(int)(status.leg[n].x)
		270	,(int)(status.leg[n].y)
		271	,(int)(status.leg[n].z));
		272
1333	giacomo	273	printf_xy(3,4,WHITE,"ALFA = %07d BETA = %07d GAMMA = %07d",(int)(angles[n].a/3600)
		274	,(int)(angles[n].b/3600)
		275	,(int)(angles[n].c/3600));
1332	giacomo	276
1333	giacomo	277	printf_xy(3,5,WHITE,"ADJ_ALFA = %07d ADJ_BETA = %07d ADJ_GAMMA = %07d",adjust(angles[n].a,n,0)/3600,adjust(angles[n].b,n,1)/3600,adjust(angles[n].c,n,2)/3600);
1332	giacomo	278
1331	giacomo	279	#endif
		280	}
		281
		282	if (new_pwm) {
		283	#ifdef SERIAL_ON
		284	sem_wait(&mx_servo);
1333	giacomo	285	(old_status.cfg[n].pwm & 1) ? servo_turn_on(COM_PORT, n3+2) : servo_turn_off(COM_PORT, n3+2);
		286	(old_status.cfg[n].pwm & 2) ? servo_turn_on(COM_PORT, n3+1) : servo_turn_off(COM_PORT, n3+1);
		287	(old_status.cfg[n].pwm & 4) ? servo_turn_on(COM_PORT, n3 ) : servo_turn_off(COM_PORT, n3 );
1331	giacomo	288	sem_post(&mx_servo);
		289	#endif
		290	}
		291	if (new_power) {
		292	#ifdef SERIAL_ON
		293	if (old_status.power) {
		294	sem_wait(&mx_servo);
		295	servo_set_RC5_switch(COM_PORT, 1);
		296	sem_post(&mx_servo);
		297	} else {
		298	sem_wait(&mx_servo);
		299	servo_set_RC5_switch(COM_PORT, 0);
		300	sem_post(&mx_servo);
		301	}
		302	#endif
		303	}
		304
		305	if (new_pos \|\| new_pwm \|\| new_power) {
		306	changes++;
		307	#ifdef DEBUG_SEND
		308	print_status(n);
		309	#endif
		310	}
		311	}
		312	task_endcycle();
		313	}
		314
		315	return 0;
		316	}
		317
		318	TASK servo_get()
		319	{
		320	int i = 0;
		321
		322	while (1) {
		323	#ifdef SERIAL_ON
		324	//sem_wait(&mx_status);
		325	sem_wait(&mx_servo);
1333	giacomo	326	status.cfg[i ].adc_in = servo_get_analog(COM_PORT, i );
		327	status.cfg[i+1].adc_in = servo_get_analog(COM_PORT, i+1);
1331	giacomo	328	sem_post(&mx_servo);
		329	#ifdef DEBUG_SEND
		330	cprintf("Leg %1d-%1d: (%4d) (%4d)\n", i, i+1, status.leg[i].adc_in, status.leg[i+1].adc_in);
		331	#endif
		332	i = (i+2)%6;
		333	//sem_post(&mx_status);
		334	#endif
		335	task_endcycle();
		336	}
		337
		338	return 0;
		339	}
		340
		341	int init_serial()
		342	{
		343	int err;
1332	giacomo	344	err = servo_open(COM_PORT, 19200);
1331	giacomo	345
		346	return err;
		347	}
		348
		349	void end_serial()
		350	{
		351	servo_close(COM_PORT);
		352	}
		353
		354	void init_send_task()
		355	{
1332	giacomo	356	SOFT_TASK_MODEL ms;
1331	giacomo	357	PID pid;
		358
1332	giacomo	359	soft_task_default_model(ms);
		360	soft_task_def_ctrl_jet(ms);
		361	soft_task_def_met(ms, SEND_TASK_WCET);
		362	soft_task_def_period(ms, SEND_TASK_MIT);
		363	soft_task_def_usemath(ms);
1331	giacomo	364	pid = task_create("Send_Task", servo_send, &ms, NULL);
		365	if (pid == NIL) {
		366	perror("Could not create task <Send_Task>");
		367	sys_end();
		368	} else
		369	task_activate(pid);
		370
1332	giacomo	371	soft_task_default_model(ms);
		372	soft_task_def_ctrl_jet(ms);
		373	soft_task_def_met(ms, GET_TASK_WCET);
		374	soft_task_def_period(ms, GET_TASK_MIT);
		375	soft_task_def_usemath(ms);
1331	giacomo	376	pid = task_create("Get_Task", servo_get, &ms, NULL);
		377	if (pid == NIL) {
		378	perror("Could not create task <Get_Task>");
		379	sys_end();
		380	} else
		381	task_activate(pid);
		382	}
		383
		384	void init_send()
		385	{
		386	int i;
		387
		388	if (init_serial()) {
		389	perror("Could not initialize serial port.");
		390	sys_end();
		391	}
		392
1333	giacomo	393	sem_init(&mx_status,0,1);
1331	giacomo	394	sem_init(&mx_servo,0,1);
		395
		396	for (i=0; i<6;i++) {
1333	giacomo	397	status.ang[i].a = 0;
		398	status.ang[i].b = 0;
		399	status.ang[i].c = 0;
1331	giacomo	400	status.leg[i].x = 150;
		401	status.leg[i].y = 0;
		402	status.leg[i].z = 0;
1333	giacomo	403	status.cfg[i].pwm = 0;
1331	giacomo	404	}
		405	status.power = 0;
		406
		407	init_send_task();
		408	}
		409
		410	void end_send()
		411	{
		412	end_serial();
		413	}

Subversion Repositories shark

(root)/demos/trunk/chimera/send.c @ 1338 - Rev 1333