Rev 1624 |
Go to most recent revision |
Blame |
Compare with Previous |
Last modification |
View Log
| RSS feed
/*
* Project: S.Ha.R.K.
*
* Coordinators: Giorgio Buttazzo <giorgio@sssup.it>
*
*
* ReTiS Lab (Scuola Superiore S.Anna - Pisa - Italy)
*
* http://www.sssup.it
* http://retis.sssup.it
* http://shark.sssup.it
*/
#include "chimera.h"
#include <ll/i386/64bit.h>
volatile int calibrate_status
= 0;
volatile int calibrate_exec_step
= 0;
extern HEXAPOD_STATE status
;
extern unsigned char active_leg
;
struct leg_calibration
{
int side
;
int pos
[6];
int delta_90
[3];
int zero
[3];
};
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,0,90},{410399,446399,399599},{154800,-3599,108000}},
};
int adjust
(int angle_sec
, int leg
, int num
) {
int temp
;
smul32div32to32
(angle_sec
,calibration_table
[leg
].
delta_90[num
],324000,temp
);
return calibration_table
[leg
].
side * temp
+ calibration_table
[leg
].
zero[num
];
}
TASK calibrate
(void *arg
) {
int i
, num
= 0, angsec
= 0, angsec_temp
[6];
static int test_angle
[20],set
,turn_on
,servo_count
;
char servo_name
[10];
for (i
=0;i
<20;i
++) test_angle
[i
] = 0;
clear
();
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
(active_leg
*3), pin
(active_leg
*3));
turn_on
= 0;
while (calibrate_status
== 1) {
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
(3*active_leg
+num
),pin
(3*active_leg
+num
));
servo_count
++;
if (servo_count
== 6) {
for (i
=0;i
<3;i
++) {
calibration_table
[active_leg
].
delta_90[i
] = abs(angsec_temp
[2*i
+1] - angsec_temp
[2*i
] + 1);
calibration_table
[active_leg
].
zero[i
] = calibration_table
[active_leg
].
side * abs(calibration_table
[active_leg
].
pos[2*i
] * calibration_table
[active_leg
].
delta_90[i
] / 90) + angsec_temp
[2*i
];
printf_xy
(22*i
,22,WHITE
,"D%d %7d Z%d %7d",
i
,calibration_table
[active_leg
].
delta_90[i
],
i
,calibration_table
[active_leg
].
zero[i
]);
}
calibrate_status
= 0;
calibrate_exec_step
= 0;
task_kill
(exec_shadow
);
task_testcancel
();
}
turn_on
= 1;
calibrate_exec_step
= 0;
}
switch (servo_count
) {
case 0:
case 1:
sprintf(servo_name
,"ALFA ");
num
= 0;
break;
case 2:
case 3:
sprintf(servo_name
,"BETA ");
num
= 1;
break;
case 4:
case 5:
sprintf(servo_name
,"GAMMA");
num
= 2;
break;
}
if (turn_on
== 1) {
servo_turn_on
(com
(3*active_leg
+num
),pin
(3*active_leg
+num
));
turn_on
= 0;
}
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
(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
(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
(3*active_leg
+num
), pin
(3*active_leg
+num
));
printf_xy
(10,21,WHITE
,"Angle Seconds = %08d",angsec
);
task_endcycle
();
}
return 0;
}
void calibrate_init
() {
SOFT_TASK_MODEL st
;
PID st_pid
;
if (calibrate_status
== 0) {
calibrate_status
= 1;
soft_task_default_model
(st
);
soft_task_def_period
(st
,300000);
soft_task_def_met
(st
,30000);
soft_task_def_usemath
(st
);
soft_task_def_ctrl_jet
(st
);
st_pid
= task_create
("Calibration task",calibrate
,&st
,NULL
);
if (st_pid
== NIL
) {
cprintf
("Error creating calibration task\n");
sys_end
();
}
task_activate
(st_pid
);
} else {
return;
}
}
void calibrate_step
(int step
) {
if (calibrate_status
!= 0 && calibrate_exec_step
== 0) {
calibrate_exec_step
= step
;
}
}