/*!*****************************************************************************
 *******************************************************************************
  \note  init_ros_servo
  \date  Dec. 1997

  \remarks 

  initializes servo specific things

 *******************************************************************************
 Function Parameters: [in]=input,[out]=output

 none

 ******************************************************************************/
void
init_ros_servo(void)
{
  int j, i;
  char string[100];

  if (ros_servo_initialized) {
    printf("Task Servo is already initialized\n");
    return;
  }

  // the servo name
  sprintf(servo_name,"ros");

  // initialize shared memories and shared semaphores
  if (!init_shared_memory())
    return;

  /* inverse dynamics and other basic kinematic stuff*/
  if (!init_dynamics())
    return;

  // initialize kinematics
  init_kinematics();

  // add variables to data collection
  ros_servo_rate=servo_base_rate/task_servo_ratio;
  initCollectData(ros_servo_rate);

  for (i=1; i<=n_dofs; ++i) {
    printf("%d...",i);
    fflush(stdout);
    sprintf(string,"%s_th",joint_names[i]);
    addVarToCollect((char *)&(joint_state[i].th),string,(char *)"rad", DOUBLE,FALSE);
    sprintf(string,"%s_thd",joint_names[i]);
    addVarToCollect((char *)&(joint_state[i].thd),string,(char *)"rad/s", DOUBLE,FALSE);
    sprintf(string,"%s_thdd",joint_names[i]);
    addVarToCollect((char *)&(joint_state[i].thdd),string,(char *)"rad/s^2", DOUBLE,FALSE);
    sprintf(string,"%s_u",joint_names[i]);
    addVarToCollect((char *)&(joint_state[i].u),string,(char *)"Nm", DOUBLE,FALSE);
    sprintf(string,"%s_ufb",joint_names[i]);
    addVarToCollect((char *)&(joint_state[i].ufb),string,(char *)"Nm", DOUBLE,FALSE);
    sprintf(string,"%s_load",joint_names[i]);
    addVarToCollect((char *)&(joint_state[i].load),string,(char *)"Nm", DOUBLE,FALSE);
    sprintf(string,"%s_des_th",joint_names[i]);
    addVarToCollect((char *)&(joint_des_state[i].th),string,(char *)"rad",DOUBLE,FALSE);
    sprintf(string,"%s_des_thd",joint_names[i]);
    addVarToCollect((char *)&(joint_des_state[i].thd),string,(char *)"rad/s",DOUBLE,FALSE);
    sprintf(string,"%s_des_thdd",joint_names[i]);
    addVarToCollect((char *)&(joint_des_state[i].thdd),string,(char *)"rad/s^2",DOUBLE,FALSE);
    sprintf(string,"%s_uff",joint_names[i]);
    addVarToCollect((char *)&(joint_des_state[i].uff),string,(char *)"Nm",DOUBLE,FALSE);
  }

  // Cartesian variables
  for (i=1; i<=n_endeffs; ++i) {

    sprintf(string,"%s_x",cart_names[i]);
    addVarToCollect((char *)&(cart_state[i].x[_X_]),string,(char *)"m",DOUBLE,FALSE);
    sprintf(string,"%s_y",cart_names[i]);
    addVarToCollect((char *)&(cart_state[i].x[_Y_]),string,(char *)"m",DOUBLE,FALSE);
    sprintf(string,"%s_z",cart_names[i]);
    addVarToCollect((char *)&(cart_state[i].x[_Z_]),string,(char *)"m",DOUBLE,FALSE);

    sprintf(string,"%s_xd",cart_names[i]);
    addVarToCollect((char *)&(cart_state[i].xd[_X_]),string,(char *)"m",DOUBLE,FALSE);
    sprintf(string,"%s_yd",cart_names[i]);
    addVarToCollect((char *)&(cart_state[i].xd[_Y_]),string,(char *)"m",DOUBLE,FALSE);
    sprintf(string,"%s_zd",cart_names[i]);
    addVarToCollect((char *)&(cart_state[i].xd[_Z_]),string,(char *)"m",DOUBLE,FALSE);

    sprintf(string,"%s_xdd",cart_names[i]);
    addVarToCollect((char *)&(cart_state[i].xdd[_X_]),string,(char *)"m",DOUBLE,FALSE);
    sprintf(string,"%s_ydd",cart_names[i]);
    addVarToCollect((char *)&(cart_state[i].xdd[_Y_]),string,(char *)"m",DOUBLE,FALSE);
    sprintf(string,"%s_zdd",cart_names[i]);
    addVarToCollect((char *)&(cart_state[i].xdd[_Z_]),string,(char *)"m",DOUBLE,FALSE);

    sprintf(string,"%s_ad",cart_names[i]);
    addVarToCollect((char *)&(cart_orient[i].ad[_X_]),string,(char *)"rad/s",DOUBLE,FALSE);
    sprintf(string,"%s_bd",cart_names[i]);
    addVarToCollect((char *)&(cart_orient[i].ad[_Y_]),string,(char *)"rad/s",DOUBLE,FALSE);
    sprintf(string,"%s_gd",cart_names[i]);
    addVarToCollect((char *)&(cart_orient[i].ad[_Z_]),string,(char *)"rad/s",DOUBLE,FALSE);

    sprintf(string,"%s_add",cart_names[i]);
    addVarToCollect((char *)&(cart_orient[i].add[_X_]),string,(char *)"rad/s2",DOUBLE,FALSE);
    sprintf(string,"%s_bdd",cart_names[i]);
    addVarToCollect((char *)&(cart_orient[i].add[_Y_]),string,(char *)"rad/s2",DOUBLE,FALSE);
    sprintf(string,"%s_gdd",cart_names[i]);
    addVarToCollect((char *)&(cart_orient[i].add[_Z_]),string,(char *)"rad/s2",DOUBLE,FALSE);

    sprintf(string,"%s_des_x",cart_names[i]);
    addVarToCollect((char *)&(cart_des_state[i].x[_X_]),string,(char *)"m",DOUBLE,FALSE);
    sprintf(string,"%s_des_y",cart_names[i]);
    addVarToCollect((char *)&(cart_des_state[i].x[_Y_]),string,(char *)"m",DOUBLE,FALSE);
    sprintf(string,"%s_des_z",cart_names[i]);
    addVarToCollect((char *)&(cart_des_state[i].x[_Z_]),string,(char *)"m",DOUBLE,FALSE);

    sprintf(string,"%s_des_xd",cart_names[i]);
    addVarToCollect((char *)&(cart_des_state[i].xd[_X_]),string,(char *)"m",DOUBLE,FALSE);
    sprintf(string,"%s_des_yd",cart_names[i]);
    addVarToCollect((char *)&(cart_des_state[i].xd[_Y_]),string,(char *)"m",DOUBLE,FALSE);
    sprintf(string,"%s_des_zd",cart_names[i]);
    addVarToCollect((char *)&(cart_des_state[i].xd[_Z_]),string,(char *)"m",DOUBLE,FALSE);

    sprintf(string,"%s_des_xdd",cart_names[i]);
    addVarToCollect((char *)&(cart_des_state[i].xdd[_X_]),string,(char *)"m",DOUBLE,FALSE);
    sprintf(string,"%s_des_ydd",cart_names[i]);
    addVarToCollect((char *)&(cart_des_state[i].xdd[_Y_]),string,(char *)"m",DOUBLE,FALSE);
    sprintf(string,"%s_des_zdd",cart_names[i]);
    addVarToCollect((char *)&(cart_des_state[i].xdd[_Z_]),string,(char *)"m",DOUBLE,FALSE);

    sprintf(string,"%s_des_ad",cart_names[i]);
    addVarToCollect((char *)&(cart_des_orient[i].ad[_X_]),string,(char *)"rad/s",DOUBLE,FALSE);
    sprintf(string,"%s_des_bd",cart_names[i]);
    addVarToCollect((char *)&(cart_des_orient[i].ad[_Y_]),string,(char *)"rad/s",DOUBLE,FALSE);
    sprintf(string,"%s_des_gd",cart_names[i]);
    addVarToCollect((char *)&(cart_des_orient[i].ad[_Z_]),string,(char *)"rad/",DOUBLE,FALSE);

    sprintf(string,"%s_q0",cart_names[i]);
    addVarToCollect((char *)&(cart_orient[i].q[_Q0_]),string,(char *)"-",DOUBLE,FALSE);
    sprintf(string,"%s_q1",cart_names[i]);
    addVarToCollect((char *)&(cart_orient[i].q[_Q1_]),string,(char *)"-",DOUBLE,FALSE);
    sprintf(string,"%s_q2",cart_names[i]);
    addVarToCollect((char *)&(cart_orient[i].q[_Q2_]),string,(char *)"-",DOUBLE,FALSE);
    sprintf(string,"%s_q3",cart_names[i]);
    addVarToCollect((char *)&(cart_orient[i].q[_Q3_]),string,(char *)"-",DOUBLE,FALSE);

    sprintf(string,"%s_q0d",cart_names[i]);
    addVarToCollect((char *)&(cart_orient[i].qd[_Q0_]),string,(char *)"-",DOUBLE,FALSE);
    sprintf(string,"%s_q1d",cart_names[i]);
    addVarToCollect((char *)&(cart_orient[i].qd[_Q1_]),string,(char *)"-",DOUBLE,FALSE);
    sprintf(string,"%s_q2d",cart_names[i]);
    addVarToCollect((char *)&(cart_orient[i].qd[_Q2_]),string,(char *)"-",DOUBLE,FALSE);
    sprintf(string,"%s_q3d",cart_names[i]);
    addVarToCollect((char *)&(cart_orient[i].qd[_Q3_]),string,(char *)"-",DOUBLE,FALSE);

    sprintf(string,"%s_q0dd",cart_names[i]);
    addVarToCollect((char *)&(cart_orient[i].qdd[_Q0_]),string,(char *)"-",DOUBLE,FALSE);
    sprintf(string,"%s_q1dd",cart_names[i]);
    addVarToCollect((char *)&(cart_orient[i].qdd[_Q1_]),string,(char *)"-",DOUBLE,FALSE);
    sprintf(string,"%s_q2dd",cart_names[i]);
    addVarToCollect((char *)&(cart_orient[i].qdd[_Q2_]),string,(char *)"-",DOUBLE,FALSE);
    sprintf(string,"%s_q3dd",cart_names[i]);
    addVarToCollect((char *)&(cart_orient[i].qdd[_Q3_]),string,(char *)"-",DOUBLE,FALSE);

    sprintf(string,"%s_des_q0",cart_names[i]);
    addVarToCollect((char *)&(cart_des_orient[i].q[_Q0_]),string,(char *)"-",DOUBLE,FALSE);
    sprintf(string,"%s_des_q1",cart_names[i]);
    addVarToCollect((char *)&(cart_des_orient[i].q[_Q1_]),string,(char *)"-",DOUBLE,FALSE);
    sprintf(string,"%s_des_q2",cart_names[i]);
    addVarToCollect((char *)&(cart_des_orient[i].q[_Q2_]),string,(char *)"-",DOUBLE,FALSE);
    sprintf(string,"%s_des_q3",cart_names[i]);
    addVarToCollect((char *)&(cart_des_orient[i].q[_Q3_]),string,(char *)"-",DOUBLE,FALSE);

    sprintf(string,"%s_des_q0d",cart_names[i]);
    addVarToCollect((char *)&(cart_des_orient[i].qd[_Q0_]),string,(char *)"-",DOUBLE,FALSE);
    sprintf(string,"%s_des_q1d",cart_names[i]);
    addVarToCollect((char *)&(cart_des_orient[i].qd[_Q1_]),string,(char *)"-",DOUBLE,FALSE);
    sprintf(string,"%s_des_q2d",cart_names[i]);
    addVarToCollect((char *)&(cart_des_orient[i].qd[_Q2_]),string,(char *)"-",DOUBLE,FALSE);
    sprintf(string,"%s_des_q3d",cart_names[i]);
    addVarToCollect((char *)&(cart_des_orient[i].qd[_Q3_]),string,(char *)"-",DOUBLE,FALSE);

    sprintf(string,"%s_des_q0dd",cart_names[i]);
    addVarToCollect((char *)&(cart_des_orient[i].qdd[_Q0_]),string,(char *)"-",DOUBLE,FALSE);
    sprintf(string,"%s_des_q1dd",cart_names[i]);
    addVarToCollect((char *)&(cart_des_orient[i].qdd[_Q1_]),string,(char *)"-",DOUBLE,FALSE);
    sprintf(string,"%s_des_q2dd",cart_names[i]);
    addVarToCollect((char *)&(cart_des_orient[i].qdd[_Q2_]),string,(char *)"-",DOUBLE,FALSE);
    sprintf(string,"%s_des_q3dd",cart_names[i]);
    addVarToCollect((char *)&(cart_des_orient[i].qdd[_Q3_]),string,(char *)"-",DOUBLE,FALSE);

  }

  // misc sensors
  for (i=1; i<=n_misc_sensors; ++i) {
    sprintf(string,"%s",misc_sensor_names[i]);
    addVarToCollect((char *)&(misc_sensor[i]),string,(char *)"-",DOUBLE,FALSE);
  }


  // the state of the base
  addVarToCollect((char *)&(base_state.x[_X_]),(char *)"base_x",(char *)"m",DOUBLE,FALSE);
  addVarToCollect((char *)&(base_state.x[_Y_]),(char *)"base_y",(char *)"m",DOUBLE,FALSE);
  addVarToCollect((char *)&(base_state.x[_Z_]),(char *)"base_z",(char *)"m",DOUBLE,FALSE);
  
  addVarToCollect((char *)&(base_state.xd[_X_]),(char *)"base_xd",(char *)"m",DOUBLE,FALSE);
  addVarToCollect((char *)&(base_state.xd[_Y_]),(char *)"base_yd",(char *)"m",DOUBLE,FALSE);
  addVarToCollect((char *)&(base_state.xd[_Z_]),(char *)"base_zd",(char *)"m",DOUBLE,FALSE);
  
  addVarToCollect((char *)&(base_state.xdd[_X_]),(char *)"base_xdd",(char *)"m",DOUBLE,FALSE);
  addVarToCollect((char *)&(base_state.xdd[_Y_]),(char *)"base_ydd",(char *)"m",DOUBLE,FALSE);
  addVarToCollect((char *)&(base_state.xdd[_Z_]),(char *)"base_zdd",(char *)"m",DOUBLE,FALSE);
  
  addVarToCollect((char *)&(base_orient.q[_Q0_]),(char *)"base_q0",(char *)"-",DOUBLE,FALSE);
  addVarToCollect((char *)&(base_orient.q[_Q1_]),(char *)"base_q1",(char *)"-",DOUBLE,FALSE);
  addVarToCollect((char *)&(base_orient.q[_Q2_]),(char *)"base_q2",(char *)"-",DOUBLE,FALSE);
  addVarToCollect((char *)&(base_orient.q[_Q3_]),(char *)"base_q3",(char *)"-",DOUBLE,FALSE);

  addVarToCollect((char *)&(base_orient.qd[_Q0_]),(char *)"base_qd0",(char *)"-",DOUBLE,FALSE);
  addVarToCollect((char *)&(base_orient.qd[_Q1_]),(char *)"base_qd1",(char *)"-",DOUBLE,FALSE);
  addVarToCollect((char *)&(base_orient.qd[_Q2_]),(char *)"base_qd2",(char *)"-",DOUBLE,FALSE);
  addVarToCollect((char *)&(base_orient.qd[_Q3_]),(char *)"base_qd3",(char *)"-",DOUBLE,FALSE);

  addVarToCollect((char *)&(base_orient.ad[_A_]),(char *)"base_ad",(char *)"-",DOUBLE,FALSE);
  addVarToCollect((char *)&(base_orient.ad[_B_]),(char *)"base_bd",(char *)"-",DOUBLE,FALSE);
  addVarToCollect((char *)&(base_orient.ad[_G_]),(char *)"base_gd",(char *)"-",DOUBLE,FALSE);

  addVarToCollect((char *)&(base_orient.add[_A_]),(char *)"base_add",(char *)"-",DOUBLE,FALSE);
  addVarToCollect((char *)&(base_orient.add[_B_]),(char *)"base_bdd",(char *)"-",DOUBLE,FALSE);
  addVarToCollect((char *)&(base_orient.add[_G_]),(char *)"base_gdd",(char *)"-",DOUBLE,FALSE);

  printf("done\n");

  updateDataCollectScript();

  // add to man pages
  addToMan((char *)"status",(char *)"displays information about the servo",status);
  addToMan((char *)"drs",(char *)"disables the ros servo",drs);

  int argc = 1;
  char name[] = "SL2ROS_Publisher";
  char* argv[1];
  argv[0] = name;
  ros::init(argc, argv, "SL2ROS_Publisher");

  if (!read_parameter_pool_int(config_files[PARAMETERPOOL], "default_publisher_enabled", &default_publisher_enabled_))
    default_publisher_enabled_ = 0;

  if(default_publisher_enabled_ && !ros_communicator_.initialize())
  {
    printf("ERROR: could not initialize ros communication\n");
    return;
  }

  // initializes user specific issues
  init_user_ros();

  // if all worked out, we mark the servo as ready to go
  ros_servo_initialized = TRUE;

  // set oscilloscope to start value
  initOsc();
  setOsc(d2a_cr,0.0);

  scd();
	return;
}
Example #2
0
/*****************************************************************************
******************************************************************************
  Function Name    : run_turing_playback_task
  Date   	 : June 2014

  Remarks:

  run the task from the task servo: REAL TIME requirements!

******************************************************************************
  Paramters:  (i/o = input/output)

  none

 *****************************************************************************/
