/*!*****************************************************************************
*******************************************************************************
\note add_goto_cart_task
\date Feb 1999
\remarks
adds the task to the task menu
*******************************************************************************
Function Parameters: [in]=input,[out]=output
none
******************************************************************************/
void
add_goto_cart_task( void )
{
int i, j;
static int firsttime = TRUE;
if (firsttime) {
firsttime = FALSE;
cart = my_vector(1,n_endeffs*6);
ctarget = (SL_Cstate *) my_calloc(n_endeffs+1,sizeof(SL_Cstate),MY_STOP);
cnext = (SL_Cstate *) my_calloc(n_endeffs+1,sizeof(SL_Cstate),MY_STOP);
cstatus = my_ivector(1,n_endeffs*6);
target = (SL_DJstate *)my_calloc(n_dofs+1,sizeof(SL_DJstate),MY_STOP);
fthdd = (Filter *)my_calloc(n_dofs+1,sizeof(Filter),MY_STOP);
target = (SL_DJstate *)my_calloc(n_dofs+1,sizeof(SL_DJstate),MY_STOP);
// initialize the filters
init_filters();
for (i=1; i<=n_dofs; ++i)
fthdd[i].cutoff = 5;
addTask("Goto Cart Task", init_goto_cart_task,
run_goto_cart_task, change_goto_cart_task);
addToMan("goVisTarget","move one endeff to blob1",goVisTarget);
}
}
/*****************************************************************************
******************************************************************************
Function Name : add_test_sine_task
Date : Feb 1999
Remarks:
adds the task to the task menu
******************************************************************************
Paramters: (i/o = input/output)
none
*****************************************************************************/
void
add_test_sine_task( void )
{
int i, j;
static int firsttime = TRUE;
char string[100];
if (firsttime) {
firsttime = FALSE;
off = my_vector(1,n_dofs);
amp = my_matrix(1,n_dofs,1,MAX_SINE);
phase = my_matrix(1,n_dofs,1,MAX_SINE);
freq = my_matrix(1,n_dofs,1,MAX_SINE);
n_sine = my_ivector(1,n_dofs);
target = (SL_DJstate *)my_calloc(n_dofs+1,sizeof(SL_DJstate),MY_STOP);
addTask("Test Sine Task", init_test_sine_task,run_test_sine_task, change_test_sine_task);
for (i=1; i<=N_DOFS; ++i) {
sprintf(string,"%s_tar_th",joint_names[i]);
addVarToCollect((char *)&(target[i].th),string,"rad", DOUBLE,FALSE);
sprintf(string,"%s_tar_thd",joint_names[i]);
addVarToCollect((char *)&(target[i].thd),string,"rad/s", DOUBLE,FALSE);
sprintf(string,"%s_tar_thdd",joint_names[i]);
addVarToCollect((char *)&(target[i].thdd),string,"rad/s^2", DOUBLE,FALSE);
}
updateDataCollectScript();
}
}
/*!*****************************************************************************
*******************************************************************************
\note goVisTarget
\date
\remarks
moves the hand to the blob1 target
*******************************************************************************
Function Parameters: [in]=input,[out]=output
none
******************************************************************************/
void
goVisTarget(void)
{
static int ans=1;
int i,j;
static int *sta;
static SL_Cstate *ct;
static int firsttime = TRUE;
if (firsttime) {
firsttime = FALSE;
ct = (SL_Cstate *) my_calloc(n_endeffs+1,sizeof(SL_Cstate), MY_STOP);
sta = my_ivector(1,n_endeffs*6);
}
if (blobs[1].status) {
get_int("Which Endeffector?",ans,&ans);
if (ans < 1 || ans > n_endeffs)
return;
for (i=1; i<=n_endeffs*6; ++i)
sta[i]=0;
for (i=1; i<=N_CART; ++i) {
sta[(ans-1)*6+i] = 1;
ct[ans].x[i] = blobs[1].blob.x[i];
}
go_cart_target_wait(ct,sta, 1.0);
}
}
/*!*****************************************************************************
*******************************************************************************
\note add_sine_task
\date Feb 1999
\remarks
adds the task to the task menu
*******************************************************************************
Function Parameters: [in]=input,[out]=output
none
******************************************************************************/
void
add_sine_task( void )
{
int i, j;
static int firsttime = TRUE;
if (firsttime) {
firsttime = FALSE;
off = my_vector(1,n_dofs);
amp = my_matrix(1,n_dofs,1,MAX_SINE);
phase = my_matrix(1,n_dofs,1,MAX_SINE);
freq = my_matrix(1,n_dofs,1,MAX_SINE);
n_sine = my_ivector(1,n_dofs);
target = (SL_DJstate *)my_calloc(n_dofs+1,sizeof(SL_DJstate),MY_STOP);
addTask("Sine Task", init_sine_task,run_sine_task, change_sine_task);
}
}
