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

Subversion Repositories shark

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