WebSVN - shark - Blame - Rev 1422 - /demos/trunk/chimera/chimera.c

Rev	Author	Line No.	Line
1331	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
1422	giacomo	15	#define POS_B_REF 0
		16	#define POS_B_UP 50
		17
		18	int vel_dx = 15;
		19	int vel_sx = 15;
		20
1395	giacomo	21	void program_key_end(KEY_EVT* e)
1331	giacomo	22	{
1395	giacomo	23
1331	giacomo	24	int i;
		25	TIME tmp;
		26
		27	end_send();
		28
		29	for (i=3; i<MAX_PROC; i++){
		30	if (!jet_getstat(i, NULL, &tmp, NULL, NULL))
		31	kern_printf("Task Name : %s - Max Time : %d\n", proc_table[i].name, (int)tmp);
		32	}
1395	giacomo	33
1408	giacomo	34	trace_send();
		35
1395	giacomo	36	sys_end();
		37
1331	giacomo	38	}
		39
1333	giacomo	40	void action_stand_up(void) {
		41
1395	giacomo	42	struct action_event e;
		43	int event;
1339	giacomo	44
		45	status.power = 1;
		46
1395	giacomo	47	kern_gettime(&(e.time));
		48	ADDUSEC2TIMESPEC(5000000,&(e.time));
1333	giacomo	49
1395	giacomo	50	e.type = EVT_SET_MASK_LEG_ANGLE;
		51	e.mask = 0x3F;
		52	e.ang.a = 0 * 3600;
		53	e.ang.b = 70 * 3600;
		54	e.ang.c = 0;
		55	e.pwm = 7;
1333	giacomo	56
1395	giacomo	57	event = insert_action_event(&(e));
1397	giacomo	58	cprintf("Event num = %d\n",event);
1339	giacomo	59
1395	giacomo	60	ADDUSEC2TIMESPEC(1000000,&(e.time));
1334	giacomo	61
1395	giacomo	62	e.type = EVT_SET_MASK_LEG_ANGLE;
		63	e.mask = 0x3F;
		64	e.ang.a = 0 * 3600;
		65	e.ang.b = 0 * 3600;
		66	e.ang.c = 0;
		67	e.pwm = 7;
1333	giacomo	68
1395	giacomo	69	event = insert_action_event(&(e));
1397	giacomo	70	cprintf("Event num = %d\n",event);
		71
1333	giacomo	72	}
		73