static int
run_turing_playback_task(void)
{

  static int firsttime = TRUE;
  int j, i, dof;
  double task_time;
  double wait_time = 5;
  double trigg_start, trigg_dur;
  //double start_elbow_angle;
  double def_elbow_angle = 1.571; //1.571rad = 90deg
  double shift = 0.2; //time shift for position
  double perturbAmp = 0.3491; //0.3491rad = 20deg | 0.5236 = 30deg
  double b = 43.8; // 31.9 for perturbAmp = 30deg;
 
  /////////////////////////////////////////////////////////////////////////////////////////////
  // RANDOMIZATION
  //srand(time(NULL)); //initialize random number generator
  // for random perturbation: perturb_satart_time = rand() %10 + 5;
  /////////////////////////////////////////////////////////////////////////////////////////////

  /////////////////////////////////////////////////////////////////////////////////////////////
  // trigger parameters
  trigg_start = 0.001; //trigger start time (s)
  trigg_dur = 60.0;  // duration of the pulse (s)
  /////////////////////////////////////////////////////////////////////////////////////////////

  // Task parameters
  task_time = task_servo_time - start_time;

 // Tirgger event for starting EMG recording ///////////////////////////////////////////////
  if ((task_time >= trigg_start) && (dataState == 0 ))
  {
	setOsc(d2a_cr,75.00); // 5V
	emgTrig = 5.0;
	dataState = 1;
      printf("dataState is 1.\n");
    scd();
	printf("EMG Data collection started\n");
  }else if ((task_time >= trigg_start) && (task_time <= trigg_start+trigg_dur) && (dataState == 1 )) {
    setOsc(d2a_cr,50.00); // 0V
	emgTrig = 0.0;
  }else if ((task_time >= trigg_start+trigg_dur) && (dataState == 1)){
	setOsc(d2a_cr,75.00); //5V
	emgTrig = 5.0;
    dataState = 2;
      printf("dataState is 2.\n");
  }else {
    setOsc(d2a_cr,50.00); // 0V
    emgTrig = 0.0;    
   }////////////////////////////////////////////////////////////////////////////////////////

  // osciallates one DOF
  //dof = 4; // degree of freedom to rotate
  //for (i=dof; i<=dof; ++i) 
  //{
/*
        //move to desired_pos_f
	if(joint_state[i].th <= def_elbow_angle && udFlag == 0)
	{
		this_time = task_time - last_time;
		some_time = task_time;
		//direction = 0;

		if (this_time >= wait_time)
		{
        		direction = 1;
			start_elbow_angle = def_elbow_angle;
			setOsc(d2a_cr,75.00);
			emgTrig = 5.0;
			udFlag = 1;
			last_time = task_time;
		}
	}
	//move to desired_pos_i
	if(joint_state[i].th >= def_elbow_angle+perturbAmp && udFlag == 1)
	{
		setOsc(d2a_cr,50.00);
		emgTrig = 0.0;
		this_time = task_time - last_time;
		some_time = task_time;
		//direction = 0;

		if (this_time >= wait_time)
		{
        		direction = -1;
			start_elbow_angle = def_elbow_angle + perturbAmp;
			last_time = task_time;
			udFlag = 0;
		}
	}

	mov_time = task_time - some_time;

	target[i].th = start_elbow_angle + direction*perturbAmp/(1 + exp(-b*(mov_time-shift)));
	target[i].thd = direction*perturbAmp*b*exp(-b*(mov_time-shift))/pow(1+exp(-b*(mov_time-shift)), 2);
	target[i].thdd = direction*perturbAmp*(pow(b,2)*exp(-b*(mov_time-shift))/pow(1+exp(-b*(mov_time-shift)),2))*(2*exp(-b*(mov_time-shift))/(1+exp(-b*(mov_time-shift)))-1);
*/

	if (ivar == 1)
	{
		if (firsttime == TRUE)
		{
			direction = -1;
			firsttime = FALSE;
		}
	/*
        	if (this_time >= wait_time)
		{
			direction = -direction;
			start_elbow_angle = joint_state[i].th;
			last_time = task_time;
			some_time = task_time;
			if (direction > 0) //if it's returning to defaul position
			{
				setOsc(d2a_cr,75.00);
				//emgTrig = 5.0;
			}
			else
			{
				setOsc(d2a_cr,50.00);
				//emgTrig = 0.0;
  				ivar = 0;
			}
		}
	*/
	}

		this_time = task_time - last_time;
		mov_time = task_time - some_time;

		/*
		target[i].th = start_elbow_angle + direction*perturbAmp/(1 + exp(-b*(mov_time-shift)));
		target[i].thd = direction*perturbAmp*b*exp(-b*(mov_time-shift))/pow(1+exp(-b*(mov_time-shift)), 2);
		target[i].thdd = direction*perturbAmp*(pow(b,2)*exp(-b*(mov_time-shift))/pow(1+exp(-b*(mov_time-shift)),2))*(2*exp(-b*(mov_time-shift))/(1+exp(-b*(mov_time-shift)))-1);
*/



   // position-servo  
   /* Frequency of playback:
     if 1point/s =>  k = (int)(task_time); 
     if 10 points/s => k = (int)(task_time*10);
     if 420 points /s => k = (int)(task_time*420);
   */
   k = (int)(420*task_time);  //420 Hz sampling
  
    
   if (k > ARRAYLEN-1){
 	k = ARRAYLEN-1;  // to prevent index out of Array 
   }
  // printf("task_time = %i, %f\n",k, posArray[k]);
   //printf("task_time = %i, %f\n",k, pos2DArray[k][1]);
   target[R_EB].th = pos2DArray[k][1];
   

 // }

  // the following variables need to be assigned
  for (i=1; i<=N_DOFS; ++i) 
  {
	joint_des_state[i].th   = target[i].th;
	joint_des_state[i].thd  = target[i].thd;
	joint_des_state[i].thdd = target[i].thdd;
	//joint_des_state[i].uff  = 0.0;
  }



  // compute inverse dynamics torques
  SL_InvDynNE(joint_state,joint_des_state,endeff,&base_state,&base_orient);
  return TRUE;
}
/*!*****************************************************************************
 *******************************************************************************
\note  run_ros_servo
\date  Feb 1999
\remarks 

This program executes the sequence of ros servo routines

 *******************************************************************************
 Function Parameters: [in]=input,[out]=output

 none

 ******************************************************************************/
