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(); |