WebSVN - shark - Blame - Rev 1332 - /demos/trunk/chimera/calibrate.c

Rev	Author	Line No.	Line
1332	giacomo	1
		2	/*
		3	* Project: S.Ha.R.K.
		4	*
		5	* Coordinators: Giorgio Buttazzo <giorgio@sssup.it>
		6	*
		7	*
		8	* ReTiS Lab (Scuola Superiore S.Anna - Pisa - Italy)
		9	*
		10	* http://www.sssup.it
		11	* http://retis.sssup.it
		12	* http://shark.sssup.it
		13	*/
		14
		15	/*
		16	* Copyright (C) 2000 Paolo Gai
		17	*
		18	* This program is free software; you can redistribute it and/or modify
		19	* it under the terms of the GNU General Public License as published by
		20	* the Free Software Foundation; either version 2 of the License, or
		21	* (at your option) any later version.
		22	*
		23	* This program is distributed in the hope that it will be useful,
		24	* but WITHOUT ANY WARRANTY; without even the implied warranty of
		25	* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
		26	* GNU General Public License for more details.
		27	*
		28	* You should have received a copy of the GNU General Public License
		29	* along with this program; if not, write to the Free Software
		30	* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
		31	*
		32	*/
		33
		34	#include "chimera.h"
		35	#include <ll/i386/64bit.h>
		36
		37	volatile int calibrate_status = 0;
		38	volatile int calibrate_exec_step = 0;
		39
		40	extern HEXAPOD_STATE status;
		41	extern unsigned char active_leg;
		42
		43	extern sem_t mx_servo;
		44
		45	struct leg_calibration {
		46	int pos[6];
		47	int delta_90[3];
		48	int zero[3];
		49	};
		50
		51	struct leg_calibration calibration_table[] = {
		52	{{0,90,-45,45,-90,0},{0,0,0},{0,0,0}},
		53	{{0,90,-45,45,-45,45},{0,0,0},{0,0,0}},
		54	{{0,90,-45,45,0,90},{417601,403201,424801},{-169200,36000,-144000}},
		55	{{0,90,-45,45,-90,0},{0,0,0},{0,0,0}},
		56	{{0,90,-45,45,-45,45},{0,0,0},{0,0,0}},
		57	{{0,90,-45,45,0,90},{0,0,0},{0,0,0}},
		58	};
		59
		60	int adjust(int angle_sec, int leg, int num) {
		61
		62	int temp;
		63
		64	smul32div32to32(angle_sec,calibration_table[leg].delta_90[num],324000,temp);
		65
		66	return temp + calibration_table[leg].zero[num];
		67
		68	}
		69
		70	TASK calibrate(void *arg) {
		71
		72	int i, num = 0, angsec = 0, angsec_temp[6];
		73	static int test_angle[20],set,turn_on,servo_count;
		74	char servo_name[10];
		75
		76	for (i=0;i<20;i++) test_angle[i] = 0;
		77
		78	sem_wait(&mx_servo);
		79	servo_turn_off(COM_PORT, active_leg*3+2);
		80	servo_turn_off(COM_PORT, active_leg*3+1);
		81	servo_turn_off(COM_PORT, active_leg*3);
		82
		83	servo_count = 0;
		84	servo_turn_on(COM_PORT, active_leg*3+2);
		85	turn_on = 0;
		86	sem_post(&mx_servo);
		87
		88	while (calibrate_status == 1) {
		89
		90	sem_wait(&mx_servo);
		91
		92	if (calibrate_exec_step == 100000) {
		93
		94	angsec_temp[servo_count] = test_angle[3*active_leg+num];
		95	printf_xy(0,10+servo_count,WHITE,"%08d",test_angle[3*active_leg+num]);
		96	servo_turn_off(COM_PORT,3*active_leg+num);
		97	servo_count++;
		98
		99	if (servo_count == 6) {
		100
		101	for (i=0;i<3;i++) {
		102	calibration_table[active_leg].delta_90[i] = abs(angsec_temp[2i+1] - angsec_temp[2i] + 1);
		103	calibration_table[active_leg].zero[i] = abs(calibration_table[active_leg].pos[2i] calibration_table[active_leg].delta_90[i] / 90) + angsec_temp[2*i];
		104
		105	printf_xy(20*i,22,RED,"D%d %7d Z%d %7d",i,calibration_table[active_leg].delta_90[i],
		106	i,calibration_table[active_leg].zero[i]);
		107
		108	}
		109
		110	calibrate_status = 0;
		111	calibrate_exec_step = 0;
		112
		113	sem_post(&mx_servo);
		114
		115	task_kill(exec_shadow);
		116	task_testcancel();
		117
		118	}
		119
		120	turn_on = 1;
		121
		122	calibrate_exec_step = 0;
		123
		124	}
		125
		126	switch (servo_count) {
		127	case 0:
		128	case 1:
		129	sprintf(servo_name,"ALFA ");
		130	num = 2;
		131	break;
		132	case 2:
		133	case 3:
		134	sprintf(servo_name,"BETA ");
		135	num = 1;
		136	break;
		137	case 4:
		138	case 5:
		139	sprintf(servo_name,"GAMMA");
		140	num = 0;
		141	break;
		142	}
		143
		144	if (turn_on == 1) {
		145	servo_turn_on(COM_PORT,3*active_leg+num);
		146	turn_on = 0;
		147	}
		148
		149	set = calibration_table[active_leg].pos[servo_count];
		150	printf_xy(10,10+servo_count,WHITE,"Set servo %s to position %d",servo_name,set);
		151
		152	servo_turn_on(COM_PORT, active_leg*3+num);
		153
		154	if (calibrate_exec_step != 0) {
		155
		156	test_angle[3*active_leg+num] += calibrate_exec_step;
		157	printf_xy(10,20,WHITE,"Set %08d to servo %03d",test_angle[3active_leg+num],3active_leg+num);
		158	servo_set_angle_sec(COM_PORT, 3active_leg+num, test_angle[3active_leg+num]);
		159
		160	calibrate_exec_step = 0;
		161
		162	}
		163
		164	angsec = servo_get_angle_sec(COM_PORT, 3*active_leg+num);
		165
		166	sem_post(&mx_servo);
		167
		168	printf_xy(10,21,WHITE,"Angle Seconds = %08d",angsec);
		169
		170	task_endcycle();
		171
		172	}
		173
		174	return 0;
		175
		176	}
		177
		178	void calibrate_init() {
		179
		180	SOFT_TASK_MODEL st;
		181	PID st_pid;
		182
		183	if (calibrate_status == 0) {
		184	calibrate_status = 1;
		185
		186	soft_task_default_model(st);
		187	soft_task_def_period(st,300000);
		188	soft_task_def_met(st,30000);
		189	soft_task_def_usemath(st);
		190	soft_task_def_ctrl_jet(st);
		191
		192	st_pid = task_create("Calibration task",calibrate,&st,NULL);
		193	if (st_pid == NIL) {
		194	cprintf("Error creating calibration task\n");
		195	sys_end();
		196	}
		197
		198	task_activate(st_pid);
		199
		200	} else {
		201	return;
		202	}
		203
		204	}
		205
		206	void calibrate_step(int step) {
		207
		208	if (calibrate_status != 0 && calibrate_exec_step == 0) {
		209
		210	calibrate_exec_step = step;
		211
		212	}
		213
		214	}

Subversion Repositories shark

(root)/demos/trunk/chimera/calibrate.c @ 1421 - Rev 1332