Skip to content

Commit

Permalink
Improve command prompt parsing
Browse files Browse the repository at this point in the history 
Signed-off-by: Javier Balloffet <[email protected]>
  • Loading branch information
@jballoffet
jballoffet committed Dec 27, 2023
1 parent bd5034f commit 9dd5f0e
Show file tree
Hide file tree
Showing 4 changed files with 103 additions and 107 deletions.
55 changes: 38 additions & 17 deletions andino_firmware/src/app.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -126,8 +126,8 @@ void App::setup() {
}

void App::loop() {
// Process command shell.
shell_.process();
// Process command prompt input.
shell_.process_input();

// Run a PID calculation at the appropriate intervals
if (millis() > nextPID) {
Expand Down Expand Up @@ -155,35 +155,47 @@ void App::loop() {
}
}

void App::cmd_unknown_cb(const char*, const char*) { Serial.println("Unknown command."); }
void App::cmd_unknown_cb(int, char**) { Serial.println("Unknown command."); }

void App::cmd_read_analog_gpio_cb(const char* arg1, const char*) {
const int pin = atoi(arg1);
void App::cmd_read_analog_gpio_cb(int argc, char** argv) {
if (argc < 2) {
return;
}

const int pin = atoi(argv[1]);
Serial.println(analogRead(pin));
}

void App::cmd_read_digital_gpio_cb(const char* arg1, const char*) {
const int pin = atoi(arg1);
void App::cmd_read_digital_gpio_cb(int argc, char** argv) {
if (argc < 2) {
return;
}

const int pin = atoi(argv[1]);
Serial.println(digitalRead(pin));
}

void App::cmd_read_encoders_cb(const char*, const char*) {
void App::cmd_read_encoders_cb(int, char**) {
Serial.print(left_encoder_.read());
Serial.print(" ");
Serial.println(right_encoder_.read());
}

void App::cmd_reset_encoders_cb(const char*, const char*) {
void App::cmd_reset_encoders_cb(int, char**) {
left_encoder_.reset();
right_encoder_.reset();
left_pid_controller_.reset(left_encoder_.read());
right_pid_controller_.reset(right_encoder_.read());
Serial.println("OK");
}

void App::cmd_set_motors_speed_cb(const char* arg1, const char* arg2) {
const int left_motor_speed = atoi(arg1);
const int right_motor_speed = atoi(arg2);
void App::cmd_set_motors_speed_cb(int argc, char** argv) {
if (argc < 3) {
return;
}

const int left_motor_speed = atoi(argv[1]);
const int right_motor_speed = atoi(argv[2]);

// Reset the auto stop timer.
lastMotorCommand = millis();
Expand All @@ -206,9 +218,13 @@ void App::cmd_set_motors_speed_cb(const char* arg1, const char* arg2) {
Serial.println("OK");
}

void App::cmd_set_motors_pwm_cb(const char* arg1, const char* arg2) {
const int left_motor_pwm = atoi(arg1);
const int right_motor_pwm = atoi(arg2);
void App::cmd_set_motors_pwm_cb(int argc, char** argv) {
if (argc < 3) {
return;
}

const int left_motor_pwm = atoi(argv[1]);
const int right_motor_pwm = atoi(argv[2]);

// Reset the auto stop timer.
lastMotorCommand = millis();
Expand All @@ -222,14 +238,19 @@ void App::cmd_set_motors_pwm_cb(const char* arg1, const char* arg2) {
Serial.println("OK");
}

void App::cmd_set_pid_tuning_gains_cb(const char* arg1, const char*) {
void App::cmd_set_pid_tuning_gains_cb(int argc, char** argv) {
// TODO(jballoffet): Refactor to expect command multiple arguments.
if (argc < 2) {
return;
}

int i = 0;
char arg[20];
char* str;
int pid_args[4];

// Example: "u 30:20:10:50".
strcpy(arg, arg1);
strcpy(arg, argv[1]);
char* p = arg;
while ((str = strtok_r(p, ":", &p)) != NULL) {
pid_args[i] = atoi(str);
Expand Down
24 changes: 8 additions & 16 deletions andino_firmware/src/app.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -50,36 +50,28 @@ class App {

private:
/// Callback method for an unknown command (default).
// TODO(jballoffet): Parse arguments within callback method.
static void cmd_unknown_cb(const char* arg1, const char* arg2);
static void cmd_unknown_cb(int argc, char** argv);

/// Callback method for the `Commands::kReadAnalogGpio` command.
// TODO(jballoffet): Parse arguments within callback method.
static void cmd_read_analog_gpio_cb(const char* arg1, const char* arg2);
static void cmd_read_analog_gpio_cb(int argc, char** argv);

/// Callback method for the `Commands::kReadDigitalGpio` command.
// TODO(jballoffet): Parse arguments within callback method.
static void cmd_read_digital_gpio_cb(const char* arg1, const char* arg2);
static void cmd_read_digital_gpio_cb(int argc, char** argv);

/// Callback method for the `Commands::kReadEncoders` command.
// TODO(jballoffet): Parse arguments within callback method.
static void cmd_read_encoders_cb(const char* arg1, const char* arg2);
static void cmd_read_encoders_cb(int argc, char** argv);

/// Callback method for the `Commands::kResetEncoders` command.
// TODO(jballoffet): Parse arguments within callback method.
static void cmd_reset_encoders_cb(const char* arg1, const char* arg2);
static void cmd_reset_encoders_cb(int argc, char** argv);

/// Callback method for the `Commands::kSetMotorsSpeed` command.
// TODO(jballoffet): Parse arguments within callback method.
static void cmd_set_motors_speed_cb(const char* arg1, const char* arg2);
static void cmd_set_motors_speed_cb(int argc, char** argv);

/// Callback method for the `Commands::kSetMotorsPwm` command.
// TODO(jballoffet): Parse arguments within callback method.
static void cmd_set_motors_pwm_cb(const char* arg1, const char* arg2);
static void cmd_set_motors_pwm_cb(int argc, char** argv);

/// Callback method for the `Commands::kSetPidsTuningGains` command.
// TODO(jballoffet): Parse arguments within callback method.
static void cmd_set_pid_tuning_gains_cb(const char* arg1, const char* arg2);
static void cmd_set_pid_tuning_gains_cb(int argc, char** argv);

/// Application command shell.
static Shell shell_;
Expand Down
87 changes: 38 additions & 49 deletions andino_firmware/src/shell.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -48,69 +48,58 @@ void Shell::register_command(const char* name, CommandCallback callback) {
commands_[commands_count_++] = command;
}

// TODO(jballoffet): Modify parsing method to allow command name to be a string.
void Shell::process() {
void Shell::process_input() {
while (stream_->available() > 0) {
// Read the next character
char chr = stream_->read();

// Terminate a command with a CR
if (chr == 13) {
if (args_count_ == 1) {
arg1_[arg_index_] = 0;
} else if (args_count_ == 2) {
arg2_[arg_index_] = 0;
}
execute_callback();
reset();
}
// Use spaces to delimit parts of the command
else if (chr == ' ') {
// Step through the arguments
if (args_count_ == 0) {
args_count_ = 1;
} else if (args_count_ == 1) {
arg1_[arg_index_] = 0;
args_count_ = 2;
arg_index_ = 0;
}
continue;
} else {
if (args_count_ == 0) {
// The first arg is the single-letter command
command_ = chr;
} else if (args_count_ == 1) {
// Subsequent arguments can be more than one character
arg1_[arg_index_] = chr;
arg_index_++;
} else if (args_count_ == 2) {
arg2_[arg_index_] = chr;
arg_index_++;
}
const char input = stream_->read();

switch (input) {
case '\r':
// Terminate command prompt message and parse it.
message_buffer_[message_index_++] = '\0';
parse_message();
// Reset message buffer.
message_index_ = 0;
break;

case '\n':
// Ignore newline characters.
break;

default:
message_buffer_[message_index_++] = input;
// Prevent buffer overflow.
if (message_index_ >= kCommandPromptLengthMax) {
message_index_ = 0;
}

break;
}
}
}

void Shell::reset() {
command_ = 0;
memset(arg1_, 0, sizeof(arg1_));
memset(arg2_, 0, sizeof(arg2_));
args_count_ = 0;
arg_index_ = 0;
void Shell::parse_message() {
char* argv[kCommandArgMax];
int argc = 0;

argv[argc] = strtok(message_buffer_, " ");
while (argv[argc] != NULL && argc < (kCommandArgMax - 1)) {
argv[++argc] = strtok(NULL, " ");
}

execute_callback(argc, argv);
}

// TODO(jballoffet): Modify parsing method to allow command name to be a string.
void Shell::execute_callback() {
void Shell::execute_callback(int argc, char** argv) {
for (size_t i = 0; i < commands_count_; i++) {
if (command_ == commands_[i].name[0]) {
commands_[i].callback(arg1_, arg2_);
if (!strcmp(argv[0], commands_[i].name)) {
commands_[i].callback(argc, argv);
return;
}
}

// Unknown command received, executing default callback.
if (default_callback_ != nullptr) {
default_callback_(arg1_, arg2_);
default_callback_(argc, argv);
}
}

Expand Down
44 changes: 19 additions & 25 deletions andino_firmware/src/shell.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,7 +39,7 @@ namespace andino {
class Shell {
public:
/// @brief Command callback type.
typedef void (*CommandCallback)(const char*, const char*);
typedef void (*CommandCallback)(int argc, char** argv);

/// @brief Initializes the shell.
///
Expand All @@ -59,17 +59,11 @@ class Shell {

/// @brief Processes the available input at the command prompt (if any). Meant to be called
/// continously.
void process();
void process_input();

private:
/// Maximum number of commands that can be registered.
static constexpr int kCommandsMax{10};

/// Maximum command name length.
static constexpr int kCommandNameLengthMax{16};

/// Maximum command argument length.
static constexpr int kCommandArgLengthMax{16};
static constexpr int kCommandNameLengthMax{8};

/// Command registry entry definition.
struct Command {
Expand All @@ -79,11 +73,20 @@ class Shell {
CommandCallback callback;
};

/// Resets the command prompt processing variables.
void reset();
/// Maximum number of commands that can be registered.
static constexpr int kCommandsMax{16};

/// Maximum number of command arguments that can be processed.
static constexpr int kCommandArgMax{16};

/// Maximum command prompt message length.
static constexpr int kCommandPromptLengthMax{64};

/// Parses the command prompt message.
void parse_message();

/// Executes the corresponding command callback function.
void execute_callback();
void execute_callback(int argc, char** argv);

/// Data stream.
Stream* stream_{nullptr};
Expand All @@ -97,20 +100,11 @@ class Shell {
/// Number of registered commands.
size_t commands_count_{0};

/// Holds the received command.
char command_;

/// Holds the first received command argument.
char arg1_[kCommandArgLengthMax];

/// Holds the second received command argument.
char arg2_[kCommandArgLengthMax];

/// Argument array index.
int arg_index_{0};
/// Command prompt message.
char message_buffer_[kCommandPromptLengthMax];

/// Number of received command arguments.
int args_count_{0};
/// Command prompt message index.
int message_index_{0};
};

} // namespace andino

0 comments on commit 9dd5f0e

Please sign in to comment.