/*****************************************************************************
******************************************************************************
Description: initialization for task
Parameters:
none
*****************************************************************************/
static int init_mpc_task(void) {
int i,j;
int ans;
static int firsttime = TRUE;
if (firsttime) {
firsttime = FALSE;
lookupTable = my_matrix(1, LOOKUP_TABLE_SIZE, 1, LOOKUP_COLUMN_SIZE);
}
/* check whether any other task is running */
if (strcmp(current_task_name,NO_TASK) != 0) {
printf("Task can only be run if no other task is running!\n");
return FALSE;
}
get_int("Use simulated ball?", FALSE, &simulation);
if(simulation) {
printf("Testing table tennis algorithms on the simulator...\n");
}
/* go to a save posture */
bzero((char *)&(init_joint_state[1]), N_DOFS * sizeof(init_joint_state[1]));
init_joint_state[1].th = 1.0;
init_joint_state[2].th = -0.2;
init_joint_state[3].th = -0.1;
init_joint_state[4].th = 1.8;
init_joint_state[5].th = -1.57;
init_joint_state[6].th = 0.1;
init_joint_state[7].th = 0.3;
for (i = 1; i <= N_DOFS; i++) {
current_joint_state[i].th = init_joint_state[i].th;
current_joint_state[i].thd = init_joint_state[i].thd;
}
go_target_wait(init_joint_state);
if (!go_target_wait_ID(init_joint_state))
return FALSE;
printf("Loading lookup table...\n");
load_vec_into_mat(lookupTable, LOOKUP_TABLE_SIZE,
LOOKUP_COLUMN_SIZE, LOOKUP_TABLE_NAME);
// just testing
if (simulation) {
// initialize ball cannon position
init_ball_cannon();
reset_sim_ball();
}
else {
// real robot mode
}
/*get_int("Friction Compensation?", FALSE, &friction_comp);
if (friction_comp) {
printf("Loading stiction model...\n");
loadStictionModel("./prefs/stiction.pref");
}*/
// for real setup
setDefaultEndeffector();
endeff[RIGHT_HAND].x[_Z_] = .3;
// indices for ball and robot trajectory
trj_time = 0; // clear planned trajectory
// debug task if you like
//debug_task();
/* ready to go */
ans = 999;
while (ans == 999) {
if (!get_int("Enter 1 to start or anthing else to abort ...",ans, &ans))
return FALSE;
}
if (ans != 1)
return FALSE;
busy = FALSE;
// turn off real time
changeRealTime(TRUE);
return TRUE;
}
/*!*****************************************************************************
*******************************************************************************
\note init_sine_task
\date Dec. 1997
\remarks
initialization for task
*******************************************************************************
Function Parameters: [in]=input,[out]=output
none
******************************************************************************/
static int
init_sine_task(void)
{
int j, i;
char string[100];
double max_range=0;
int ans;
/* check whether any other task is running */
if (strcmp(current_task_name,NO_TASK) != 0) {
printf("New task can only be run if no other task is running!\n");
return FALSE;
}
/* allow or speed adjustment */
get_double("Frequency Multiplier",speed,&speed);
/* allow inverse dynamics */
get_int("Use Inverse Dynamics",invdyn,&invdyn);
/* read the script for this task */
if (!read_sine_script())
return FALSE;
/* transient multiplier: ramps up the amplitude in one period to
avoid discontinuous motor commands */
trans_mult = 0.0;
/* what is the longest period? this is what we use to ramp up trans_mult to one */
trans_period = 0.0;
for (i=1; i<=n_dofs; ++i)
for (j=1; j<=n_sine[i]; ++j)
if (freq[i][j] > 0)
if (1./freq[i][j] > trans_period)
trans_period = 1./freq[i][j];
/* go to a save posture */
bzero((char *)&(target[1]),n_dofs*sizeof(target[1]));
for (i=1; i<=n_dofs; ++i) {
target[i].th = off[i];
for (j=1; j<=n_sine[i]; ++j) {
target[i].th += trans_mult*amp[i][j]*sin(phase[i][j]);
}
}
if (invdyn) {
if (!go_target_wait_ID(target))
return FALSE;
} else {
if (!go_target_wait(target))
return FALSE;
}
/* do we really want to do this task? */
ans = 999;
while (ans == 999) {
if (!get_int("Enter 1 to start or anthing else to abort ...",ans,&ans))
return FALSE;
}
if (ans != 1)
return FALSE;
task_time = 0.0;
scd();
return TRUE;
}
