/*****************************************************************************
******************************************************************************
Function Name : change_ranger_task
Date : Dec. 1997
Remarks:
changes the task parameters
******************************************************************************
Paramters: (i/o = input/output)
none
*****************************************************************************/
static int
change_ranger_task(void)
{
int i;
char string[20];
get_string("name",name,name);
for (i=1; i<=N_CART; ++i) {
sprintf(string,"%d.pos",i);
get_double(string,trans[i],&trans[i]);
}
for (i=1; i<=N_CART; ++i) {
sprintf(string,"%d.rot",i);
get_double(string,rot[i],&rot[i]);
}
changeObjPosByName(name,trans,rot);
return TRUE;
}
/*!*****************************************************************************
*******************************************************************************
\note checkForMessages
\date Nov. 2007
\remarks
Messages can be given to the servo for hard-coded tasks.This allows
some information passing between the different processes on variables
of common interest, e.g., the endeffector specs, object information,
etc.
*******************************************************************************
Function Parameters: [in]=input,[out]=output
none
******************************************************************************/
int
checkForMessages(void)
{
int i,j,k;
char name[20];
// check whether a message is available
if (semTake(sm_simulation_message_ready_sem,NO_WAIT) == ERROR) {
return FALSE;
}
// receive the message
if (semTake(sm_simulation_message_sem,ns2ticks(TIME_OUT_NS)) == ERROR) {
printf("Couldn't take simulation message semaphore\n");
return FALSE;
}
for (k=1; k<=sm_simulation_message->n_msgs; ++k) {
// get the name of this message
strcpy(name,sm_simulation_message->name[k]);
// act according to the message name
// -------------------------------------------------------------------------
if (strcmp(name,"reset") == 0) { // reset simulation -----------------------
float buf[N_CART+N_QUAT+1];
memcpy(&(buf[1]),sm_simulation_message->buf+sm_simulation_message->moff[k],
sizeof(float)*(N_CART+N_QUAT));
j = 0;
for (i=1; i<=N_CART; ++i) {
freeze_base_pos[i] = buf[++j];
}
for (i=1; i<=N_QUAT; ++i) {
freeze_base_quat[i] = buf[++j];
}
reset();
// -------------------------------------------------------------------------
} else if (strcmp(name,"changePIDGains") == 0) { // change the gains -------
float buf[n_dofs*3+1];
memcpy(&(buf[1]),sm_simulation_message->buf+sm_simulation_message->moff[k],
sizeof(float)*(3*n_dofs));
for (i=1; i<=n_dofs; ++i) {
controller_gain_th[i] = (double) buf[i];
controller_gain_thd[i] = (double) buf[i+n_dofs];
controller_gain_int[i] = (double) buf[i+2*n_dofs];
}
// ---------------------------------------------------------------------------
} else if (strcmp(name,"where_gains") == 0) {
where_gains();
// -------------------------------------------------------------------------
} else if (strcmp(name,"setUextSim") == 0) { // receive external simulated forces
int count = 0;
float buf[n_dofs*N_CART*2+1];
memcpy(&(buf[1]),sm_simulation_message->buf+sm_simulation_message->moff[k],
sizeof(float)*(2*n_dofs*N_CART));
for (i=1; i<=n_dofs; ++i) {
for (j=1; j<=N_CART; ++j) {
uext_sim[i].f[j] = buf[++count];
uext_sim[i].t[j] = buf[++count];
}
}
// -------------------------------------------------------------------------
} else if (strcmp(name,"changeRealTime") == 0) { // real time simualtion on/off
float buf[1+1];
memcpy(&(buf[1]),sm_simulation_message->buf+sm_simulation_message->moff[k],
sizeof(float));
if (buf[1] == 0) {
real_time = FALSE;
printf("Real-time processing switched off\n");
} else {
real_time = TRUE;
printf("Real-time processing switched on\n");
}
// -------------------------------------------------------------------------
} else if (strcmp(name,"freezeBase") == 0) { // freeze base in simualtion on/off
float buf[1+1];
memcpy(&(buf[1]),sm_simulation_message->buf+sm_simulation_message->moff[k],
sizeof(float));
if (buf[1] == 0) {
freeze_base = FALSE;
printf("Freeze base switched off\n");
} else {
freeze_base = TRUE;
printf("Freeze base switched on\n");
}
// -------------------------------------------------------------------------
} else if (strcmp(name,"setG") == 0) { // change the gravity constant
float buf[1+1];
memcpy(&(buf[1]),sm_simulation_message->buf+sm_simulation_message->moff[k],
sizeof(float));
gravity = buf[1];
printf("Gravity set to %f\n",gravity);
// ---------------------------------------------------------------------------
} else if (strcmp(name,"addObject") == 0) {
struct {
char name[STRING100]; /*!< object name */
int type; /*!< object type */
double trans[N_CART+1]; /*!< translatory offset of object */
double rot[N_CART+1]; /*!< rotational offset of object */
double scale[N_CART+1]; /*!< scaling in x,y,z */
double rgb[N_CART+1]; /*!< color information */
double object_parms[MAX_OBJ_PARMS+1]; /*!< object parameters */
int contact_model; /*!< which contact model to be used */
double contact_parms[MAX_CONTACT_PARMS+1]; /*!< contact parameters */
} data;
memcpy(&data,sm_simulation_message->buf+sm_simulation_message->moff[k],sizeof(data));
addObject(data.name, data.type, data.contact_model, data.rgb, data.trans, data.rot,
data.scale, data.contact_parms,data.object_parms);
// ---------------------------------------------------------------------------
} else if (strcmp(name,"hideObject") == 0) {
struct {
int hide;
char obj_name[100];
} data;
memcpy(&data,sm_simulation_message->buf+sm_simulation_message->moff[k],sizeof(data));
changeHideObjByName(data.obj_name, data.hide);
// ---------------------------------------------------------------------------
} else if (strcmp(name,"changeObjectPos") == 0) {
struct {
char obj_name[100];
double pos[N_CART+1];
double rot[N_CART+1];
} data;
memcpy(&data,sm_simulation_message->buf+sm_simulation_message->moff[k],sizeof(data));
changeObjPosByName(data.obj_name, data.pos, data.rot);
// ---------------------------------------------------------------------------
} else if (strcmp(name,"deleteObject") == 0) {
struct {
char obj_name[100];
} data;
memcpy(&data,sm_simulation_message->buf+sm_simulation_message->moff[k],sizeof(data));
deleteObjByName(data.obj_name);
// ---------------------------------------------------------------------------
} else if (strcmp(name,"status") == 0) {
status();
}
// ---------------------------------------------------------------------------
}
// give back semaphore
sm_simulation_message->n_msgs = 0;
sm_simulation_message->n_bytes_used = 0;
semGive(sm_simulation_message_sem);
return TRUE;
}
