uint16_t readProxData()
{
initDataCollection();
delayMS(12);
uint16_t val = readProxDataNonBlocking();
return val;
}
/*!*****************************************************************************
*******************************************************************************
\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;
}