void
controllerKind()
{
int aux,i;
long count=0;
#ifdef VX
if (!no_receive_flag) {
printf("Shut down the task loop before changing the controller\n");
return;
}
#endif
AGAIN:
printf("\n\n\nChoose Controller Kind:\n\n");
printf(" PD ---> %d\n",PD);
printf(" PID ---> %d\n",PID);
printf(" PD + Feedforward ---> %d\n",PDFF);
printf(" PID + Feedforward ---> %d\n",PIDFF);
printf("\n");
get_int(" ----> Input",controller_kind,&aux);
if (aux > PIDFF || aux < PD) {
goto AGAIN;
} else {
if (controller_kind != PIDFF && aux == PIDFF)
zero_integrator();
if(controller_kind != PID && aux == PID)
zero_integrator();
controller_kind = aux;
}
}
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" );
}
}
/*!*****************************************************************************
*******************************************************************************
\note stop
\date August 7, 1992
\remarks
send pump into low pressure and terminate loops
*******************************************************************************
Function Parameters: [in]=input,[out]=output
none
******************************************************************************/
int
stop(char *msg)
{
int i;
user_kill();
zero_integrator();
dms();
beep(1);
printf("%s\n",msg);
return TRUE;
}
/*!*****************************************************************************
*******************************************************************************
\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;
}
