void
enable_motor_servo( int param )
{
int status;
int temp;
if (!motor_servo_initialized) {
printf("ERROR: Motor servo is not initialized\n");
return;
}
if ( param < 1 ) param = servo_base_rate;
if ( servo_enabled == 0 ) {
servo_enabled = 1;
motor_servo_calls = 0;
servo_time = 0;
motor_servo_time = 0;
motor_servo_rate = param;
motor_servo_errors = 0;
count_no_receive = 0;
count_no_receive_total = 0;
count_no_broadcast = 0;
changeCollectFreq(param);
zero_integrator();
setDefaultPosture();
sysAuxClkDisable();
printf( "sysAuxClkRateGet() returns: %d\n", sysAuxClkRateGet() );
status = sysAuxClkConnect( (FUNCPTR) motor_servo_isr, 0 );
printf( "sysAuxClkConnect() returns: %d (%d)\n", status, OK );
if ( status != OK ) {
fprintf( stderr, "sysAuxClkConnect() NOT OK: %d (%d)\n",status, OK );
return;
}
sysAuxClkRateSet( param );
printf( "sysAuxClkRateGet() returns: %d\n", sysAuxClkRateGet() );
motor_servo_sem = semBCreate(SEM_Q_FIFO,SEM_EMPTY);
motor_servo_tid = taskSpawn("motor_servo",0,VX_FP_TASK,10000,
(FUNCPTR) motor_servo,0,0,0,0,0,0,0,0,0,0);
sysAuxClkEnable();
} else {
fprintf( stderr, "motor servo is already on!\n" );
}
}
void
enable_task_servo( int param )
{
int status;
int temp;
if (!task_servo_initialized) {
printf("ERROR: Task servo is not initialized\n");
return;
}
if ( param < 1 ) param = task_servo_ratio;
if ( servo_enabled == 0 ) {
servo_enabled = 1;
task_servo_calls = 0;
task_servo_time = 0;
local_task_servo_ratio = param;
task_servo_errors = 0;
task_servo_rate = servo_base_rate/param;
if (param == R60HZ)
task_servo_rate = R60HZ;
changeCollectFreq(task_servo_rate);
setTaskByName(NO_TASK);
setDefaultPosture();
task_servo_tid = taskSpawn("task_servo",0,VX_FP_TASK,10000,
(FUNCPTR) task_servo,0,0,0,0,0,0,0,0,0,0);
} else {
fprintf( stderr, "task servo is already on!\n" );
}
}
/*!*****************************************************************************
*******************************************************************************
\note initSimulation
\date July 1998
\remarks
initializes everything needed for the simulation.
*******************************************************************************
Function Parameters: [in]=input,[out]=output
\param[in] argc : number of elements in argv
\param[in] argv : array of argc character strings
******************************************************************************/
int
initSimulation(int argc, char** argv)
{
int i,j,n;
int rc;
int ans;
/* initialize the servos */
init_task_servo(); /* the first servo sets the sampling
rate in collect data */
read_whichDOFs("task_sim_servo");
init_motor_servo();
read_whichDOFs("motor_sim_servo");
init_vision_servo();
read_whichDOFs("vision_sim_servo");
init_invdyn_servo();
read_whichDOFs("invdyn_sim_servo");
/* we need the dynamics initalized */
init_dynamics();
/* user specific tasks */
initUserTasks();
/* create simulation windows */
if (!createWindows())
return FALSE;
/* reset motor_servo variables */
servo_enabled = 1;
motor_servo_calls = 0;
servo_time = 0;
motor_servo_rate = SERVO_BASE_RATE;
zero_integrator();
bzero((char *)&(joint_sim_state[1]),sizeof(SL_DJstate)*N_DOFS);
for (i=1; i<=N_DOFS; ++i) {
joint_sim_state[i].th = joint_default_state[i].th;
joint_des_state[i].th = joint_default_state[i].th;
}
/* reset task_servo variables */
servo_enabled = 1;
task_servo_calls = 0;
task_servo_time = 0;
task_servo_errors = 0;
task_servo_rate = SERVO_BASE_RATE/(double) task_servo_ratio;
setTaskByName(NO_TASK);
setDefaultPosture();
changeCollectFreq(task_servo_rate);
/* reset vision servo variables */
servo_enabled = 1;
vision_servo_calls = 0;
vision_servo_time = 0;
vision_servo_errors = 0;
vision_servo_rate = VISION_SERVO_RATE;
/* initialize all vision variables to safe values */
init_vision_states();
init_learning();
/* reset the invdyn servo variables */
servo_enabled = 1;
invdyn_servo_errors = 0;
invdyn_lookup_data = 0;
invdyn_learning_data = 0;
/* initalize objects in the environment */
readObjects();
/* assign contact force mappings */
#include "LEKin_contact.h"
initContacts();
return TRUE;
}
