Skip to content

Commit

Permalink
Add Shell class (#193)
Browse files Browse the repository at this point in the history
Signed-off-by: Javier Balloffet <[email protected]>
  • Loading branch information
jballoffet authored Jan 31, 2024
1 parent 9f7793d commit c795db8
Show file tree
Hide file tree
Showing 5 changed files with 276 additions and 120 deletions.
121 changes: 24 additions & 97 deletions andino_firmware/src/app.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -71,6 +71,7 @@
#include "hw.h"
#include "motor.h"
#include "pid.h"
#include "shell.h"

// TODO(jballoffet): Move this variables to a different module.

Expand All @@ -79,22 +80,10 @@ unsigned long nextPID = andino::Constants::kPidPeriod;

long lastMotorCommand = andino::Constants::kAutoStopWindow;

// A pair of varibles to help parse serial commands
int arg = 0;
int index = 0;

// Variable to hold an input character
char chr;

// Variable to hold the current single-character command
char cmd;

// Character arrays to hold the first and second arguments
char argv1[16];
char argv2[16];

namespace andino {

Shell App::shell_;

Motor App::left_motor_(Hw::kLeftMotorEnableGpioPin, Hw::kLeftMotorForwardGpioPin,
Hw::kLeftMotorBackwardGpioPin);
Motor App::right_motor_(Hw::kRightMotorEnableGpioPin, Hw::kRightMotorForwardGpioPin,
Expand Down Expand Up @@ -123,47 +112,22 @@ void App::setup() {

left_pid_controller_.reset(left_encoder_.read());
right_pid_controller_.reset(right_encoder_.read());

// Initialize command shell.
shell_.init(Serial);
shell_.set_default_callback(cmd_unknown_cb);
shell_.register_command(Commands::kReadAnalogGpio, cmd_read_analog_gpio_cb);
shell_.register_command(Commands::kReadDigitalGpio, cmd_read_digital_gpio_cb);
shell_.register_command(Commands::kReadEncoders, cmd_read_encoders_cb);
shell_.register_command(Commands::kResetEncoders, cmd_reset_encoders_cb);
shell_.register_command(Commands::kSetMotorsSpeed, cmd_set_motors_speed_cb);
shell_.register_command(Commands::kSetMotorsPwm, cmd_set_motors_pwm_cb);
shell_.register_command(Commands::kSetPidsTuningGains, cmd_set_pid_tuning_gains_cb);
}

void App::loop() {
while (Serial.available() > 0) {
// Read the next character
chr = Serial.read();

// Terminate a command with a CR
if (chr == 13) {
if (arg == 1)
argv1[index] = 0;
else if (arg == 2)
argv2[index] = 0;
run_command();
reset_command();
}
// Use spaces to delimit parts of the command
else if (chr == ' ') {
// Step through the arguments
if (arg == 0)
arg = 1;
else if (arg == 1) {
argv1[index] = 0;
arg = 2;
index = 0;
}
continue;
} else {
if (arg == 0) {
// The first arg is the single-letter command
cmd = chr;
} else if (arg == 1) {
// Subsequent arguments can be more than one character
argv1[index] = chr;
index++;
} else if (arg == 2) {
argv2[index] = chr;
index++;
}
}
}
// Process command shell.
shell_.process();

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

void App::reset_command() {
cmd = 0;
memset(argv1, 0, sizeof(argv1));
memset(argv2, 0, sizeof(argv2));
arg = 0;
index = 0;
}

void App::run_command() {
switch (cmd) {
case Commands::kReadAnalogGpio:
cmd_read_analog_gpio(argv1, argv2);
break;
case Commands::kReadDigitalGpio:
cmd_read_digital_gpio(argv1, argv2);
break;
case Commands::kReadEncoders:
cmd_read_encoders(argv1, argv2);
break;
case Commands::kResetEncoders:
cmd_reset_encoders(argv1, argv2);
break;
case Commands::kSetMotorsSpeed:
cmd_set_motors_speed(argv1, argv2);
break;
case Commands::kSetMotorsPwm:
cmd_set_motors_pwm(argv1, argv2);
break;
case Commands::kSetPidsTuningGains:
cmd_set_pid_tuning_gains(argv1, argv2);
break;
default:
cmd_unknown(argv1, argv2);
break;
}
}

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

void App::cmd_read_analog_gpio(const char* arg1, const char*) {
void App::cmd_read_analog_gpio_cb(const char* arg1, const char*) {
const int pin = atoi(arg1);
Serial.println(analogRead(pin));
}

void App::cmd_read_digital_gpio(const char* arg1, const char*) {
void App::cmd_read_digital_gpio_cb(const char* arg1, const char*) {
const int pin = atoi(arg1);
Serial.println(digitalRead(pin));
}

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

void App::cmd_reset_encoders(const char*, const char*) {
void App::cmd_reset_encoders_cb(const char*, const 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(const char* arg1, const char* arg2) {
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);

Expand All @@ -279,7 +206,7 @@ void App::cmd_set_motors_speed(const char* arg1, const char* arg2) {
Serial.println("OK");
}

void App::cmd_set_motors_pwm(const char* arg1, const char* arg2) {
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);

Expand All @@ -295,7 +222,7 @@ void App::cmd_set_motors_pwm(const char* arg1, const char* arg2) {
Serial.println("OK");
}

void App::cmd_set_pid_tuning_gains(const char* arg1, const char*) {
void App::cmd_set_pid_tuning_gains_cb(const char* arg1, const char*) {
static constexpr int kSizePidArgs{4};
int i = 0;
char arg[20];
Expand Down
28 changes: 12 additions & 16 deletions andino_firmware/src/app.h
Original file line number Diff line number Diff line change
Expand Up @@ -32,6 +32,7 @@
#include "encoder.h"
#include "motor.h"
#include "pid.h"
#include "shell.h"

namespace andino {

Expand All @@ -48,45 +49,40 @@ class App {
static void loop();

private:
/// Clears the current command parameters.
// TODO(jballoffet): Move this method to a different module.
static void reset_command();

/// Runs a command.
// TODO(jballoffet): Move this method to a different module.
static void run_command();

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

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

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

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

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

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

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

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

/// Application command shell.
static Shell shell_;

/// Motors (one per wheel).
static Motor left_motor_;
Expand Down
14 changes: 7 additions & 7 deletions andino_firmware/src/commands.h
Original file line number Diff line number Diff line change
Expand Up @@ -69,19 +69,19 @@ namespace andino {
/// @brief CLI commands.
struct Commands {
/// @brief Reads an analog GPIO.
static constexpr char kReadAnalogGpio{'a'};
static constexpr const char* kReadAnalogGpio{"a"};
/// @brief Reads a digital GPIO.
static constexpr char kReadDigitalGpio{'d'};
static constexpr const char* kReadDigitalGpio{"d"};
/// @brief Reads the encoders tick count values.
static constexpr char kReadEncoders{'e'};
static constexpr const char* kReadEncoders{"e"};
/// @brief Sets the encoders ticks count to zero.
static constexpr char kResetEncoders{'r'};
static constexpr const char* kResetEncoders{"r"};
/// @brief Sets the motors speed [ticks/s].
static constexpr char kSetMotorsSpeed{'m'};
static constexpr const char* kSetMotorsSpeed{"m"};
/// @brief Sets the motors PWM value [duty range: 0-255].
static constexpr char kSetMotorsPwm{'o'};
static constexpr const char* kSetMotorsPwm{"o"};
/// @brief Sets the PIDs tuning gains [format: "kp:kd:ki:ko"].
static constexpr char kSetPidsTuningGains{'u'};
static constexpr const char* kSetPidsTuningGains{"u"};
};

} // namespace andino
117 changes: 117 additions & 0 deletions andino_firmware/src/shell.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,117 @@
// BSD 3-Clause License
//
// Copyright (c) 2023, Ekumen Inc.
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// 1. Redistributions of source code must retain the above copyright notice, this
// list of conditions and the following disclaimer.
//
// 2. Redistributions in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
//
// 3. Neither the name of the copyright holder nor the names of its
// contributors may be used to endorse or promote products derived from
// this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "shell.h"

#include <string.h>

namespace andino {

void Shell::init(Stream& stream) { stream_ = &stream; }

void Shell::set_default_callback(CommandCallback callback) { default_callback_ = callback; }

void Shell::register_command(const char* name, CommandCallback callback) {
if (commands_count_ >= kCommandsMax) {
return;
}

Command command;
strcpy(command.name, name);
command.callback = callback;
commands_[commands_count_++] = command;
}

// TODO(jballoffet): Modify parsing method to allow command name to be a string.
void Shell::process() {
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_++;
}
}
}
}

void Shell::reset() {
command_ = 0;
memset(arg1_, 0, sizeof(arg1_));
memset(arg2_, 0, sizeof(arg2_));
args_count_ = 0;
arg_index_ = 0;
}

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

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

} // namespace andino
Loading

0 comments on commit c795db8

Please sign in to comment.