/*!*****************************************************************************
*******************************************************************************
\note init_controller
\date Feb 1999
\remarks
initializes all necessary things
*******************************************************************************
Function Parameters: [in]=input,[out]=output
none
******************************************************************************/
int
init_controller( void )
{
int i, j;
FILE *in;
char string[100];
static int firsttime = TRUE;
if (firsttime) {
firsttime = FALSE;
u = my_vector(1,n_dofs);
ufb = my_vector(1,n_dofs);
upd = my_vector(1,n_dofs);
controller_gain_th = my_vector(1,n_dofs);
controller_gain_thd = my_vector(1,n_dofs);
controller_gain_int = my_vector(1,n_dofs);
integrator_state = my_vector(1,n_dofs);
}
/* read the control gains and max controls */
if (!read_gains(config_files[GAINS],controller_gain_th, controller_gain_thd, controller_gain_int))
return FALSE;
addToMan("ck","change controller",ck);
addToMan("setGains","Change PID gain settings",setGainsSim);
return TRUE;
}
/*****************************************************************************
******************************************************************************
Function Name : add_gravityComp_task
Date : Feb 1999
Remarks:
adds the task to the task menu
******************************************************************************
Paramters: (i/o = input/output)
none
*****************************************************************************/
void
add_gravityComp_task( void )
{
int i, j;
static int firsttime = TRUE;
if (firsttime) {
firsttime = FALSE;
addTask("Gravity Compensation Task", init_gravityComp_task,
run_gravityComp_task, change_gravityComp_task);
/* add variables to data collection */
addVarToCollect((char *)&(fieldon),"force-field","-",INT,FALSE);
addVarToCollect((char *)&(joint_filt_state[1].thd),"R_SFE_filt_thd","rad/s",DOUBLE,FALSE);
addVarToCollect((char *)&(joint_filt_state[2].thd),"R_SAA_filt_thd","rad/s",DOUBLE,FALSE);
addVarToCollect((char *)&(joint_filt_state[3].thd),"R_HR_filt_thd","rad/s",DOUBLE,FALSE);
addVarToCollect((char *)&(joint_filt_state[4].thd),"R_EB_filt_thd","rad/s",DOUBLE,FALSE);
addVarToCollect((char *)&(joint_filt_state[5].thd),"R_WR_filt_thd","rad/s",DOUBLE,FALSE);
addVarToCollect((char *)&(joint_filt_state[6].thd),"R_WFE_filt_thd","rad/s",DOUBLE,FALSE);
addVarToCollect((char *)&(joint_filt_state[7].thd),"R_WAA_filt_thd","rad/s",DOUBLE,FALSE);
addVarToCollect((char *)&(joint_filt_state[1].thdd),"R_SFE_filt_thdd","rad/s^2",DOUBLE,FALSE);
addVarToCollect((char *)&(joint_filt_state[2].thdd),"R_SAA_filt_thdd","rad/s^2",DOUBLE,FALSE);
addVarToCollect((char *)&(joint_filt_state[3].thdd),"R_HR_filt_thdd","rad/s^2",DOUBLE,FALSE);
addVarToCollect((char *)&(joint_filt_state[4].thdd),"R_EB_filt_thdd","rad/s^2",DOUBLE,FALSE);
addVarToCollect((char *)&(joint_filt_state[5].thdd),"R_WR_filt_thdd","rad/s^2",DOUBLE,FALSE);
addVarToCollect((char *)&(joint_filt_state[6].thdd),"R_WFE_filt_thdd","rad/s^2",DOUBLE,FALSE);
addVarToCollect((char *)&(joint_filt_state[7].thdd),"R_WAA_filt_thdd","rad/s^2",DOUBLE,FALSE);
addVarToCollect((char *)&(fieldtorque),"fieldtorque","N/m",DOUBLE,FALSE);
}
/* read the control gains and max controls */
if (!read_gains("Gains.cf",controller_gain_th, controller_gain_thd, controller_gain_int))
//if (!read_gains(controller_gain_th, controller_gain_thd, controller_gain_int))
printf("Error reading Gains.cf\n");
}
/*!*****************************************************************************
*******************************************************************************
\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;
}
