WebSVN - shark - Blame - Rev 1637 - /advdemos/trunk/chimera/chimera.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 POS_B_REF 0
		16	#define POS_B_UP 50
		17
		18	int vel_dx = 15;
		19	int vel_sx = 15;
		20
		21	void program_key_end(KEY_EVT* e)
		22	{
		23
		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	}
		33
		34	sys_end();
		35
		36	}
		37
		38	int beta = 30;
		39
		40	void action_stand_up(void) {
		41
		42	struct action_event e;
		43	int event,alfa;
		44
		45	status.power = 1;
		46
		47	kern_gettime(&(e.time));
		48
		49	for (alfa=0;alfa<=90;alfa+=15) {
		50
		51	ADDUSEC2TIMESPEC(5000000,&(e.time));
		52
		53	e.type = EVT_SET_MASK_LEG_ANGLE;
		54	e.mask = 0x3F;
		55	e.ang.a = alfa * 3600;
		56	e.ang.b = beta * 3600;
		57	e.ang.c = 0 * 3600;
		58	e.pwm = 7;
		59
		60	event = insert_action_event(&(e));
		61	cprintf("Event num = %d\n",event);
		62
		63	}
		64
		65	}
		66
		67	TASK walk_auto_6()
		68	{
1637	giacomo	69	short i, n = 0;
1624	giacomo	70	int vd, vs, auto_count = 0;
		71	int pos_a[6], pos_b[6], pos_c[6];
		72	struct action_event e;
		73
		74	float fake_sin[36] = { .00, .50, .80, 1.0, .80, .50, .00, .00, .00, .00, .00, .00,
		75	.00, .00, .00, .00, .00, .00, .00, .00, .00, .00, .00, .00,
		76	.00, .00, .00, .00, .00, .00, .00, .00, .00, .00, .00, .00};
		77
		78	float fake_cos[36] = {-1.0,-.75,-.50, .00, .50, .75, 1.0, .93, .86, .80, .73, .66,
		79	.60, .53, .46, .40, .33, .26, .20, .13, .06, .00,-.06, .13,
		80	-.20,-.26,-.33,-.40,-.46,-.53,-.60,-.66,-.73,-.80,-.86,-.93};
		81
		82	while (1) {
		83	vd = vel_dx;
		84	vs = vel_sx;
		85
		86	kern_gettime(&(e.time));
		87	ADDUSEC2TIMESPEC(10000,&(e.time));
		88
		89	for (i = 0; i<6; i++) {
		90	switch(i) {
		91	case 0:
		92	n = 0;
		93	break;
		94	case 1:
		95	n = 3;
		96	break;
		97
		98	case 2:
		99	n = 4;
		100	break;
		101
		102	case 3:
		103	n = 1;
		104	break;
		105
		106	case 4:
		107	n = 2;
		108	break;
		109
		110	case 5:
		111	n = 5;
		112	break;
		113	}
		114	if (i%3)
		115	pos_c[i] = vd * fake_cos[(auto_count+6*n)%36];
		116	else
		117	pos_c[i] = vs * fake_cos[(auto_count+6*n)%36];
		118	if ((vs) \|\| (vd))
		119	pos_b[i] = POS_B_REF + fake_sin[(auto_count+6n)%36] POS_B_UP;
		120	else
		121	pos_b[i] = POS_B_REF;
		122
		123	e.type = EVT_SET_MASK_LEG_ANGLE;
		124	e.mask = 1 << i;
		125	e.ang.a = (80) * 3600;
		126	e.ang.b = (pos_b[i]) * 3600;
		127	e.ang.c = (pos_c[i]) * 3600;
		128	e.pwm = 7;
		129	insert_action_event(&(e));
		130	}
		131	auto_count++;
		132	if (auto_count > 35) auto_count = 0;
		133
		134	task_testcancel();
		135	task_endcycle();
		136	}
		137	return 0;
		138	}
		139
		140	TASK walk_auto_3()
		141	{
		142	short i;
		143	int vd, vs, auto_count = 0;
		144	float cos_a;
		145	int pos_a[6], pos_b[6], pos_c[6];
		146
		147	struct action_event e;
		148
		149
		150	float fake_cos[32] = { 1.0000, .99518, .98079, .95694, .92388, .88192, .83147, .77301,
		151	.70711, .63439, .55557, .47140, .38268, .29028, .19509, .09802,
		152	.00000,-.09802,-.19509,-.29028,-.38268,-.47140,-.55557,-.63439,
		153	-.70711,-.77301,-.83147,-.88192,-.92388,-.95694,-.98079,-.99518};
		154	float fake_sin[64] = { -0.50, -0.45, -0.40, -0.30, -0.20, -0.15, -0.10, -0.05,
		155	0.00, 0.00, 0.40, 0.40, 0.70, 0.70, 0.90, 0.90,
		156	1.00, 1.00, 0.90, 0.90, 0.70, 0.70, 0.40, 0.40,
		157	0.00, 0.00, -0.05, -0.10, -0.15, -0.20, -0.30, -0.40,
		158	-0.45, -0.50, -0.50, -0.50, -0.50, -0.50, -0.50, -0.50,
		159	-0.50, -0.50, -0.50, -0.50, -0.50, -0.50, -0.50, -0.50,
		160	-0.50, -0.50, -0.50, -0.50, -0.50, -0.50, -0.50, -0.50,
		161	-0.50, -0.50, -0.50, -0.50, -0.50, -0.50, -0.50, -0.50 };
		162
		163	while (1) {
		164	cos_a = (auto_count < 32) ? fake_cos[auto_count] : -fake_cos[auto_count-32];
		165
		166	vd = vel_dx;
		167	vs = vel_sx;
		168
		169	kern_gettime(&(e.time));
		170	ADDUSEC2TIMESPEC(10000,&(e.time));
		171
		172	for (i = 0; i<6; i++) {
		173	switch (i) {
		174	case 0:
		175	/* Leg 0 */
		176	pos_c[i] = vs * cos_a;
		177	if ((vs) \|\| (vd))
		178	pos_b[i] = POS_B_REF + fake_sin[(auto_count+32)%64] * POS_B_UP;
		179	else
		180	pos_b[i] = POS_B_REF;
		181	break;
		182	case 1:
		183	/* Leg 1 */
		184	pos_c[i] = -vd * cos_a;
		185	if ((vs) \|\| (vd))
		186	pos_b[i] = POS_B_REF + fake_sin[auto_count] * POS_B_UP;
		187	else
		188	pos_b[i] = POS_B_REF;
		189	break;
		190	case 2:
		191	/* Leg 2 */
		192	pos_c[i] = -vs * cos_a;
		193	if ((vs) \|\| (vd))
		194	pos_b[i] = POS_B_REF + fake_sin[auto_count] * POS_B_UP;
		195	else
		196	pos_b[i] = POS_B_REF;
		197	break;
		198	case 3:
		199	/* Leg 3 */
		200	pos_c[i] = vd * cos_a;
		201	if ((vs) \|\| (vd))
		202	pos_b[i] = POS_B_REF + fake_sin[(auto_count+32)%64] * POS_B_UP;
		203	else
		204	pos_b[i] = POS_B_REF;
		205	break;
		206	case 4:
		207	/* Leg 4 */
		208	pos_c[i] = vs * cos_a;
		209	if ((vs) \|\| (vd))
		210	pos_b[i] = POS_B_REF + fake_sin[(auto_count+32)%64] * POS_B_UP;
		211	else
		212	pos_b[i] = POS_B_REF;
		213	break;
		214	case 5:
		215	/* Leg 5 */
		216	pos_c[i] = -vd * cos_a;
		217	if ((vs) \|\| (vd))
		218	pos_b[i] = POS_B_REF + fake_sin[auto_count] * POS_B_UP;
		219	else
		220	pos_b[i] = POS_B_REF;
		221	break;
		222	}
		223
		224	e.type = EVT_SET_MASK_LEG_ANGLE;
		225	e.mask = 1 << i;
		226	e.ang.a = (80) * 3600;
		227	e.ang.b = (pos_b[i]) * 3600;
		228	e.ang.c = (pos_c[i]) * 3600;
		229	e.pwm = 7;
		230	insert_action_event(&(e));
		231	}
		232
		233	auto_count += 4;
		234	if (auto_count > 63) auto_count = 0;
		235
		236	task_testcancel();
		237	task_endcycle();
		238	}
		239	return 0;
		240	}
		241
		242	void action_walk(void) {
		243	HARD_TASK_MODEL ms;
		244	PID pid_walk;
		245
		246	hard_task_default_model(ms);
		247	hard_task_def_ctrl_jet(ms);
		248	hard_task_def_wcet(ms, 1000);
1637	giacomo	249	hard_task_def_mit(ms, 60000);
1624	giacomo	250	hard_task_def_usemath(ms);
		251	pid_walk = task_create("Walk_Task", walk_auto_3, &ms, NULL);
		252	if (pid_walk == NIL) {
		253	perror("Could not create task <Walk_Auto>");
		254	sys_end();
		255	} else
		256	task_activate(pid_walk);
		257	}
		258
		259	int main(int argc, char **argv)
		260	{
		261	TIME seme;
		262	KEY_EVT k;
		263
		264	seme = sys_gettime(NULL);
		265	srand(seme);
		266
		267	k.flag = ALTL_BIT;
		268	k.scan = KEY_C;
		269	k.ascii = 'c';
		270	keyb_hook(k,program_key_end);
		271
		272	clear();
		273
		274	init_send(); /* Comunication */
		275	init_key(); /* Keyboard */
		276
		277	init_action_event(100);
		278
		279	action_walk();
		280
		281	return 0;
		282
		283	}

Subversion Repositories shark

(root)/advdemos/trunk/chimera/chimera.c @ 1647 - Rev 1637