/*!*****************************************************************************
*******************************************************************************
\note init_sensor_processing
\date May 2000
\remarks
Initializes all sensory processing
*******************************************************************************
Function Parameters: [in]=input,[out]=output
none
******************************************************************************/
int
init_sensor_processing(void)
{
int i,j;
FILE *in;
char string[100];
static int firsttime = TRUE;
if (firsttime) {
firsttime = FALSE;
joint_lin_rot = my_matrix(1,n_dofs,1,6);
pos_polar = my_vector(1,n_dofs);
load_polar = my_vector(1,n_dofs);
joint_raw_state = (SL_Jstate *)
my_calloc((unsigned long)(n_dofs+1),sizeof(SL_Jstate),MY_STOP);
misc_raw_sensor = (double *)
my_calloc((unsigned long)(n_misc_sensors+1),sizeof(double),MY_STOP);
fth = (Filter *)
my_calloc((unsigned long)(n_dofs+1),sizeof(Filter),MY_STOP);
fthd = (Filter *)
my_calloc((unsigned long)(n_dofs+1),sizeof(Filter),MY_STOP);
fthdd = (Filter *)
my_calloc((unsigned long)(n_dofs+1),sizeof(Filter),MY_STOP);
fload = (Filter *)
my_calloc((unsigned long)(n_dofs+1),sizeof(Filter),MY_STOP);
fmisc_sensor = (Filter *)
my_calloc((unsigned long)(n_misc_sensors+1),sizeof(Filter),MY_STOP);
}
/* initalizes translation to and from units */
if (!init_user_sensor_processing())
return FALSE;
/* initialize filtering */
if (!init_filters())
return FALSE;
/* read several variables from files */
/* first, get the calibration values for dealing with the linear to
rotary conversion */
if (!read_sensor_calibration(config_files[SENSORCALIBRATION],
joint_lin_rot,pos_polar,load_polar))
return FALSE;
/* second, get the max, min , and offsets of the position sensors */
if (!read_sensor_offsets(config_files[SENSOROFFSETS]))
return FALSE;
/* third, get the filter cutoff values for all variable */
if (!read_sensor_filters(config_files[SENSORFILTERS]))
return FALSE;
/* add function to man pages */
addToMan("where_off","sensor readings without offsets",where_off);
addToMan("where_raw","raw sensor readings",where_raw);
addToMan("monitor_min_max","records min/max values of sensors",monitor_min_max);
if (!real_robot_flag)
addToMan("toggle_filter","toggles sensor filtering on and off",toggle_filter);
/* make raw variables available for output */
for (i=1; i<=n_dofs; ++i) {
sprintf(string,"%s_rth",joint_names[i]);
addVarToCollect((char *)&(joint_raw_state[i].th),string,"rad", DOUBLE,FALSE);
sprintf(string,"%s_rthd",joint_names[i]);
addVarToCollect((char *)&(joint_raw_state[i].thd),string,"rad/s", DOUBLE,FALSE);
sprintf(string,"%s_rload",joint_names[i]);
addVarToCollect((char *)&(joint_raw_state[i].load),string,"Nm", DOUBLE,FALSE);
}
for (i=1; i<=n_misc_sensors; ++i) {
sprintf(string,"%s_r",misc_sensor_names[i]);
addVarToCollect((char *)&(misc_raw_sensor[i]),string,"-", DOUBLE,FALSE);
}
return TRUE;
}
/*!*****************************************************************************
*******************************************************************************
\note init_simulation_servo
\date Nov. 2007
\remarks
initializes everything for this servo
*******************************************************************************
Function Parameters: [in]=input,[out]=output
none
******************************************************************************/
int
init_simulation_servo(void)
{
int i, j;
// the servo name
sprintf(servo_name,"sim");
// initialization of variables
simulation_servo_rate = servo_base_rate;
// read controller gains
controller_gain_th = my_vector(1,n_dofs);
controller_gain_thd = my_vector(1,n_dofs);
controller_gain_int = my_vector(1,n_dofs);
if (!read_gains(config_files[GAINS],controller_gain_th,
controller_gain_thd, controller_gain_int))
return FALSE;
/* inverse dynamics */
if (!init_dynamics())
return FALSE;
/* initialize kinematicss */
init_kinematics();
// get shared memory
if (!init_shared_memory())
return FALSE;
// object handling
if (!initObjects())
return FALSE;
// need sensor offsets
if (!read_sensor_offsets(config_files[SENSOROFFSETS]))
return FALSE;
// initializes user specific issues
if (!init_user_simulation())
return FALSE;
// add to man pages
addToMan("setIntRate","set number of integration cycles",setIntRate);
addToMan("setIntMethod","set integration method",setIntMethod);
addToMan("realTime","toggle real-time processing",toggleRealTime);
addToMan("status","displays status information about servo",status);
addToMan("dss","disables the simulation servo",dss);
addToMan("where_gains","Print gains of the joints",where_gains);
// data collection
initDataCollection();
// oscilloscope
initOsc();
// initialize user specific simulations
initUserSim(); // general initialization
if (!initUserSimulation()) // user specific intialization
return FALSE;
servo_enabled = TRUE;
return TRUE;
}
/*
* Load the joint limits from config/ file into joint_range array
*
*/
void load_joint_limits() {
char *fname = "SensorOffset.cf";
read_sensor_offsets(fname);
}
