/***************************************************************************** ****************************************************************************** Function Name : init_test_sine_task Date : Dec. 1997 Remarks: initialization for task ****************************************************************************** Paramters: (i/o = input/output) none *****************************************************************************/ static int init_test_sine_task(void) { int j, i; char string[100]; double max_range=0; int ans; /* check whether any other task is running */ if (strcmp(current_task_name,NO_TASK) != 0) { printf("New task can only be run if no other task is running!\n"); return FALSE; } /* allow or speed adjustment */ get_double("Frequency Multiplier",speed,&speed); /* enable inverse dynamics control */ get_int("Which InvDyn: NewtonEuler=1 ArtBody=2",which_invdyn,&which_invdyn); /* read the script for this task */ if (!read_sine_script()) return FALSE; /* go to a save posture */ bzero((char *)&(target[1]),n_dofs*sizeof(target[1])); for (i=1; i<=n_dofs; ++i) { target[i].th = off[i]; for (j=1; j<=n_sine[i]; ++j) { target[i].th += amp[i][j]*sin(phase[i][j]); } } if (!go_target_wait_ID(target)) return FALSE; /* switch the servo mode */ if (invdyn_servo_flag) setServoMode(INVDYNSERVO); else setServoMode(MOTORSERVO); /* do we really want to do this task? */ ans = 999; while (ans == 999) { if (!get_int("Enter 1 to start or anthing else to abort ...",ans,&ans)) return FALSE; } if (ans != 1) return FALSE; task_time = 0.0; scd(); return TRUE; }
static int init_teacher_task ( void ) { startFlag=1; unifyFlag=1; fileNameIndex++; rest_joint_state[1].th = 1.8; rest_joint_state[2].th = -0.2; rest_joint_state[3].th = -0.1; rest_joint_state[4].th = 1.8; rest_joint_state[5].th = -1.57; rest_joint_state[6].th = 0.1; rest_joint_state[7].th = 0.3; /*start_joint_state[1].th = 1.8; start_joint_state[2].th = -0.2; start_joint_state[3].th = 0.3; start_joint_state[4].th = 1.8; start_joint_state[5].th = -1.57; start_joint_state[6].th = 0.1; start_joint_state[7].th = 0.3;*/ int i,j; for (i=1;i<=N_DOFS;i++) { rest_joint_state[i].thd=0; rest_joint_state[i].thdd=0; //start_joint_state[i].thd=0; //start_joint_state[i].thdd=0; } /*for(j=_X_; j<=_Z_; j++) printf("%.3f\t",cart_state[1].x[j]);printf("\n"); for(j=_X_; j<=_Z_; j++) printf("%.3f\t",cart_state[1].xd[j]);printf("\n");*/ //saveJoint_state(1, 1, 1, 1, 1, 1); if (!go_target_wait_ID(rest_joint_state)) return FALSE; /*for(j=_X_; j<=_Z_; j++) printf("%.3f\t",cart_state[1].x[j]); printf("\n"); for(j=_X_; j<=_Z_; j++) printf("%.3f\t",cart_state[1].xd[j]); printf("\n");*/ //saveJoint_state(1, 1, 1, 1, 1, 1); /*if (!go_target_wait_ID(start_joint_state)) return FALSE; for(j=_X_; j<=_Z_; j++) printf("%.3f\t",cart_state[1].x[j]); printf("\n"); for(j=_X_; j<=_Z_; j++) printf("%.3f\t",cart_state[1].xd[j]); printf("\n");*/ //saveJoint_state(1, 1, 1, 1, 1, 1); printf("\nball data and human demonstration are recording...\n\n"); return TRUE; }
/***************************************************************************** ****************************************************************************** Function Name : init_turing_record_task Date : June 2014 Remarks: initialization for task ****************************************************************************** Paramters: (i/o = input/output) none *****************************************************************************/ static int init_turing_record_task(void) { int j, i; int ans; char string[100]; static int firsttime = TRUE; 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 signal to virtual oscilloscope: add the variable name to prog/masterUser/prefs/task_default.osc*/ updateOscVars(); // 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); // start data collection //scd(); //printf("Data collection started\n"); return TRUE; }
int SampleTask::initialize() { start_time_ = 0.0; static int firsttime = TRUE; 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 (int i=1; i<=N_DOFS; i++) { target_[i].th = joint_default_state[i].th; } target_[RA_J3].th=PI/4; target_[LA_J3].th=PI/4; // go to the target using inverse dynamics (ID) if (!go_target_wait_ID(target_)) { return FALSE; } // ready to go int ans = 999; while (ans == 999) { if (!get_int(const_cast<char*>("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); return TRUE; }
/***************************************************************************** ****************************************************************************** Function Name : init_sample_trigger Date : Dec. 1997 Remarks: initialization for task ****************************************************************************** Paramters: (i/o = input/output) none *****************************************************************************/ static int init_sample_trigger(void) { int j, i; int ans; static int firsttime = TRUE; if (firsttime){ firsttime = FALSE; freq0 = 0.5; // frequency freq1 = 0.9; freq2 = 1.4; amp0 = 1.0/2; // amplitude amp1 = 3.0/2; amp2 = 6.0/2; } // 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; // 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); return TRUE; }
static int init_rosrt_test_task(void) { int j, i; int ans; static int firsttime = TRUE; ros::init(a,null,"ros_rt_test_subscriber"); init_ros(); 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; // 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); return TRUE; }
/***************************************************************************** ****************************************************************************** Function Name : init_ranger_task Date : Dec. 1997 Remarks: initialization for task ****************************************************************************** Paramters: (i/o = input/output) none *****************************************************************************/ static int init_ranger_task(void) { int j, i; int ans; static int firsttime = TRUE; if (firsttime) { firsttime = FALSE; create_contact_objects(); } // 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; // draw the ranger float b[N_CART] = {0.,1.0,-0.95}; sendUserGraphics("rangerScenario",b,N_CART*sizeof(float)); // 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); return TRUE; }
/***************************************************************************** ****************************************************************************** Function Name : init_elbow_perturbation Date : January 2015 Remarks: initialization for task ****************************************************************************** Parameters: (i/o = input/output) none *****************************************************************************/ static int init_elbow_perturbation(void) { int j, i; int ans; // go to a save posture bzero((char *)&(goto_state[1]),N_DOFS*sizeof(goto_state[1])); for (i=1; i<=N_DOFS; ++i) { goto_state[i].th = joint_default_state[i].th; } if (!go_target_wait_ID(goto_state)) { return FALSE; } // initialize desired joint drag state for (i=1; i<=N_DOFS; ++i) { joint_des_drag_state[i].th = joint_state[i].th; } // zero the filters for (i=1; i<=N_CART; ++i) { ff[i].cutoff = 40; for (j=0; j<=FILTER_ORDER; ++j) { ff[i].raw[j] = ff[i].filt[j] = 0; } } // initialize filter variables for(i=1; i<=N_DOFS; i++) { for(j=1; j<=NZEROS; j++) { xv[i][j]=0.0; xa[i][j]=0.0; } for(j=1; j<=NPOLES; j++) { yv[i][j]=0.0; ya[i][j]=0.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; } } if (ans != 1) { return FALSE; } start_time = task_servo_time; printf("start time = %.3f, task_servo_time = %.3f\n", start_time, task_servo_time); // start data collection scd(); printf("Data collection started\n"); return TRUE; }
int dmpTask::initialize() { double freq; int i; int ans = 999; start_time_ = 0.0; // Make sure that everything is clear if (vnames_CBi != NULL) { for (i = 1; i <= dof_CBi; i++) free((void *) vnames_CBi[i]); free((void *) vnames_CBi); vnames_CBi = NULL; } if (units_CBi != NULL) { for (i = 1; i <= dof_CBi; i++) free((void *) units_CBi[i]); free((void *) units_CBi); units_CBi = NULL; } if (CBi_traj != NULL) { my_free_matrix(CBi_traj, 1, n_CBi, 1, dof_CBi); CBi_traj = NULL; } if (vnames_Kinect != NULL) { for (i = 1; i <= dof_Kinect; i++) free((void *) vnames_Kinect[i]); free((void *) vnames_Kinect); vnames_Kinect = NULL; } vnames_Kinect = NULL; if (units_Kinect != NULL) { for (i = 1; i <= dof_Kinect; i++) free((void *) units_Kinect[i]); free((void *) units_Kinect); units_Kinect = NULL; } if (Kinect_traj != NULL) { my_free_matrix(Kinect_traj, 1, n_Kinect, 1, dof_Kinect); Kinect_traj = NULL; } if (DMP_object != NULL) { delete DMP_object; DMP_object = NULL; } // Read robot trajectory // mrdplot_convert(CBi_traj_file, &CBi_traj, &vnames_CBi, &units_CBi, &freq, &dof_CBi, &n_CBi); mrdplot_read(CBi_traj_file, &CBi_traj, &vnames_CBi, &units_CBi, &freq, &dof_CBi, &n_CBi); if (!set_active_dofs(vnames_CBi, dof_CBi, active_dofs)) return FALSE; printf("%d active DoFs:", active_dofs[0]); for (i = 2; i <= active_dofs[0]+1; i++) { if (!((i-2)%8)) printf("\n"); printf("%s, ", vnames_CBi[i]); } printf("\n\n"); // Read Kinect trajectory // mrdplot_convert(Kinect_traj_file, &Kinect_traj, &vnames_Kinect, &units_Kinect, &freq, &dof_Kinect, &n_Kinect); mrdplot_read(Kinect_traj_file, &Kinect_traj, &vnames_Kinect, &units_Kinect, &freq, &dof_Kinect, &n_Kinect); // Filter Kinect trajectory if desired if (Kinect_traj != NULL && filter_Kinect_data) { if (!init_filters()) return FALSE; for (int i = 1; i <= 19; i++) fth[i].cutoff = 9; for (int i = 1; i <= 19; i++) { fth[i].raw[0] = fth[i].raw[1] = fth[i].raw[2] = Kinect_traj[1][i+1]; fth[i].filt[0] = fth[i].filt[1] = fth[i].filt[2] = Kinect_traj[1][i+1]; } for (int j = 1; j <= n_Kinect; j++) for (int i = 1; i <= 19; i++) Kinect_traj[j][i+1] = filt(Kinect_traj[j][i+1], &fth[i]); } // Compute DMP for robot trajectory if (DMP_object != NULL) delete DMP_object; // DMP_object = new DMP_structure(DMP_file); DMP_object = new DMP_structure(active_dofs[0], 50, 2.0, 12.0, 3.0); printf("\n"); if (!DMP_object->example_Matrix(CBi_traj, n_CBi, dof_CBi)) return FALSE; DMP_object->DMP_estimate(0); // DMP_object->DMP_param_print(); printf("\n"); // Initialize DMP integration for (int i = 1; i <= active_dofs[0]; i++) { initial_positions[i] = CBi_traj[1][i+1]; initial_velocities[i] = 0.0; } DMP_object->set_initial_DMP_state(&(initial_positions[1]), &(initial_velocities[1])); // include this file to define contact points (needed to compute center of pressure) #include "LEKin_contact.h" // 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]; target_[L_EB].th = target_[R_EB].th = 0.05; target_[B_TFE].th = 0.2; for (int i = 1; i <= active_dofs[0]; i++) { target_[active_dofs[i]].th = initial_positions[i]; target_[active_dofs[i]].thd = 0.0; target_[active_dofs[i]].thdd = 0.0; target_[active_dofs[i]].uff = 0.0; } bool there = true; for (int i = 1; i <= B_HR; i++) if (fabs(target_[i].th - joint_des_state[i].th) > 1.0e-3) { there = false; break; } // go to the target using inverse dynamics (ID)int SampleTask::changeParameters() if (!there) { if (!go_target_wait_ID(target_)) { return FALSE; } } // ready to go while (ans == 999) { if (!get_int(const_cast<char*>("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); return TRUE; }
/***************************************************************************** ****************************************************************************** Description: initialization for task Parameters: none *****************************************************************************/ static int init_mpc_task(void) { int i,j; int ans; static int firsttime = TRUE; if (firsttime) { firsttime = FALSE; lookupTable = my_matrix(1, LOOKUP_TABLE_SIZE, 1, LOOKUP_COLUMN_SIZE); } /* check whether any other task is running */ if (strcmp(current_task_name,NO_TASK) != 0) { printf("Task can only be run if no other task is running!\n"); return FALSE; } get_int("Use simulated ball?", FALSE, &simulation); if(simulation) { printf("Testing table tennis algorithms on the simulator...\n"); } /* go to a save posture */ bzero((char *)&(init_joint_state[1]), N_DOFS * sizeof(init_joint_state[1])); init_joint_state[1].th = 1.0; init_joint_state[2].th = -0.2; init_joint_state[3].th = -0.1; init_joint_state[4].th = 1.8; init_joint_state[5].th = -1.57; init_joint_state[6].th = 0.1; init_joint_state[7].th = 0.3; for (i = 1; i <= N_DOFS; i++) { current_joint_state[i].th = init_joint_state[i].th; current_joint_state[i].thd = init_joint_state[i].thd; } go_target_wait(init_joint_state); if (!go_target_wait_ID(init_joint_state)) return FALSE; printf("Loading lookup table...\n"); load_vec_into_mat(lookupTable, LOOKUP_TABLE_SIZE, LOOKUP_COLUMN_SIZE, LOOKUP_TABLE_NAME); // just testing if (simulation) { // initialize ball cannon position init_ball_cannon(); reset_sim_ball(); } else { // real robot mode } /*get_int("Friction Compensation?", FALSE, &friction_comp); if (friction_comp) { printf("Loading stiction model...\n"); loadStictionModel("./prefs/stiction.pref"); }*/ // for real setup setDefaultEndeffector(); endeff[RIGHT_HAND].x[_Z_] = .3; // indices for ball and robot trajectory trj_time = 0; // clear planned trajectory // debug task if you like //debug_task(); /* ready to go */ ans = 999; while (ans == 999) { if (!get_int("Enter 1 to start or anthing else to abort ...",ans, &ans)) return FALSE; } if (ans != 1) return FALSE; busy = FALSE; // turn off real time changeRealTime(TRUE); return TRUE; }
static int init_fpga_task(void) { int j, i; int ans; static int firsttime = TRUE; //enter code /* get home path */ homePath= getenv ("HOME"); ////////////////////////////////////////////////////////////////////////// // Create a MMAP ////////////////////////////////////////////////////////////////////////// #ifdef WIN32 /*// Create a 4K memory-mapped file (MMF) hFile = CreateFile((LPCTSTR) "input.dat", GENERIC_WRITE | GENERIC_READ, FILE_SHARE_READ | FILE_SHARE_WRITE, NULL, OPEN_ALWAYS, FILE_ATTRIBUTE_NORMAL, NULL); if(hFile == INVALID_HANDLE_VALUE) { // Failed to create file return 1; } map = CreateFileMapping(hFile, NULL, PAGE_READWRITE, 0, 1024, "MMAPShmem"); g_fpga2robot = (char *) MapViewOfFile (map, FILE_MAP_READ | FILE_MAP_WRITE, 0, 0, 0); */ #else printf("buffer_size: %d\n\n", statbuf.st_size); int fd_i; char* fgpa2robot_FilePath; fgpa2robot_FilePath = strncat(homePath, "/prog/masterUser/src/fpga2robot.dat", 100); //max number of characers to be concatinated :100 if((fd_i = open(fgpa2robot_FilePath, O_RDWR)) == -1) { printf("Couldn't open 'fpga2robot.dat'\n"); } g_fpga2robot = mmap(NULL, 1024, PROT_READ | PROT_WRITE, MAP_SHARED, fd_i, 0); if (g_fpga2robot == MAP_FAILED) { close(fd_i); perror("Error mmapping the file"); exit(EXIT_FAILURE); } int fd_o; if((fd_o = open("/home/eric/prog/masterUser/src/robot2fpga.dat", O_RDWR)) == -1) { printf("Couldn't open 'robot2fpga.dat'\n"); } lseek(fd_o, 0, SEEK_SET); g_robot2fpga = mmap(NULL, 1024, PROT_READ | PROT_WRITE, MAP_SHARED, fd_o, 0); if (g_robot2fpga == MAP_FAILED) { close(fd_o); perror("Error mmapping the file"); exit(EXIT_FAILURE); } #endif if(g_fpga2robot != NULL) { printf("Wrapper has created a MMAP for file 'fpga2robot.dat'\n"); } if(g_robot2fpga != NULL) { printf("Wrapper has created a MMAP for file 'robot2fpga.dat'\n"); } /* find size of input file */ if (fstat (fd_i, &statbuf) < 0) err_sys ("fstat error"); system("/home/eric/prog/masterUser/src/py/py_mmap.py &"); //end code 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; // 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); return TRUE; }
/*!***************************************************************************** ******************************************************************************* \note init_sine_task \date Dec. 1997 \remarks initialization for task ******************************************************************************* Function Parameters: [in]=input,[out]=output none ******************************************************************************/ static int init_sine_task(void) { int j, i; char string[100]; double max_range=0; int ans; /* check whether any other task is running */ if (strcmp(current_task_name,NO_TASK) != 0) { printf("New task can only be run if no other task is running!\n"); return FALSE; } /* allow or speed adjustment */ get_double("Frequency Multiplier",speed,&speed); /* allow inverse dynamics */ get_int("Use Inverse Dynamics",invdyn,&invdyn); /* read the script for this task */ if (!read_sine_script()) return FALSE; /* transient multiplier: ramps up the amplitude in one period to avoid discontinuous motor commands */ trans_mult = 0.0; /* what is the longest period? this is what we use to ramp up trans_mult to one */ trans_period = 0.0; for (i=1; i<=n_dofs; ++i) for (j=1; j<=n_sine[i]; ++j) if (freq[i][j] > 0) if (1./freq[i][j] > trans_period) trans_period = 1./freq[i][j]; /* go to a save posture */ bzero((char *)&(target[1]),n_dofs*sizeof(target[1])); for (i=1; i<=n_dofs; ++i) { target[i].th = off[i]; for (j=1; j<=n_sine[i]; ++j) { target[i].th += trans_mult*amp[i][j]*sin(phase[i][j]); } } if (invdyn) { if (!go_target_wait_ID(target)) return FALSE; } else { if (!go_target_wait(target)) return FALSE; } /* do we really want to do this task? */ ans = 999; while (ans == 999) { if (!get_int("Enter 1 to start or anthing else to abort ...",ans,&ans)) return FALSE; } if (ans != 1) return FALSE; task_time = 0.0; scd(); 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; 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/."); 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/"; 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 scd(); printf("Data collection started\n"); return TRUE; }
/*!***************************************************************************** ******************************************************************************* \note init_goto_cart_task \date Dec. 1997 \remarks initialization for task ******************************************************************************* Function Parameters: [in]=input,[out]=output none ******************************************************************************/ static int init_goto_cart_task(void) { int j, i; char string[100]; double max_range=0; int ans; double aux; int flag = FALSE; int iaux; /* check whether any other task is running */ if (strcmp(current_task_name,NO_TASK) != 0) { printf("Goto task can only be run if no other task is running!\n"); return FALSE; } /* zero the filters */ for (i=1; i<=n_dofs; ++i) for (j=0; j<=FILTER_ORDER; ++j) fthdd[i].raw[j] = fthdd[i].filt[j] = 0; /* initialize some variables */ init_vars(); time_step = 1./(double)task_servo_rate; if (!special_flag) { /* go to the same target as the current one, but with ID */ for (i=1; i<=n_dofs; ++i) target[i] = joint_des_state[i]; if (!go_target_wait_ID(target)) return FALSE; /* input the movement speed */ get_double("Movement Time?",movement_time,&movement_time); if (movement_time < 0.2) movement_time = 0.2; tau = movement_time; /* input the cartesian goal */ for (i=1; i<=n_endeffs; ++i) { sprintf(string,"%s_x Status",cart_names[i]); iaux = cstatus[(i-1)*6+ _X_]; get_int(string,cstatus[(i-1)*6+ _X_],&(cstatus[(i-1)*6+ _X_])); if (cstatus[(i-1)*6+ _X_ ]) { if (!iaux) ctarget[i].x[_X_] = cart_des_state[i].x[_X_]; flag = TRUE; sprintf(string,"%s_x Target",cart_names[i]); get_double(string,ctarget[i].x[_X_],&aux); ctarget[i].x[_X_] = aux; } sprintf(string,"%s_y Status",cart_names[i]); iaux = cstatus[(i-1)*6+ _Y_]; get_int(string,cstatus[(i-1)*6+ _Y_],&(cstatus[(i-1)*6+_Y_])); if (cstatus[(i-1)*6+_Y_]) { if (!iaux) ctarget[i].x[_Y_] = cart_des_state[i].x[_Y_]; flag = TRUE; sprintf(string,"%s_y Target",cart_names[i]); get_double(string,ctarget[i].x[_Y_],&aux); ctarget[i].x[_Y_] = aux; } sprintf(string,"%s_z Status",cart_names[i]); iaux = cstatus[(i-1)*6+ _Z_]; get_int(string,cstatus[(i-1)*6+ _Z_],&(cstatus[(i-1)*6+_Z_])); if (cstatus[(i-1)*6+_Z_]) { if (!iaux) ctarget[i].x[_Z_] = cart_des_state[i].x[_Z_]; flag = TRUE; sprintf(string,"%s_z Target",cart_names[i]); get_double(string,ctarget[i].x[_Z_],&aux); ctarget[i].x[_Z_] = aux; } } if (!flag) return FALSE; /* check whether target is reachable */ SL_DJstate js[n_dofs+1]; for (i=1; i<=n_dofs; ++i) js[i] = joint_des_state[i]; if (!checkIKTarget(js, &base_state, &base_orient, endeff, ctarget, cstatus, 1000)) { printf("Target was reduced to reachable limits:\n"); for (j=1; j<=n_endeffs; ++j) { for (i=1; i<=N_CART; ++i) { if (cstatus[(j-1)*6+i]) { printf("%s.%d = %f \n",cart_names[j],i,ctarget[j].x[i]); } } } } } /* the cnext state is the desired state as seen form this program */ for (i=1; i<=n_endeffs;++i) { cnext[i] = cart_des_state[i]; } if (!special_flag) { /* ready to go */ ans = 999; while (ans == 999) { if (!get_int("Enter 1 to start or anthing else to abort ...",ans,&ans)) return FALSE; } if (ans != 1) return FALSE; scd(); } else { special_flag = FALSE; } return TRUE; }
/***************************************************************************** ****************************************************************************** Function Name : init_gravityComp_task Date : Dec. 1997 Remarks: initialization for task ****************************************************************************** Paramters: (i/o = input/output) none *****************************************************************************/ static int init_gravityComp_task(void) { int ans; int j,i; static int firsttime = TRUE; if (firsttime == TRUE) { xd = my_vector(1,3); B = my_matrix(1,3,1,3); J7T = my_matrix(1,7,1,3); u_field = my_vector(1,7); B[1][1] = -10.1; B[1][2] = -11.2; B[1][3] = 0.0; B[2][1] = -11.2; B[2][2] = -11.1; B[2][3] = 0.0; B[3][1] = 0.0; B[3][2] = 0.0; B[3][3] = 1.0; //for (i=1;i<=3;i++) { // for (j=1;j<=3;j++) { //if (i==j) { // B[i][j] = -1.0; //} //else { // B[i][j] = 0.0; //} //} //} firsttime = FALSE; } /* go to a save posture */ bzero((char *)&(target[1]),N_DOFS*sizeof(target[1])); for (i=1; i<=N_DOFS; ++i) { target[i].th = joint_default_state[i].th; } if (!go_target_wait_ID(target)) return FALSE; /* initialize filter variables */ for(i=1; i<N_DOFS; i++) { for(j=1; j<NZEROS; j++) { xv[i][j]=0.0; xa[i][j]=0.0; } for(j=1; j<NPOLES; j++) { yv[i][j]=0.0; ya[i][j]=0.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; } if (ans != 1) return FALSE; return TRUE; }