/*!*****************************************************************************
*******************************************************************************
\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;
}
/*****************************************************************************
******************************************************************************
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;
}
/*****************************************************************************
******************************************************************************
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;
}