1422	giacomo	74	TASK walk_auto()
		75	{
		76	short i;
		77	int vd, vs, auto_count = 0;
		78	float cos_a;
		79	int pos_a[6], pos_b[6], pos_c[6];
		80
		81	struct action_event e;
		82
		83
		84	float fake_cos[32] = { 1.0000, .99518, .98079, .95694, .92388, .88192, .83147, .77301,
		85	.70711, .63439, .55557, .47140, .38268, .29028, .19509, .09802,
		86	.00000,-.09802,-.19509,-.29028,-.38268,-.47140,-.55557,-.63439,
		87	-.70711,-.77301,-.83147,-.88192,-.92388,-.95694,-.98079,-.99518};
		88	float fake_sin[64] = { -0.50, -0.45, -0.40, -0.30, -0.20, -0.15, -0.10, -0.05,
		89	0.00, 0.00, 0.40, 0.40, 0.70, 0.70, 0.90, 0.90,
		90	1.00, 1.00, 0.90, 0.90, 0.70, 0.70, 0.40, 0.40,
		91	0.00, 0.00, -0.05, -0.10, -0.15, -0.20, -0.30, -0.40,
		92	-0.45, -0.50, -0.50, -0.50, -0.50, -0.50, -0.50, -0.50,
		93	-0.50, -0.50, -0.50, -0.50, -0.50, -0.50, -0.50, -0.50,
		94	-0.50, -0.50, -0.50, -0.50, -0.50, -0.50, -0.50, -0.50,
		95	-0.50, -0.50, -0.50, -0.50, -0.50, -0.50, -0.50, -0.50 };
		96
		97	while (1) {
		98	cos_a = (auto_count < 32) ? fake_cos[auto_count] : -fake_cos[auto_count-32];
		99
		100	vd = vel_dx;
		101	vs = vel_sx;
		102
		103	kern_gettime(&(e.time));
		104	ADDUSEC2TIMESPEC(10000,&(e.time));
		105
		106	for (i = 0; i<6; i++) {
		107	switch (i) {
		108	case 0:
		109	/* Leg 0 */
		110	pos_c[i] = vs * cos_a;
		111	if ((vs) \|\| (vd))
		112	pos_b[i] = POS_B_REF + fake_sin[(auto_count+32)%64] * POS_B_UP;
		113	else
		114	pos_b[i] = POS_B_REF;
		115	break;
		116	case 1:
		117	/* Leg 1 */
		118	pos_c[i] = -vd * cos_a;
		119	if ((vs) \|\| (vd))
		120	pos_b[i] = POS_B_REF + fake_sin[auto_count] * POS_B_UP;
		121	else
		122	pos_b[i] = POS_B_REF;
		123	break;
		124	case 2:
		125	/* Leg 2 */
		126	pos_c[i] = -vs * cos_a;
		127	if ((vs) \|\| (vd))
		128	pos_b[i] = POS_B_REF + fake_sin[auto_count] * POS_B_UP;
		129	else
		130	pos_b[i] = POS_B_REF;
		131	break;
		132	case 3:
		133	/* Leg 3 */
		134	pos_c[i] = vd * cos_a;
		135	if ((vs) \|\| (vd))
		136	pos_b[i] = POS_B_REF + fake_sin[(auto_count+32)%64] * POS_B_UP;
		137	else
		138	pos_b[i] = POS_B_REF;
		139	break;
		140	case 4:
		141	/* Leg 4 */
		142	pos_c[i] = vs * cos_a;
		143	if ((vs) \|\| (vd))
		144	pos_b[i] = POS_B_REF + fake_sin[(auto_count+32)%64] * POS_B_UP;
		145	else
		146	pos_b[i] = POS_B_REF;
		147	break;
		148	case 5:
		149	/* Leg 5 */
		150	pos_c[i] = -vd * cos_a;
		151	if ((vs) \|\| (vd))
		152	pos_b[i] = POS_B_REF + fake_sin[auto_count] * POS_B_UP;
		153	else
		154	pos_b[i] = POS_B_REF;
		155	break;
		156	}
		157
		158	e.type = EVT_SET_MASK_LEG_ANGLE;
		159	e.mask = 1 << i;
		160	e.ang.a = (80) * 3600;
		161	e.ang.b = (pos_b[i]) * 3600;
		162	e.ang.c = (pos_c[i]) * 3600;
		163	e.pwm = 7;
		164	insert_action_event(&(e));
		165	}
		166
		167	auto_count++;
		168	if (auto_count > 63) auto_count = 0;
		169
		170	task_testcancel();
		171	task_endcycle();
		172	}
		173	return 0;
		174	}
		175
		176	void action_walk(void) {
		177	HARD_TASK_MODEL ms;
		178	PID pid_walk;
		179
		180	hard_task_default_model(ms);
		181	hard_task_def_ctrl_jet(ms);
		182	hard_task_def_wcet(ms, 1000);
		183	hard_task_def_mit(ms, 25000);
		184	hard_task_def_usemath(ms);
		185	pid_walk = task_create("Walk_Task", walk_auto, &ms, NULL);
		186	if (pid_walk == NIL) {
		187	perror("Could not create task <Walk_Auto>");
		188	sys_end();
		189	} else
		190	task_activate(pid_walk);
		191	}
		192
1331	giacomo	193	int main(int argc, char **argv)
		194	{
		195	TIME seme;
1395	giacomo	196	KEY_EVT k;
1331	giacomo	197
		198	seme = sys_gettime(NULL);
		199	srand(seme);
		200
1395	giacomo	201	k.flag = ALTL_BIT;
		202	k.scan = KEY_C;
		203	k.ascii = 'c';
		204	keyb_hook(k,program_key_end);
1331	giacomo	205
1335	giacomo	206	clear();
		207
1408	giacomo	208	trace_init(1000000);
		209
1331	giacomo	210	init_send(); /* Comunication */
		211	init_key(); /* Keyboard */
		212
1396	giacomo	213	init_action_event(100);
		214
1422	giacomo	215	//action_stand_up();
		216	action_walk();
1333	giacomo	217
1331	giacomo	218	return 0;
1359	giacomo	219
1331	giacomo	220	}

Subversion Repositories shark

(root)/demos/trunk/chimera/chimera.c - Rev 1422