int main(int argc, char **argv) {
int vflag = 0;
int c;
char* ach_chan = HUBO_CHAN_REF_FILTER_NAME;
int i = 1;
while(argc > i) {
if(strcmp(argv[i], "-d") == 0) {
debug = 1;
}
if(strcmp(argv[i], "-r") == 0) {
ach_chan = HUBO_CHAN_REF_NAME;
}
i++;
}
/* RT */
struct sched_param param;
/* Declare ourself as a real time task */
param.sched_priority = MY_PRIORITY;
if(sched_setscheduler(0, SCHED_FIFO, ¶m) == -1) {
perror("sched_setscheduler failed");
exit(-1);
}
/* Lock memory */
if(mlockall(MCL_CURRENT|MCL_FUTURE) == -1) {
perror("mlockall failed");
exit(-2);
}
/* Pre-fault our stack */
stack_prefault();
/* open ach channel */
//int r = ach_open(&chan_hubo_ref, HUBO_CHAN_REF_NAME , NULL);
int r = ach_open(&chan_hubo_ref, ach_chan , NULL);
assert( ACH_OK == r );
r = ach_open(&chan_hubo_state, HUBO_CHAN_STATE_NAME , NULL);
assert( ACH_OK == r );
// get initial values for hubo
struct hubo_ref H_ref;
struct hubo_init_cmd H_init;
struct hubo_state H_state;
struct hubo_param H_param;
memset( &H_ref, 0, sizeof(H_ref));
memset( &H_init, 0, sizeof(H_init));
memset( &H_state, 0, sizeof(H_state));
memset( &H_param, 0, sizeof(H_param));
usleep(250000);
// set default values for Hubo
setJointParams(&H_param, &H_state);
huboLoop(&H_param);
pause();
return 0;
}
int main(int argc, char **argv) {
int vflag = 0;
int c;
int i = 1;
while(argc > i) {
if(strcmp(argv[i], "-d") == 0) { // debug
debug = 1;
}
if(strcmp(argv[i], "-n") == 0) {
if( argc > (i+1)) {
fileName = argv[i+1];
printf("Trejectory file changed\n");
}
else {
printf("ERROR: File name not changed\n");
}
}
if(strcmp(argv[i], "-f") == 0) {
int j = i+1;
if( argc > (j)) {
if(strcmp(argv[j], "100") == 0) { interval = 10000000; /* 100 hz (0.01 sec) */ }
else if(strcmp(argv[j], "50") == 0) { interval = 20000000; /* 50 hz (0.02 sec) */ }
else if(strcmp(argv[j], "25") == 0) { interval = 40000000; /* 25 hz (0.04 sec) */ }
else if(strcmp(argv[j], "10") == 0) { interval = 100000000; /* 25 hz (0.04 sec) */}
else if(strcmp(argv[j], "200") == 0) { interval = 5000000; /* 200 hz (0.005 sec)*/ }
else if(strcmp(argv[j], "500") == 0) { interval = 2000000; /* 500 hz (0.002 sec)*/ }
else { printf("ERROR: Frequency not changed\n"); }
}
else {
printf("ERROR: Frequency not changed\n");
}
}
if(strcmp(argv[i], "-h") == 0) {
printf("------------------------------------------\n");
printf("-----------hubo-read-trajectory-----------\n");
printf("------------------------------------------\n");
printf("\n");
printf("Usage: ./hubo-read-trajectory -f 100 -n fileName.traj\n");
printf("\tOptions:\n");
printf("\t\t-h help menu\n");
printf("\t\t-n change trajectory\n");
printf("\t\t\t\tdefault: no file\n");
printf("\t\t\t\tatguements: filename\n");
printf("\t\t-f change frequency\n");
printf("\t\t\tdefault: 25hz\n");
printf("\t\t\tatguements: frequency\n");
printf("\t\t\t\toptions (hz):\n");
printf("\t\t\t\t\t10\n");
printf("\t\t\t\t\t25\n");
printf("\t\t\t\t\t50\n");
printf("\t\t\t\t\t100\n");
printf("\t\t\t\t\t200\n");
printf("\t\t\t\t\t500\n");
printf("\n");
printf("File format (Each Column)\n");
printf("\tRHY RHR RHP RKN RAP RAR LHY LHR LHP LKN LAP LAR RSP RSR RSY REB RWY RWR RWP LSP LSR LSY LEB LWY LWR LWP NKY NK1 NK2 WST RF1 RF2 RF3 RF4 RF5 LF1 LF2 LF3 LF4 LF5\n");
printf("\n");
return 0;
}
i++;
}
printf("\n");
printf("-----------------------------\n");
printf("Using file: %s \n",fileName);
printf("Sampling frequency %f\n", 1/((double)interval/(double)NSEC_PER_SEC));
printf("-----------------------------\n");
printf("\n");
/* RT */
struct sched_param param;
/* Declare ourself as a real time task */
param.sched_priority = MY_PRIORITY;
if(sched_setscheduler(0, SCHED_FIFO, ¶m) == -1) {
perror("sched_setscheduler failed");
exit(-1);
}
/* Lock memory */
if(mlockall(MCL_CURRENT|MCL_FUTURE) == -1) {
perror("mlockall failed");
exit(-2);
}
/* Pre-fault our stack */
stack_prefault();
/* open ach channel */
int r = ach_open(&chan_hubo_ref, HUBO_CHAN_REF_NAME , NULL);
assert( ACH_OK == r );
huboLoop();
// pause();
return 0;
}
