int huboLoop(int mode, bool compliance_mode, bool pause_feature) { double newRef[2] = {1.0, 0.0}; // get initial values for hubo struct hubo_ref H_ref; memset( &H_ref, 0, sizeof(H_ref)); hubo_virtual_t H_virtual; memset( &H_virtual, 0, sizeof(H_virtual)); size_t fs; int r = ach_get( &chan_hubo_ref, &H_ref, sizeof(H_ref), &fs, NULL, ACH_O_COPY ); if(ACH_OK != r) { if(hubo_debug) { // printf("Ref ini r = %s\n",ach_result_to_string(r)); } } else{ assert( sizeof(H_ref) == fs ); } struct hubo_state H_state; memset( &H_state, 0, sizeof(H_state) ); r = ach_get( &chan_hubo_state, &H_state, sizeof(H_state), &fs, NULL, ACH_O_COPY ); if (ACH_OK != r) { if (hubo_debug) { //printf("State ini r = %s\n", ach_result_to_string(r)); } } else { assert( sizeof(H_state) == fs ); } // time info struct timespec t; /* Sampling Period */ double T = (double)interval/(double)NSEC_PER_SEC; // (sec) clock_gettime( 0,&t); // ------------------------------------------------------------------------------ // ---------------[ DO NOT EDIT AVBOE THIS LINE]--------------------------------- // ------------------------------------------------------------------------------ // char* fileName = "valve0.traj"; runTraj(fileName,mode, &H_ref, &t, &H_state, compliance_mode, pause_feature); // ------------------------------------------------------------------------------ // ---------------[ DO NOT EDIT BELOW THIS LINE]--------------------------------- // ------------------------------------------------------------------------------ printf("Trajectory Finished\n"); return 0; }
int huboLoop() { double newRef[2] = {1.0, 0.0}; // get initial values for hubo struct hubo_ref H_ref; memset( &H_ref, 0, sizeof(H_ref)); size_t fs; int r = ach_get( &chan_hubo_ref, &H_ref, sizeof(H_ref), &fs, NULL, ACH_O_COPY ); if(ACH_OK != r) { if(hubo_debug) { printf("Ref ini r = %s\n",ach_result_to_string(r));} } else{ assert( sizeof(H_ref) == fs ); } // time info struct timespec t; /* Sampling Period */ double T = (double)interval/(double)NSEC_PER_SEC; // (sec) clock_gettime( 0,&t); // ------------------------------------------------------------------------------ // ---------------[ DO NOT EDIT AVBOE THIS LINE]--------------------------------- // ------------------------------------------------------------------------------ // char* fileName = "valve0.traj"; runTraj(fileName, &H_ref, &t); // runTraj("ybTest1.traj",&H_ref_filter, &t); // runTraj("valve0.traj", &H_ref_filter, &t); // runTraj("valve1.traj", &H_ref_filter, &t); // runTraj("valve2.traj", &H_ref_filter, &t); // runTraj("valve1.traj", &H_ref_filter, &t); // runTraj("valve2.traj", &H_ref_filter, &t); // runTraj("valve1.traj", &H_ref_filter, &t); // runTraj("valve2.traj", &H_ref_filter, &t); // runTraj("valve1.traj", &H_ref_filter, &t); // runTraj("valve2.traj", &H_ref_filter, &t); // runTraj("valve1.traj", &H_ref_filter, &t); // runTraj("valve2.traj", &H_ref_filter, &t); // runTraj("valve3.traj", &H_ref_filter, &t); // ------------------------------------------------------------------------------ // ---------------[ DO NOT EDIT BELOW THIS LINE]--------------------------------- // ------------------------------------------------------------------------------ printf("Trajectory Finished\n"); return 0; }
int runTrajFunction(char* s, int mode, bool compliance_mode, bool pause_feature) { /* open ach channel */ int t1 = ach_open(&chan_hubo_ref, HUBO_CHAN_REF_NAME , NULL); assert( ACH_OK == t1); int t2 = ach_open(&chan_hubo_state, HUBO_CHAN_STATE_NAME, NULL); assert( ACH_OK == t2); // open to sim chan t1 = ach_open(&chan_hubo_from_sim, HUBO_CHAN_VIRTUAL_FROM_SIM_NAME, NULL); assert( ACH_OK == t1); printf("\n ======================================= \n"); printf(" Hubo-Read-Trajectory-Function-Mode \n"); printf(" ======================================= \n"); // fflush(stdout); // printf("into the func \n"); double newRef[2] = {1.0, 0.0}; // get initial values for hubo struct hubo_ref H_ref; memset( &H_ref, 0, sizeof(H_ref)); hubo_virtual_t H_virtual; memset( &H_virtual, 0, sizeof(H_virtual)); // printf("after creating \n"); // fflush(stdout); size_t fs; int r = ach_get( &chan_hubo_ref, &H_ref, sizeof(H_ref), &fs, NULL, ACH_O_COPY ); // printf("after r \n"); // fflush(stdout); if(ACH_OK != r) { if(hubo_debug) { //printf("Ref ini r = %s\n",ach_result_to_string(r)); } } else{ assert( sizeof(H_ref) == fs ); } //printf("after achget \n"); // fflush(stdout); struct hubo_state H_state; memset( &H_state, 0, sizeof(H_state) ); r = ach_get( &chan_hubo_state, &H_state, sizeof(H_state), &fs, NULL, ACH_O_COPY ); if (ACH_OK != r) { if (hubo_debug) { //printf("State ini r = %s\n", ach_result_to_string(r)); } } else { assert( sizeof(H_state) == fs ); } //printf("after ach \n"); // time info struct timespec t; /* Sampling Period */ double T = (double)interval/(double)NSEC_PER_SEC; // (sec) clock_gettime( 0,&t); // ------------------------------------------------------------------------------ // ---------------[ DO NOT EDIT AVBOE THIS LINE]--------------------------------- // ------------------------------------------------------------------------------ // char* fileName = "valve0.traj"; printf("Getting into trajectory \n"); fflush(stdout); runTraj(s,mode, &H_ref, &t, &H_state, compliance_mode, pause_feature); // ------------------------------------------------------------------------------ // ---------------[ DO NOT EDIT BELOW THIS LINE]--------------------------------- // ------------------------------------------------------------------------------ printf("Trajectory Finished \n"); fflush(stdout); return 0; }