예제 #1
0
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" );
  }

}
예제 #2
0
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" );
  }

}
예제 #3
0
파일: SL_user.c 프로젝트: wonjsohn/sarcos
/*!*****************************************************************************
 *******************************************************************************
 \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;
}