/* Release resources */
void release_command(command *cmd) {
if(cmd->scmd)
if(cmd->scmd->in || cmd->scmd->out || cmd->scmd->err)
free(cmd->scmd->tokens);
if(cmd->cmd1)
release_command(cmd->cmd1);
if(cmd->cmd2)
release_command(cmd->cmd2);
}
int main(int argc, char** argv) {
char cwd[MAX_DIRNAME]; /* Current working directory */
char command_line[MAX_COMMAND]; /* The command */
char *tokens[MAX_TOKEN]; /* Command tokens (program name,
* parameters, pipe, etc.) */
while (1) {
/* Display prompt */
getcwd(cwd, MAX_DIRNAME-1);
printf("%s> ", cwd);
/* Read the command line */
fgets(command_line, MAX_COMMAND, stdin);
/* Strip the new line character */
if (command_line[strlen(command_line) - 1] == '\n') {
command_line[strlen(command_line) - 1] = '\0';
}
/* Parse the command into tokens */
parse_line(command_line, tokens);
/* Check for empty command */
if (!(*tokens)) {
continue;
}
/* Construct chain of commands, if multiple commands */
command *cmd = construct_command(tokens);
// print_command(cmd, 0);
int exitcode = 0;
if (cmd->scmd) {
exitcode = execute_simple_command(cmd->scmd);
if (exitcode == -1) {
release_command(cmd);
break;
}
}
else {
exitcode = execute_complex_command(cmd);
if (exitcode == -1) {
release_command(cmd);
break;
}
}
release_command(cmd);
}
return 0;
}
void release_commands(cmdEntry* cmds)
{
int i;
for( i = 0; i < anz_commands; i++)
{
release_command(cmds[i]);
}
free(cmds);
}
void M3LedX2XNEcShm::SetCommandFromSds(unsigned char * data)
{
M3LedX2XNEcShmSdsCommand * sds = (M3LedX2XNEcShmSdsCommand *) data;
request_command();
memcpy(&command_from_sds, sds, GetCommandSdsSize());
release_command();
int64_t dt = GetBaseStatus()->timestamp()-command_from_sds.timestamp; // microseconds
bool shm_timeout = ABS(dt) > (timeout*1000);
/* if (tmp_cnt++ == 200)
{
M3_DEBUG("hi\n");
tmp_cnt = 0;
}*/
if (led_x2xn != NULL)
{
if (shm_timeout)
{
/* if (tmp_cnt++ == 200)
{
M3_DEBUG("hi\n");
tmp_cnt = 0;
}*/
((M3LedX2XNEcCommand*)led_x2xn->GetCommand())->set_enable_a(0);
((M3LedX2XNEcCommand*)led_x2xn->GetCommand())->set_enable_b(0);
} else {
/* if (tmp_cnt++ == 200)
{
M3_DEBUG("here\n");
tmp_cnt = 0;
}*/
((M3LedX2XNEcCommand*)led_x2xn->GetCommand())->set_enable_a(command_from_sds.enable_a);
((M3LedX2XNEcCommand*)led_x2xn->GetCommand())->set_enable_b(command_from_sds.enable_b);
for (int i = 0; i < NUM_PER_BRANCH; i++)
{
((M3LedX2XNEcCommand*)led_x2xn->GetCommand())->mutable_branch_a()->set_r(i,command_from_sds.r_a[i]);
((M3LedX2XNEcCommand*)led_x2xn->GetCommand())->mutable_branch_a()->set_g(i,command_from_sds.g_a[i]);
((M3LedX2XNEcCommand*)led_x2xn->GetCommand())->mutable_branch_a()->set_b(i,command_from_sds.b_a[i]);
((M3LedX2XNEcCommand*)led_x2xn->GetCommand())->mutable_branch_b()->set_r(i,command_from_sds.r_b[i]);
((M3LedX2XNEcCommand*)led_x2xn->GetCommand())->mutable_branch_b()->set_g(i,command_from_sds.g_b[i]);
((M3LedX2XNEcCommand*)led_x2xn->GetCommand())->mutable_branch_b()->set_b(i,command_from_sds.b_b[i]);
}
}
}
}
void M3TorqueShm::SetCommandFromSds(unsigned char * data)
{
M3TorqueShmSdsCommand * sds = (M3TorqueShmSdsCommand *) data;
request_command();
memcpy(&command_from_sds, sds, GetCommandSdsSize());
release_command();
command.set_timestamp(command_from_sds.timestamp);
int64_t dt = GetBaseStatus()->timestamp()-command.timestamp(); // microseconds
bool shm_timeout = ABS(dt) > (timeout*1000);
for (int i = 0; i < bot->GetNdof(RIGHT_ARM); i++)
{
command.mutable_right_arm()->set_tq_desired(i, command_from_sds.right_arm.tq_desired[i]);
if (shm_timeout)
{
bot->DisableTorqueShmRightArm();
} else{
bot->EnableTorqueShmRightArm();
bot->SetTorqueSharedMem_mNm(RIGHT_ARM, i, command.right_arm().tq_desired(i));
}
}
for (int i = 0; i < bot->GetNdof(LEFT_ARM); i++)
{
command.mutable_left_arm()->set_tq_desired(i, command_from_sds.left_arm.tq_desired[i]);
if (shm_timeout)
{
bot->DisableTorqueShmLeftArm();
} else{
bot->EnableTorqueShmLeftArm();
bot->SetTorqueSharedMem_mNm(LEFT_ARM, i, command.left_arm().tq_desired(i));
}
}
for (int i = 0; i < bot->GetNdof(TORSO); i++)
{
command.mutable_torso()->set_tq_desired(i, command_from_sds.torso.tq_desired[i]);
if (shm_timeout)
{
bot->DisableTorqueShmTorso();
} else{
bot->EnableTorqueShmTorso();
bot->SetTorqueSharedMem_mNm(TORSO, i, command.torso().tq_desired(i));
}
}
}
int main(int argc, char** argv) {
char cwd[MAX_DIRNAME]; /* Current working directory */
char command_line[MAX_COMMAND]; /* The command */
char *tokens[MAX_TOKEN]; /* Command tokens (program name, parameters, pipe, etc.) */
while (1) {
/* Display prompt */
getcwd(cwd, MAX_DIRNAME-1);
printf("%s> ", cwd);
/* Read the command line */
gets(command_line);
/* Parse the command into tokens */
parse_line(command_line, tokens);
/* Empty command */
if (!(*tokens))
continue;
/* Exit */
if (strcmp(tokens[0], "exit") == 0)
exit(0);
/* Construct chain of commands, if multiple commands */
command *cmd = construct_command(tokens);
int exitcode = 0;
if (cmd->scmd) {
exitcode = execute_simple_command(cmd->scmd);
if (exitcode == -1)
break;
}
else {
exitcode = execute_complex_command(cmd);
if (exitcode == -1)
break;
}
release_command(cmd);
}
return 0;
}
