void HuboInitWidget::initializeAchStructs()
{
memset(&h_state, 0, sizeof(h_state));
memset(&h_param, 0, sizeof(h_param));
memset(&h_cmd, 0, sizeof(h_cmd));
hubo_pwm_gains_t temp;
setJointParams(&h_param, &h_state, &temp);
setSensorDefaults(&h_param);
for(int i=0; i<HUBO_JOINT_COUNT; i++)
if( strcmp(h_param.joint[i].name, "") == 0 )
sprintf( h_param.joint[i].name, "N/A\0" );
}
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;
}
