WebSVN - shark - Blame - Rev 1624 - /advdemos/branches/advdemos/chimera/calibrate.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	*
		7	* ReTiS Lab (Scuola Superiore S.Anna - Pisa - Italy)
		8	*
		9	* http://www.sssup.it
		10	* http://retis.sssup.it
		11	* http://shark.sssup.it
		12	*/
		13
		14	#include "chimera.h"
		15	#include <ll/i386/64bit.h>
		16
		17	volatile int calibrate_status = 0;
		18	volatile int calibrate_exec_step = 0;
		19
		20	extern HEXAPOD_STATE status;
		21	extern unsigned char active_leg;
		22
		23	struct leg_calibration {
		24	int side;
		25	int pos[6];
		26	int delta_90[3];
		27	int zero[3];
		28	};
		29
		30	struct leg_calibration calibration_table[] = {
		31	{1,{0,90,-45,45,-90,0},{432001,403201,432001},{-201600,28800,241201}},
		32	{-1,{0,90,-45,45,-90,0},{413949,453599,431999},{216000,50401,-215599}},
		33	{1,{0,90,-45,45,-45,45},{417601,388801,424741},{-216000,-43200,82770}},
		34	{-1,{0,90,-45,45,-45,45},{421199,421199,443799},{165600,-8999,30600}},
		35	{1,{0,90,-45,45,0,90},{414001,424801,410401},{-162000,39600,-122400}},
		36	{-1,{0,90,-45,45,0,90},{410399,446399,399599},{154800,-3599,108000}},
		37	};
		38
		39	int adjust(int angle_sec, int leg, int num) {
		40
		41	int temp;
		42
		43	smul32div32to32(angle_sec,calibration_table[leg].delta_90[num],324000,temp);
		44
		45	return calibration_table[leg].side * temp + calibration_table[leg].zero[num];
		46
		47	}
		48
		49	TASK calibrate(void *arg) {
		50
		51	int i, num = 0, angsec = 0, angsec_temp[6];
		52	static int test_angle[20],set,turn_on,servo_count;
		53	char servo_name[10];
		54
		55	for (i=0;i<20;i++) test_angle[i] = 0;
		56
		57	clear();
		58
		59	servo_turn_off(com(active_leg3), pin(active_leg3));
		60	servo_turn_off(com(active_leg3+1), pin(active_leg3+1));
		61	servo_turn_off(com(active_leg3+2), pin(active_leg3+2));
		62
		63	servo_count = 0;
		64	servo_turn_on(com(active_leg3), pin(active_leg3));
		65	turn_on = 0;
		66
		67	while (calibrate_status == 1) {
		68
		69	if (calibrate_exec_step == 100000) {
		70
		71	angsec_temp[servo_count] = test_angle[3*active_leg+num];
		72	printf_xy(0,10+servo_count,WHITE,"%08d",test_angle[3*active_leg+num]);
		73	servo_turn_off(com(3active_leg+num),pin(3active_leg+num));
		74	servo_count++;
		75
		76	if (servo_count == 6) {
		77
		78	for (i=0;i<3;i++) {
		79	calibration_table[active_leg].delta_90[i] = abs(angsec_temp[2i+1] - angsec_temp[2i] + 1);
		80	calibration_table[active_leg].zero[i] = calibration_table[active_leg].side * abs(calibration_table[active_leg].pos[2i] calibration_table[active_leg].delta_90[i] / 90) + angsec_temp[2*i];
		81
		82	printf_xy(22*i,22,WHITE,"D%d %7d Z%d %7d",
		83	i,calibration_table[active_leg].delta_90[i],
		84	i,calibration_table[active_leg].zero[i]);
		85
		86	}
		87
		88	calibrate_status = 0;
		89	calibrate_exec_step = 0;
		90
		91	task_kill(exec_shadow);
		92	task_testcancel();
		93
		94	}
		95
		96	turn_on = 1;
		97
		98	calibrate_exec_step = 0;
		99
		100	}
		101
		102	switch (servo_count) {
		103	case 0:
		104	case 1:
		105	sprintf(servo_name,"ALFA ");
		106	num = 0;
		107	break;
		108	case 2:
		109	case 3:
		110	sprintf(servo_name,"BETA ");
		111	num = 1;
		112	break;
		113	case 4:
		114	case 5:
		115	sprintf(servo_name,"GAMMA");
		116	num = 2;
		117	break;
		118	}
		119
		120	if (turn_on == 1) {
		121	servo_turn_on(com(3active_leg+num),pin(3active_leg+num));
		122	turn_on = 0;
		123	}
		124
		125	set = calibration_table[active_leg].pos[servo_count];
		126	printf_xy(10,10+servo_count,WHITE,"Set servo %s to position %d",servo_name,set);
		127
		128	servo_turn_on(com(3active_leg+num),pin(3active_leg+num));
		129
		130	if (calibrate_exec_step != 0) {
		131
		132	test_angle[3*active_leg+num] += calibrate_exec_step;
		133	printf_xy(10,20,WHITE,"Set %08d to servo %03d",test_angle[3active_leg+num],3active_leg+num);
		134	servo_set_angle_sec(com(3active_leg+num), pin(3active_leg+num), test_angle[3*active_leg+num]);
		135
		136	calibrate_exec_step = 0;
		137
		138	}
		139
		140	angsec = servo_get_angle_sec(com(3active_leg+num), pin(3active_leg+num));
		141
		142	printf_xy(10,21,WHITE,"Angle Seconds = %08d",angsec);
		143
		144	task_endcycle();
		145
		146	}
		147
		148	return 0;
		149
		150	}
		151
		152	void calibrate_init() {
		153
		154	SOFT_TASK_MODEL st;
		155	PID st_pid;
		156
		157	if (calibrate_status == 0) {
		158	calibrate_status = 1;
		159
		160	soft_task_default_model(st);
		161	soft_task_def_period(st,300000);
		162	soft_task_def_met(st,30000);
		163	soft_task_def_usemath(st);
		164	soft_task_def_ctrl_jet(st);
		165
		166	st_pid = task_create("Calibration task",calibrate,&st,NULL);
		167	if (st_pid == NIL) {
		168	cprintf("Error creating calibration task\n");
		169	sys_end();
		170	}
		171
		172	task_activate(st_pid);
		173
		174	} else {
		175	return;
		176	}
		177
		178	}
		179
		180	void calibrate_step(int step) {
		181
		182	if (calibrate_status != 0 && calibrate_exec_step == 0) {
		183
		184	calibrate_exec_step = step;
		185
		186	}
		187
		188	}

Subversion Repositories shark

(root)/advdemos/branches/advdemos/chimera/calibrate.c - Rev 1624