/*!*****************************************************************************
*******************************************************************************
\note run_user_task
\date Nov. 2007
\remarks
this function is clocked out of the task servo
*******************************************************************************
Function Parameters: [in]=input,[out]=output
none
******************************************************************************/
int
run_user_task(void)
{
int i,j;
MY_MATRIX(M,1,N_CART,1,N_CART);
MY_VECTOR(v,1,N_CART);
// compute the contact forces in world coordinates at the feet from the
// force sensors
/* rotation matrix from world to L_AAA coordinates:
we can borrow this matrix from the toes, which have the same
rotation, but just a different offset vector, which is not
needed here */
// transform forces
v[_X_] = misc_sensor[L_CFx];
v[_Y_] = misc_sensor[L_CFy];
v[_Z_] = misc_sensor[L_CFz];
mat_vec_mult_size(Alink[L_IN_HEEL],N_CART,N_CART,v,N_CART,endeff[LEFT_FOOT].cf);
// transform torques
v[_A_] = misc_sensor[L_CTa];
v[_B_] = misc_sensor[L_CTb];
v[_G_] = misc_sensor[L_CTg];
mat_vec_mult_size(Alink[L_IN_HEEL],N_CART,N_CART,v,N_CART,endeff[LEFT_FOOT].ct);
/* rotation matrix from world to R_AAA coordinates :
we can borrow this matrix from the toes, which have the same
rotation, but just a different offset vector, which is not
needed here */
// transform forces
// transform forces
v[_X_] = misc_sensor[R_CFx];
v[_Y_] = misc_sensor[R_CFy];
v[_Z_] = misc_sensor[R_CFz];
mat_vec_mult_size(Alink[R_IN_HEEL],N_CART,N_CART,v,N_CART,endeff[RIGHT_FOOT].cf);
// transform torques
v[_A_] = misc_sensor[R_CTa];
v[_B_] = misc_sensor[R_CTb];
v[_G_] = misc_sensor[R_CTg];
mat_vec_mult_size(Alink[R_IN_HEEL],N_CART,N_CART,v,N_CART,endeff[RIGHT_FOOT].ct);
// use the simulated base state if required
if (use_simulated_base_state)
read_simulated_base();
/*//sending
char buf[BUFLEN];
sprintf(buf, 'to je paket %d\n', 5);
mainSend(buf);
printf("send\n");*/
return TRUE;
}
/*!*****************************************************************************
*******************************************************************************
\note run_user_task
\date Nov. 2007
\remarks
this function is clocked out of the task servo
*******************************************************************************
Function Parameters: [in]=input,[out]=output
none
******************************************************************************/
int
run_user_task(void)
{
int i,j;
MY_MATRIX(M,1,N_CART,1,N_CART);
MY_VECTOR(v,1,N_CART);
// compute the contact forces in world coordinates at the feet from the
// force sensors
/* rotation matrix from world to L_AAA coordinates:
we can borrow this matrix from the toes, which have the same
rotation, but just a different offset vector, which is not
needed here */
#ifdef HAS_LOWER_BODY
// transform forces
v[_X_] = misc_sensor[L_CFx];
v[_Y_] = misc_sensor[L_CFy];
v[_Z_] = misc_sensor[L_CFz];
mat_vec_mult_size(Alink[L_IN_HEEL],N_CART,N_CART,v,N_CART,endeff[LEFT_FOOT].cf);
// transform torques
v[_A_] = misc_sensor[L_CTa];
v[_B_] = misc_sensor[L_CTb];
v[_G_] = misc_sensor[L_CTg];
mat_vec_mult_size(Alink[L_IN_HEEL],N_CART,N_CART,v,N_CART,endeff[LEFT_FOOT].ct);
/* rotation matrix from world to R_AAA coordinates :
we can borrow this matrix from the toes, which have the same
rotation, but just a different offset vector, which is not
needed here */
// transform forces
// transform forces
v[_X_] = misc_sensor[R_CFx];
v[_Y_] = misc_sensor[R_CFy];
v[_Z_] = misc_sensor[R_CFz];
mat_vec_mult_size(Alink[R_IN_HEEL],N_CART,N_CART,v,N_CART,endeff[RIGHT_FOOT].cf);
// transform torques
v[_A_] = misc_sensor[R_CTa];
v[_B_] = misc_sensor[R_CTb];
v[_G_] = misc_sensor[R_CTg];
mat_vec_mult_size(Alink[R_IN_HEEL],N_CART,N_CART,v,N_CART,endeff[RIGHT_FOOT].ct);
#endif
differentiate_cog(&cog);
test_fall(&cog);
// use the simulated base state if required
if (use_simulated_base_state)
read_simulated_base();
else
{
// do base state estimation
// update_base_state_estimation(&base_orient, &base_state);
// run_state_est_lin_task();
// getPelv(&base_orient, &base_state);
// base state
if (semTake(sm_base_state_sem,ns2ticks(NO_WAIT)) == ERROR)
{
//printf("sm_base_state_sem take error\n");
return TRUE;
}
cSL_Cstate((&base_state)-1, sm_base_state_data, 1, DOUBLE2FLOAT);
sm_base_state->state[1] = sm_base_state_data[1];
semGive(sm_base_state_sem);
// base orient
if (semTake(sm_base_orient_sem,ns2ticks(NO_WAIT)) == ERROR)
{
//printf("sm_base_orien_sem take error\n");
return TRUE;
}
cSL_quat(&base_orient-1, sm_base_orient_data, 1, DOUBLE2FLOAT);
sm_base_orient->orient[1] = sm_base_orient_data[1];
semGive(sm_base_orient_sem);
}
return TRUE;
}
