Skip to content

Develop #27

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
billiegoose wants to merge 3 commits into
base: develop
Choose a base branch
from
Open

Develop #27

Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
87d996d
Added 'Command not recognized' response to hubo-console
May 6, 2013
ca64b66
Print a final \n before quitting hubo-console and added 'exit' as an …
May 6, 2013
0355fca
hubo-console checks for invalid joint names
May 6, 2013
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
196 changes: 110 additions & 86 deletions src/hubo-console.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -76,15 +76,13 @@ int test(char *s , struct hubo *h);
char* getArg(string s, int argNum);
void hubo_update(struct hubo_ref *h_ref, struct hubo_state *h_state);
int name2mot(char*s, hubo_param_t *h);
double hubo_get(char*s, struct hubo_ref *h, hubo_param_t *p);
void hubo_jmc_beep(hubo_param_t *h, hubo_board_cmd_t *c, char* buff);
void hubo_jmc_home(hubo_param_t *h, hubo_board_cmd_t *c, char* buff);
//char* cmd [] ={ "test","hello", "world", "hell" ,"word", "quit", " " };
void hubo_jmc_home_all(hubo_param_t *h, hubo_board_cmd_t *c, char* buff);
void hubo_enc_reset(hubo_param_t *h, hubo_board_cmd_t *c, int jnt);
void hubo_startup_all(hubo_param_t *h, hubo_board_cmd_t *c, char* buff);
int name2sensor(char* name, hubo_param_t *h);
double hubo_set(char*s, hubo_param_t *p);
char* cmd [] ={ "initialize","fet","initializeAll","homeAll","zero","zeroacc","iniSensors","reset",
"ctrl","ctrlAll","enczero", "goto","get","test","update", "quit","beep", "home"," ",
"resetAll","status","statusAll"}; //,
Expand Down Expand Up @@ -151,53 +149,69 @@ int main() {
printf("--->Hubo Information Updated\n");
}
else if (strcmp(buf0,"get")==0) {
double jRef = hubo_get(buf,&H_ref, &H_param);
char* tmp = getArg(buf,1);
printf(">> %s = %f rad \n",tmp,jRef);
}
else if (strcmp(buf0,"goto")==0) {
int jnt = hubo_set(buf, &H_param);
float f = 0.0;
char* str = getArg(buf,2);
if(sscanf(str, "%f", &f) != 0){ //It's a float.
H_ref.ref[jnt] = (double)f;
int r = ach_put( &chan_hubo_ref, &H_ref, sizeof(H_ref) );
int jnt = name2mot(getArg(buf,1), &H_param);
if( jnt == -1) {
printf("Unrecognized joint '%s'\n",getArg(buf,1));}
else {
double jRef = H_ref.ref[jnt];
char* tmp = getArg(buf,1);
printf(">> %s = %f rad \n",tmp,f);
printf(">> %s = %f rad \n",tmp,jRef);
}
}
else if (strcmp(buf0,"goto")==0) {
int jnt = name2mot(getArg(buf,1), &H_param);
if( jnt == -1) {
printf("Unrecognized joint '%s'\n",getArg(buf,1));}
else {
printf(">> Bad input \n");
float f = 0.0;
char* str = getArg(buf,2);
if(sscanf(str, "%f", &f) != 0){ //It's a float.
H_ref.ref[jnt] = (double)f;
int r = ach_put( &chan_hubo_ref, &H_ref, sizeof(H_ref) );
char* tmp = getArg(buf,1);
printf(">> %s = %f rad \n",tmp,f);
}
else {
printf(">> Bad input \n");
}
}
}
else if (strcmp(buf0,"beep")==0) {
hubo_jmc_beep(&H_param, &H_cmd, buf);
}
else if (strcmp(buf0,"home")==0) {
hubo_jmc_home(&H_param, &H_cmd, buf);
printf("%s - Home \n",getArg(buf,1));
if( name2mot(getArg(buf,1),&H_param) == -1 ) {
printf("Unrecognized joint '%s'\n",getArg(buf,1));}
else {
hubo_jmc_home(&H_param, &H_cmd, buf);
printf("%s - Home \n",getArg(buf,1));
}
}
else if (strcmp(buf0,"homeAll")==0) {
hubo_jmc_home_all(&H_param, &H_cmd, buf);
printf("%s - Home All \n",getArg(buf,1));
tsleep = 5;

}

else if (strcmp(buf0,"ctrl")==0) {
int hOnOff = atof(getArg(buf,2));
if(hOnOff == 0 | hOnOff == 1) {
H_cmd.type = D_CTRL_ON_OFF;
H_cmd.joint = name2mot(getArg(buf,1),&H_param); // set motor num
if( hOnOff == 1)
H_cmd.param[0] = D_ENABLE; // 1 = on
if( hOnOff == 0)
H_cmd.param[0] = D_DISABLE; // 0 = off
if( hOnOff == 1 | hOnOff == 0 )
r = ach_put( &chan_hubo_board_cmd, &H_cmd, sizeof(H_cmd) );
if( hOnOff == 0) {
printf("%s - Turning Off CTRL\n",getArg(buf,1));}
else {
printf("%s - Turning On CTRL\n",getArg(buf,1));}
if( H_cmd.joint == -1) {
printf("Unrecognized joint '%s'\n",getArg(buf,1));}
else {
if( hOnOff == 1)
H_cmd.param[0] = D_ENABLE; // 1 = on
if( hOnOff == 0)
H_cmd.param[0] = D_DISABLE; // 0 = off
if( hOnOff == 1 | hOnOff == 0 )
r = ach_put( &chan_hubo_board_cmd, &H_cmd, sizeof(H_cmd) );
if( hOnOff == 0) {
printf("%s - Turning Off CTRL\n",getArg(buf,1));}
else {
printf("%s - Turning On CTRL\n",getArg(buf,1));}
}
}

}
Expand All @@ -223,25 +237,29 @@ int main() {
else if (strcmp(buf0,"status")==0) {
H_cmd.type = D_GET_STATUS;
H_cmd.joint = name2mot(getArg(buf,1),&H_param); // set motor num
r = ach_put( &chan_hubo_board_cmd, &H_cmd, sizeof(H_cmd) );
printf("%s - Getting Status \n", &H_param.joint[H_cmd.joint].name);
usleep(50*1000);
hubo_update(&H_ref, &H_state);
hubo_joint_status_t e = H_state.status[H_cmd.joint];
printf("Mode : %d \n", H_ref.mode[H_cmd.joint]);
printf("Zeroed : %d \n", H_state.joint[H_cmd.joint].zeroed);
printf("Homed : %d \n", e.homeFlag);
printf("Jam : %d \n", e.jam);
printf("PWM Saturated: %d \n", e.pwmSaturated);
printf("Big Error : %d \n", e.bigError);
printf("Enc Error : %d \n", e.encError);
printf("Drive Fault : %d \n", e.driverFault);
printf("Pos Err (min): %d \n", e.posMinError);
printf("Pos Err (max): %d \n", e.posMaxError);
printf("Velos Error : %d \n", e.velError);
printf("Acc Error : %d \n", e.accError);
printf("Temp Error : %d \n", e.tempError);
printf("Active : %d \n", H_state.joint[H_cmd.joint].active);
if( H_cmd.joint == -1) {
printf("Unrecognized joint '%s'\n",getArg(buf,1));}
else {
r = ach_put( &chan_hubo_board_cmd, &H_cmd, sizeof(H_cmd) );
printf("%s - Getting Status \n", H_param.joint[H_cmd.joint].name);
usleep(50*1000);
hubo_update(&H_ref, &H_state);
hubo_joint_status_t e = H_state.status[H_cmd.joint];
printf("Mode : %d \n", H_ref.mode[H_cmd.joint]);
printf("Zeroed : %d \n", H_state.joint[H_cmd.joint].zeroed);
printf("Homed : %d \n", e.homeFlag);
printf("Jam : %d \n", e.jam);
printf("PWM Saturated: %d \n", e.pwmSaturated);
printf("Big Error : %d \n", e.bigError);
printf("Enc Error : %d \n", e.encError);
printf("Drive Fault : %d \n", e.driverFault);
printf("Pos Err (min): %d \n", e.posMinError);
printf("Pos Err (max): %d \n", e.posMaxError);
printf("Velos Error : %d \n", e.velError);
printf("Acc Error : %d \n", e.accError);
printf("Temp Error : %d \n", e.tempError);
printf("Active : %d \n", H_state.joint[H_cmd.joint].active);
}
}
else if (strcmp(buf0,"statusAll")==0) {
printf(" - Getting Status All\n");
Expand All @@ -250,7 +268,7 @@ int main() {
for( int i = 0; i < HUBO_JOINT_COUNT; i++) {
hubo_joint_status_t e = H_state.status[i];
printf("%s - %d %d %d %d %d %d %d %d %d %d %d %d %d %d \n"
, &H_param.joint[i].name
, H_param.joint[i].name
, H_ref.mode[i]
, H_state.joint[i].zeroed
, e.homeFlag
Expand All @@ -270,15 +288,19 @@ int main() {
else if (strcmp(buf0,"resetAll")==0) {
for( int i = 0; i < HUBO_JOINT_COUNT; i++) {
hubo_enc_reset(&H_param, &H_cmd, i);
printf("%s - Resetting Encoder \n", &H_param.joint[i].name);
printf("%s - Resetting Encoder \n", H_param.joint[i].name);
usleep(10*1000);
}
}

else if (strcmp(buf0,"reset")==0) {
int jnt = name2mot(getArg(buf, 1), &H_param);
hubo_enc_reset(&H_param, &H_cmd, jnt);
printf("%s - Resetting Encoder \n",getArg(buf,1));
if( jnt == -1 ) {
printf("Unrecognized joint '%s'\n",getArg(buf,1));}
else {
hubo_enc_reset(&H_param, &H_cmd, jnt);
printf("%s - Resetting Encoder \n",getArg(buf,1));
}
}
else if (strcmp(buf0,"startup")==0) {
hubo_startup_all(&H_param, &H_cmd, buf);
Expand All @@ -290,39 +312,51 @@ int main() {
if(onOrOff == 0 | onOrOff == 1) {
H_cmd.type = D_CTRL_SWITCH;
H_cmd.joint = name2mot(getArg(buf,1),&H_param); // set motor num
if(onOrOff==1) // 1 = on, 0 = 0ff
H_cmd.param[0] = D_ENABLE;
else if(onOrOff==0)
H_cmd.param[0] = D_DISABLE;
r = ach_put( &chan_hubo_board_cmd, &H_cmd, sizeof(H_cmd) );
if(onOrOff == 0) {
printf("%s - Turning Off CTRL\n",getArg(buf,1));}
if( H_cmd.joint == -1) {
printf("Unrecognized joint '%s'\n",getArg(buf,1));}
else {
printf("%s - Turning On CTRL\n",getArg(buf,1));}
if(onOrOff==1) // 1 = on, 0 = 0ff
H_cmd.param[0] = D_ENABLE;
else if(onOrOff==0)
H_cmd.param[0] = D_DISABLE;
r = ach_put( &chan_hubo_board_cmd, &H_cmd, sizeof(H_cmd) );
if(onOrOff == 0) {
printf("%s - Turning Off CTRL\n",getArg(buf,1));}
else {
printf("%s - Turning On CTRL\n",getArg(buf,1));}
}
}
}
else if (strcmp(buf0,"fet")==0) {
int onOrOff = atof(getArg(buf,2));
if(onOrOff == 0 | onOrOff == 1) {
H_cmd.type = D_FET_SWITCH;
H_cmd.joint = name2mot(getArg(buf,1),&H_param); // set motor num
if(onOrOff==1)
H_cmd.param[0] = D_ENABLE;
else if(onOrOff==0)
H_cmd.param[0] = D_DISABLE;
int r = ach_put( &chan_hubo_board_cmd, &H_cmd, sizeof(H_cmd) );
if(onOrOff == 0) {
printf("%s - Turning Off FET\n",getArg(buf,1));}
if( H_cmd.joint == -1) {
printf("Unrecognized joint '%s'\n",getArg(buf,1));}
else {
printf("%s - Turning On FET\n",getArg(buf,1));}
if(onOrOff==1)
H_cmd.param[0] = D_ENABLE;
else if(onOrOff==0)
H_cmd.param[0] = D_DISABLE;
int r = ach_put( &chan_hubo_board_cmd, &H_cmd, sizeof(H_cmd) );
if(onOrOff == 0) {
printf("%s - Turning Off FET\n",getArg(buf,1));}
else {
printf("%s - Turning On FET\n",getArg(buf,1));}
}
}
}
else if (strcmp(buf0,"initialize")==0) {
H_cmd.type = D_JMC_INITIALIZE;
H_cmd.joint = name2mot(getArg(buf,1),&H_param); // set motor num
//C.val[0] = atof(getArg(buf,2));
int r = ach_put( &chan_hubo_board_cmd, &H_cmd, sizeof(H_cmd) );
printf("%s - Initialize \n",getArg(buf,1));
if( H_cmd.joint == -1) {
printf("Unrecognized joint '%s'\n",getArg(buf,1));}
else {
//C.val[0] = atof(getArg(buf,2));
int r = ach_put( &chan_hubo_board_cmd, &H_cmd, sizeof(H_cmd) );
printf("%s - Initialize \n",getArg(buf,1));
}
}
else if (strcmp(buf0,"initializeAll")==0) {
H_cmd.type = D_JMC_INITIALIZE_ALL;
Expand Down Expand Up @@ -366,8 +400,13 @@ int main() {

}
/* Quit */
else if (strcmp(buf0,"quit")==0)
else if (strcmp(buf0,"quit")==0 || strcmp(buf0,"exit")==0) {
printf("\n");
break;
}
else {
printf("Command '%s' not recognized.\n",getArg(buf,0));
}
if (buf[0]!=0)
add_history(buf);
sleep(tsleep); // sleep for tsleep sec
Expand All @@ -376,21 +415,6 @@ int main() {
return 0;
}

double hubo_get(char*s, struct hubo_ref *h, hubo_param_t *p) {

/* get joint number */
int jointNo = name2mot(getArg(s,1),p);

return h->ref[jointNo];
}

double hubo_set(char*s, hubo_param_t *p) {

/* get joint number */
int jointNo = name2mot(getArg(s,1),p);
return jointNo;
}

void hubo_jmc_beep(hubo_param_t *h, hubo_board_cmd_t *c, char* buff) {
/* make beep */
c->type = D_JMC_BEEP;
Expand Down