int
run_ros_servo(void)

{
  int    j,i;
  double dt;
  int    dticks;

  setOsc(d2a_cr,0.0);
  
  /*********************************************************************
   * adjust time
   */
  ++ros_servo_calls;
  ros_servo_time += 1./(double)ros_servo_rate;
  servo_time = ros_servo_time;

  // check for unexpected time drift
  dticks = round((ros_servo_time - last_ros_servo_time)*(double)ros_servo_rate);
  if (dticks != 1 && ros_servo_calls > 2) // need transient ticks to sync servos
    ros_servo_errors += abs(dticks-1);


  /*********************************************************************
   * check for messages
   */
  
  checkForMessages();

  /*********************************************************************
   * receive sensory data
   */

  if (!receive_ros_state()) {
    printf("Problem when receiving ros state\n");
    return FALSE;
  }
  
  setOsc(d2a_cr,10.0);

  /*********************************************************************
   * compute useful kinematic variables
   */
  
  compute_kinematics();
  
  setOsc(d2a_cr,20.0);

#ifdef __XENO__
  // we want to be in secondary mode here
  //rt_task_set_mode(T_PRIMARY,0,NULL);
#endif

  /**********************************************************************
   * do ROS communication
   */
  if (default_publisher_enabled_)
    ros_communicator_.publish();
  
  /**********************************************************************
   * do user specific ROS functions
   */

  run_user_ros();

  ros::spinOnce();

#ifdef __XENO__
  // we want to be in real-time mode here
#if (CONFIG_XENO_VERSION_MAJOR < 2) || (CONFIG_XENO_VERSION_MAJOR == 2 && CONFIG_XENO_VERSION_MINOR < 6)
       // we are on xenomai version < 2.6
       rt_task_set_mode(0,T_PRIMARY,NULL);
#else
       // we are on xenomai version < 2.6
      rt_task_set_mode(0,T_CONFORMING,NULL);
#endif
#endif

  setOsc(d2a_cr,90.0);
  
  /*************************************************************************
   * collect data
   */

  writeToBuffer();
  sendOscilloscopeData();


  setOsc(d2a_cr,100.0);

  
  /*************************************************************************
   * end of program sequence
   */

  last_ros_servo_time = ros_servo_time;

  return TRUE;
  
}
Example #4
0
/*****************************************************************************
******************************************************************************
  Function Name    : init_turing_playback_task
  Date   	 : June 2014

  Remarks:

  initialization for task

******************************************************************************
  Paramters:  (i/o = input/output)

   	none

 *****************************************************************************/
