Subversion Repositories shark

Compare Revisions

Ignore whitespace Rev 1358 → Rev 1359

/demos/trunk/chimera/calibrate.c
1,4 → 1,3
 
/*
* Project: S.Ha.R.K.
*
12,25 → 11,6
* http://shark.sssup.it
*/
 
/*
* Copyright (C) 2000 Paolo Gai
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
 
#include "chimera.h"
#include <ll/i386/64bit.h>
 
40,8 → 20,6
extern HEXAPOD_STATE status;
extern unsigned char active_leg;
 
extern sem_t mx_servo;
 
struct leg_calibration {
int side;
int pos[6];
51,10 → 29,10
 
struct leg_calibration calibration_table[] = {
{1,{0,90,-45,45,-90,0},{432001,403201,432001},{-201600,28800,241201}},
{-1,{0,90,-45,45,-90,0},{413949,453599,431999},{216000,50401,-215599}},
{1,{0,90,-45,45,-45,45},{417601,388801,424741},{-216000,-43200,82770}},
{-1,{0,90,-45,45,-45,45},{421199,421199,443799},{165600,-8999,30600}},
{1,{0,90,-45,45,0,90},{414001,424801,410401},{-162000,39600,-122400}},
{-1,{0,90,-45,45,-90,0},{413949,453599,431999},{216000,50401,-215599}},
{-1,{0,90,-45,45,-45,45},{421199,421199,443799},{165600,-8999,30600}},
{-1,{0,90,-45,45,0,90},{0,0,0},{0,0,0}},
};
 
78,25 → 56,21
 
clear();
sem_wait(&mx_servo);
servo_turn_off(COM_PORT, active_leg*3+2);
servo_turn_off(COM_PORT, active_leg*3+1);
servo_turn_off(COM_PORT, active_leg*3);
servo_turn_off(com(active_leg*3), pin(active_leg*3));
servo_turn_off(com(active_leg*3+1), pin(active_leg*3+1));
servo_turn_off(com(active_leg*3+2), pin(active_leg*3+2));
 
servo_count = 0;
servo_turn_on(COM_PORT, active_leg*3+2);
servo_turn_on(com(active_leg*3), pin(active_leg*3));
turn_on = 0;
sem_post(&mx_servo);
 
while (calibrate_status == 1) {
 
sem_wait(&mx_servo);
 
if (calibrate_exec_step == 100000) {
angsec_temp[servo_count] = test_angle[3*active_leg+num];
printf_xy(0,10+servo_count,WHITE,"%08d",test_angle[3*active_leg+num]);
servo_turn_off(COM_PORT,3*active_leg+num);
servo_turn_off(com(3*active_leg+num),pin(3*active_leg+num));
servo_count++;
 
if (servo_count == 6) {
114,8 → 88,6
calibrate_status = 0;
calibrate_exec_step = 0;
 
sem_post(&mx_servo);
 
task_kill(exec_shadow);
task_testcancel();
 
131,7 → 103,7
case 0:
case 1:
sprintf(servo_name,"ALFA ");
num = 2;
num = 0;
break;
case 2:
case 3:
141,12 → 113,12
case 4:
case 5:
sprintf(servo_name,"GAMMA");
num = 0;
num = 2;
break;
}
 
if (turn_on == 1) {
servo_turn_on(COM_PORT,3*active_leg+num);
servo_turn_on(com(3*active_leg+num),pin(3*active_leg+num));
turn_on = 0;
}
 
153,22 → 125,20
set = calibration_table[active_leg].pos[servo_count];
printf_xy(10,10+servo_count,WHITE,"Set servo %s to position %d",servo_name,set);
servo_turn_on(COM_PORT, active_leg*3+num);
servo_turn_on(com(3*active_leg+num),pin(3*active_leg+num));
 
if (calibrate_exec_step != 0) {
test_angle[3*active_leg+num] += calibrate_exec_step;
printf_xy(10,20,WHITE,"Set %08d to servo %03d",test_angle[3*active_leg+num],3*active_leg+num);
servo_set_angle_sec(COM_PORT, 3*active_leg+num, test_angle[3*active_leg+num]);
servo_set_angle_sec(com(3*active_leg+num), pin(3*active_leg+num), test_angle[3*active_leg+num]);
 
calibrate_exec_step = 0;
 
}
 
angsec = servo_get_angle_sec(COM_PORT, 3*active_leg+num);
angsec = servo_get_angle_sec(com(3*active_leg+num), pin(3*active_leg+num));
sem_post(&mx_servo);
 
printf_xy(10,21,WHITE,"Angle Seconds = %08d",angsec);
 
task_endcycle();