WebSVN - shark - Blame - Rev 1650 - /advdemos/trunk/chimera/send.c

Rev	Author	Line No.	Line
1624	giacomo	1	/*
		2	* Project: S.Ha.R.K.
		3	*
		4	* Coordinators: Giorgio Buttazzo <giorgio@sssup.it>
		5	*
		6	* ReTiS Lab (Scuola Superiore S.Anna - Pisa - Italy)
		7	*
		8	* http://www.sssup.it
		9	* http://retis.sssup.it
		10	* http://shark.sssup.it
		11	*/
		12
		13	#include "chimera.h"
		14
		15	//#define DEBUG_SEND /* Print Sent Values */
		16	#define SERIAL_ON /* Send Data using Serial Port */
		17
		18	/* Servo Tasks Constants */
		19	#ifdef DUBUG_SEND
1647	giacomo	20	#define SEND_TASK_WCET 10000
1624	giacomo	21	#else
1647	giacomo	22	#define SEND_TASK_WCET 10000
1624	giacomo	23	#endif
1646	giacomo	24	#define SEND_TASK_MIT 30000
1624	giacomo	25
		26	#define LEG_A 100.0
		27	#define LEG_B 66.0
		28	#define LEG_C 26.0
		29	#define LEG_D 38.0
		30	#define LEG_CD_2IPO 92.087 /* 2 * sqrt(LEG_C^2 + LEG_D^2) */
		31	#define LEG_CD_ANG 0.600 /* arctg(LEG_C/LEG_D) in radianti */
		32
		33	const float c0 = LEG_C * LEG_C;
		34	const float c1 = LEG_B * LEG_B;
		35	const float c2 = LEG_B * LEG_B - LEG_A * LEG_A;
		36	const float todeg = 180.0 / PI;
		37	const float torad = PI / 180.0;
		38
		39	HEXAPOD_STATE status;
		40
		41	extern struct leg_calibration calibration_table[];
		42
		43	int leg_to_ang(float px, float py, float pz, int alfa, int beta, int *gamma)
		44	{
		45	float px2 = px * px;
		46	float py2 = py * py;
		47	float pz2 = pz * pz;
		48
		49	float pxz2 = px2 + pz2;
		50
		51	float alfa1,beta1,alfa2,beta2,gamma1,gamma2;
		52	float m,dsqrt;
		53
		54	float delta_xz = pxz2 - c0;
		55	float s,k,k2,y1,delta_xy;
		56
		57	if (delta_xz < 0.0) return -1;
		58
		59	if (pz >= LEG_C) {
		60	gamma2 = acos((pz * LEG_C + px * sqrt(delta_xz)) / pxz2);
		61	gamma1 = gamma2 * todeg;
		62	} else {
		63	gamma2 = -acos((pz * LEG_C + px * sqrt(delta_xz)) / pxz2);
		64	gamma1 = gamma2 * todeg;
		65	}
		66
		67	m = pxz2 - LEG_CD_2IPO * (pz * sin(gamma2+LEG_CD_ANG) + px * cos(gamma2+LEG_CD_ANG) - LEG_CD_2IPO / 4.0);
		68
		69	s = m + py2;
		70	k = c2 + s;
		71	k2 = k * k;
		72
		73	delta_xy = py2 * k2 - s * (k2 - 4.0 * m * c1);
		74
		75	if (delta_xy >= 0.0) {
		76	dsqrt = sqrt(delta_xy);
		77	y1 = (py * k + dsqrt) / (2.0 * s);
		78	beta1 = asin(y1/LEG_B) * todeg;
		79	alfa1 = asin((y1 - py)/LEG_A) * todeg + beta1;
		80	y1 = (py * k - dsqrt) / (2.0 * s);
		81	beta2 = asin(y1/LEG_B) * todeg;
		82	alfa2 = asin((y1 - py)/LEG_A) * todeg + beta2;
		83
		84	if ((alfa1 >= 0.0 && alfa1 <= 180.0) && (beta1 >= -90.0 && beta1 <= 90.0)) {
		85	alfa = (int)(alfa1 3600.0);
		86	beta = (int)(beta1 3600.0);
		87	gamma = (int)(gamma1 3600.0);
		88	return 0;
		89	} else if ((alfa2 >= 0.0 && alfa2 <= 180.0) && (beta2 >= -90.0 && beta2 <= 90.0)) {
		90	alfa = (int)(alfa2 3600.0);
		91	beta = (int)(beta2 3600.0);
		92	gamma = (int)(gamma1 3600.0);
		93	return 0;
		94	} else {
		95	return -1;
		96	}
		97	} else
		98	return -1;
		99
		100	return -1;
		101
		102	}
		103
		104	int ang_to_leg(int alfa, int beta, int gamma, float px, float py, float *pz) {
		105
		106	float alfa1 = (float)(alfa)/3600.0 * torad;
		107	float beta1 = (float)(beta)/3600.0 * torad;
		108	float sin_gamma = sin((float)(gamma)/3600.0 * torad);
		109	float cos_gamma = cos((float)(gamma)/3600.0 * torad);
		110	float m;
		111
		112	m = LEG_B * cos(beta1) + LEG_A * cos(alfa1 - beta1);
		113	py = LEG_B sin(beta1) - LEG_A * sin(alfa1 - beta1);
		114
		115	pz = (LEG_D + m) sin_gamma + LEG_C * cos_gamma;
		116	px = (LEG_D + m) cos_gamma - LEG_C * sin_gamma;
		117
		118	return 0;
		119
		120	}
		121
		122	void update_event_action(void) {
		123
		124	struct timespec t;
		125	struct action_event *e;
		126	int i;
		127
		128	kern_gettime(&t);
		129
		130	while ((e = get_first_old_event(&t)) != NULL) {
		131
		132	if (e->type == EVT_SET_MASK_LEG_ANGLE) {
		133
		134	for (i=0;i<6;i++)
		135	if ((e->mask >> i) & 1) {
		136
		137	status.ang[i].a = e->ang.a;
		138	status.ang[i].b = e->ang.b;
		139	status.ang[i].c = e->ang.c;
		140
		141	status.cfg[i].pwm = e->pwm;
		142
		143	#ifdef DEBUG_SEND
		144	printf_xy(3,2,WHITE,"%8d: Update leg %2d angle",(int)kern_gettime(NULL),i);
		145	#endif
		146
		147
		148	}
		149
		150	e->status = EVT_STATUS_DONE;
		151
		152	}
		153
		154	}
		155
		156	}
		157
		158	TASK servo_send()
		159	{
		160	HEXAPOD_STATE old_status;
		161	register char new_pos, new_pwm, new_power;
		162	int res,n;
		163
		164	for (n=0; n<6;n++) {
		165	old_status.ang[n].a = 0;
		166	old_status.ang[n].b = 0;
		167	old_status.ang[n].c = 0;
		168	old_status.cfg[n].pwm = 0;
		169	}
		170	old_status.power = 0;
		171
		172	while (1) {
		173	new_pos = 0;
		174	new_pwm = 0;
		175	new_power = 0;
		176
		177	update_event_action();
		178
		179	if (status.power != old_status.power) {
		180	#ifdef SERIAL_ON
		181	if (old_status.power) {
		182	servo_set_RC5_switch(COM2, 1);
		183	} else {
		184	servo_set_RC5_switch(COM2, 0);
		185	}
		186	old_status.power = status.power;
		187	#endif
		188	}
		189
		190	for (n=0; n<6; n++){
		191
		192	if ((status.ang[n].a != old_status.ang[n].a) \|\|
		193	(status.ang[n].b != old_status.ang[n].b) \|\|
		194	(status.ang[n].c != old_status.ang[n].c)) {
		195
		196	old_status.ang[n].a = status.ang[n].a;
		197	old_status.ang[n].b = status.ang[n].b;
		198	old_status.ang[n].c = status.ang[n].c;
		199	new_pos += 1 << n;
		200	}
		201
		202	if (status.cfg[n].pwm != old_status.cfg[n].pwm) {
		203	old_status.cfg[n].pwm = status.cfg[n].pwm;
		204	new_pwm += 1 << n;
		205	}
		206
		207	if (new_pos && (1<<n)) {
		208	#ifdef SERIAL_ON
		209	res = servo_set_angle_sec(com(n3 ), pin(n3 ), adjust(status.ang[n].a,n,0));
		210	if (res != 0) cprintf("Error send data\n");
1637	giacomo	211
1624	giacomo	212	res = servo_set_angle_sec(com(n3+1), pin(n3+1), adjust(status.ang[n].b,n,1));
		213	if (res != 0) cprintf("Error send data\n");
1637	giacomo	214
1624	giacomo	215	res = servo_set_angle_sec(com(n3+2), pin(n3+2), adjust(status.ang[n].c,n,2));
		216	if (res != 0) cprintf("Error send data\n");
		217	#endif
		218
		219	}
		220
		221	if (new_pwm && (1<<n)) {
		222	#ifdef SERIAL_ON
		223	(old_status.cfg[n].pwm & 1) ? servo_turn_on(com(n3 ), pin(n3 )) : servo_turn_off(com(n3 ), pin(n3 ));
1637	giacomo	224
1624	giacomo	225	(old_status.cfg[n].pwm & 2) ? servo_turn_on(com(n3+1), pin(n3+1)) : servo_turn_off(com(n3+1), pin(n3+1));
1637	giacomo	226
1624	giacomo	227	(old_status.cfg[n].pwm & 4) ? servo_turn_on(com(n3+2), pin(n3+2)) : servo_turn_off(com(n3+2), pin(n3+2));
1645	mauro	228	#endif
1637	giacomo	229
1624	giacomo	230	}
1646	giacomo	231
1624	giacomo	232	}
		233
1637	giacomo	234	task_endcycle();
1624	giacomo	235	task_testcancel();
		236	}
		237
		238	return 0;
		239	}
		240
		241	int init_serial()
		242	{
		243	int err;
		244	err = servo_open(COM1, COM_SPEED);
		245	if (!err)
		246	err = servo_open(COM2, COM_SPEED);
		247
		248	return err;
		249	}
		250
		251	void end_serial()
		252	{
		253	servo_close(COM1);
		254	servo_close(COM2);
		255	}
		256
		257	PID servo_pid;
		258
		259	void init_send_task()
		260	{
		261	HARD_TASK_MODEL ms;
		262
		263	hard_task_default_model(ms);
		264	hard_task_def_ctrl_jet(ms);
		265	hard_task_def_wcet(ms, SEND_TASK_WCET);
		266	hard_task_def_mit(ms, SEND_TASK_MIT);
		267	hard_task_def_usemath(ms);
		268	servo_pid = task_create("Servo_Task", servo_send, &ms, NULL);
		269	if (servo_pid == NIL) {
		270	perror("Could not create task <Send_Task>");
1650	pj	271	exit(1);
1624	giacomo	272	} else
		273	task_activate(servo_pid);
		274
		275	}
		276
		277	void init_send()
		278	{
		279	int i;
		280
		281	if (init_serial()) {
		282	perror("Could not initialize serial port.");
1650	pj	283	exit(1);
1624	giacomo	284	}
		285
		286	for (i=0; i<6;i++) {
		287	status.ang[i].a = 0;
		288	status.ang[i].b = 0;
		289	status.ang[i].c = 0;
		290	status.cfg[i].pwm = 0;
		291	}
		292	status.power = 0;
		293
		294	init_send_task();
		295
		296	}
		297
		298	void end_send()
		299	{
		300	task_kill(servo_pid);
		301
		302	end_serial();
		303	}

Subversion Repositories shark

(root)/advdemos/trunk/chimera/send.c - Rev 1650