static int
init_turing_playback_task(void)
{
  int j, i;
  int ans;
  char string[100];
  static int firsttime = TRUE;
  dataState = 0;

  if (firsttime){
    firsttime = FALSE;
    //freq = 0.1; // frequency
    //amp  = 0.5; // amplitude
  }


  // prepare going to the default posture
  bzero((char *)&(target[1]),N_DOFS*sizeof(target[1]));
  for (i=1; i<=N_DOFS; i++)
	target[i] = joint_default_state[i];

  // go to the target using inverse dynamics (ID)
  if (!go_target_wait_ID(target))
	return FALSE;

  // add trigger to data collection
  sprintf(string,"emgTrig");
  addVarToCollect((char *)&(emgTrig),string,"V", DOUBLE,TRUE);

  /* add trigger signal to virtual oscilloscope:	 
  add the variable name to prog/masterUser/prefs/task_default.osc*/
  updateOscVars();
 
  // clear start data collection flag
 //dataFlag = 0;



 // input file option to ask
  // list files in directory

    DIR *d;
    struct dirent *dir;
    d = opendir("selected_vars/trajectory/.");
    if (d)
    {
        while ((dir = readdir(d)) != NULL)
        {
            printf("%s\n", dir->d_name);
        }
        closedir(d);
    }

  // get user input 
   char file_selected_vars[80];

   printf ("Enter a file name to open: ");
   scanf ("%s", file_selected_vars);
   
   
  // read position array from txt.
  char path[100] = "selected_vars/trajectory/";
  strcat (path, file_selected_vars);
  FILE *fp = fopen(path, "r"); 
  if (!fp) {
    fprintf(stderr, "Can't open file.\n");
    //exit(EXIT_FAILURE);
  }
 /*
  int *a,temp,count=0;
  a=(int *)malloc(10*sizeof(int));
  temp=fgetc(fp);
  while(!feof(fp))
  {
    if(temp!=44&&temp!=10&&temp!=32)
    {
      count++;
      a[count]=temp;
      printf("%c\n", a[count]);
    }
    temp=fgetc(fp);
  }
    fclose(fp);
 */


 // read the number of lines in the text file. 
/*
  char line[1024];
  int NumberOfLines = 0;
  while( fgets(line,sizeof(line),fp) != NULL)
     NumberOfLines++;
  printf("the number of lines : %i\n", NumberOfLines);
*/

  double temp;
  int p = 0;
 
    //read lines 
    const char s[2] = ",";
    char *token;
    int m;
    if(fp != NULL)
    {
        char line[100]; //10 is enough per line
        while(fgets(line, sizeof line, fp) != NULL)
        {
            token = strtok(line, s);
            for(m=0;m<2;m++)
            {
                if(m==0)
                {   
                    printf("%s\t",token);
                    token = strtok(NULL,s);
		    pos2DArray[p][0] = atof(token);
                } else {
                    printf("%f\n",atof(token));
		    pos2DArray[p][1] = atof(token);
                }       
            }
	p++;
        }
        fclose(fp);
    }

/*
  while (!feof(fp)){ //repeat until end of file
         if (p>ARRAYLEN) { p = ARRAYLEN; break;}
		
         fscanf(fp,"%f",&temp);
   	 pos2DArray[p][1] = temp;  // time
	 fscanf(fp,"%f",&temp);
         pos2DArray[p][2] = temp;  // pos
	p++;	 
        printf("p is...%i\n", p); 
  }
  fclose(fp);
  */
  
  // print the trajectory
  int l;
  for (l = 0; l < ARRAYLEN ; l++) {
  //    printf("pos2dArray %i,  %f\n", l, pos2DArray[l][0]);
  }


  
  // ready to go
  ans = 999;
  while (ans == 999) {
	if (!get_int("Enter 1 to start or anthing else to abort ...",ans,&ans))
  	return FALSE;
  }

  // only go when user really types the right thing
  if (ans != 1)
	return FALSE;

  start_time = task_servo_time;
  printf("start time = %.3f, task_servo_time = %.3f\n",
     start_time, task_servo_time);

  printf("Input var = %i\n",ivar);

  // start data collection
  setOsc(d2a_cr,50.00); //0V

  printf("Data collection started\n");
  return TRUE;
}