From f0ab17d6524326534763e2b0d9b11da1e99485b0 Mon Sep 17 00:00:00 2001 From: Javier Balloffet Date: Wed, 27 Dec 2023 01:10:49 +0100 Subject: [PATCH 01/23] Add Shell class Signed-off-by: Javier Balloffet --- andino_firmware/src/app.cpp | 121 +++++++-------------------------- andino_firmware/src/app.h | 28 ++++---- andino_firmware/src/commands.h | 14 ++-- andino_firmware/src/shell.cpp | 117 +++++++++++++++++++++++++++++++ andino_firmware/src/shell.h | 116 +++++++++++++++++++++++++++++++ 5 files changed, 276 insertions(+), 120 deletions(-) create mode 100644 andino_firmware/src/shell.cpp create mode 100644 andino_firmware/src/shell.h diff --git a/andino_firmware/src/app.cpp b/andino_firmware/src/app.cpp index faf41ef7..f7d97361 100644 --- a/andino_firmware/src/app.cpp +++ b/andino_firmware/src/app.cpp @@ -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. @@ -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, @@ -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) { @@ -191,62 +155,25 @@ 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()); @@ -254,7 +181,7 @@ void App::cmd_reset_encoders(const char*, const char*) { 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); @@ -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); @@ -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]; diff --git a/andino_firmware/src/app.h b/andino_firmware/src/app.h index b0745f6e..1c125695 100644 --- a/andino_firmware/src/app.h +++ b/andino_firmware/src/app.h @@ -32,6 +32,7 @@ #include "encoder.h" #include "motor.h" #include "pid.h" +#include "shell.h" namespace andino { @@ -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_; diff --git a/andino_firmware/src/commands.h b/andino_firmware/src/commands.h index 43efa111..ff3e5ac5 100644 --- a/andino_firmware/src/commands.h +++ b/andino_firmware/src/commands.h @@ -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 diff --git a/andino_firmware/src/shell.cpp b/andino_firmware/src/shell.cpp new file mode 100644 index 00000000..736c4f3b --- /dev/null +++ b/andino_firmware/src/shell.cpp @@ -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 + +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 diff --git a/andino_firmware/src/shell.h b/andino_firmware/src/shell.h new file mode 100644 index 00000000..3b988f83 --- /dev/null +++ b/andino_firmware/src/shell.h @@ -0,0 +1,116 @@ +// 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. +#pragma once + +#include + +#include "Arduino.h" + +namespace andino { + +/// @brief This class interprets and processes the commands entered by the user. +class Shell { + public: + /// @brief Command callback type. + typedef void (*CommandCallback)(const char*, const char*); + + /// @brief Initializes the shell. + /// + /// @param stream Data stream. + void init(Stream& stream); + + /// @brief Sets the default callback for unknown commands. + /// + /// @param callback Callback function. + void set_default_callback(CommandCallback callback); + + /// @brief Adds a command registry entry to the shell. + /// + /// @param name Command name. + /// @param callback Callback function. + void register_command(const char* name, CommandCallback callback); + + /// @brief Processes the available input at the command prompt (if any). Meant to be called + /// continously. + void process(); + + 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}; + + /// Command registry entry definition. + struct Command { + /// Command name. + char name[kCommandNameLengthMax]; + /// Callback function. + CommandCallback callback; + }; + + /// Resets the command prompt processing variables. + void reset(); + + /// Executes the corresponding command callback function. + void execute_callback(); + + /// Data stream. + Stream* stream_{nullptr}; + + /// Default callback for unknown commands. + CommandCallback default_callback_{nullptr}; + + /// Command registry. + Command commands_[kCommandsMax]; + + /// 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}; + + /// Number of received command arguments. + int args_count_{0}; +}; + +} // namespace andino From dfaa85df79b0f9a36ebe604143c8fee9cb3e36cd Mon Sep 17 00:00:00 2001 From: Javier Balloffet Date: Wed, 27 Dec 2023 17:45:00 +0100 Subject: [PATCH 02/23] Improve command prompt parsing Signed-off-by: Javier Balloffet --- andino_firmware/src/app.cpp | 55 +++++++++++++++------- andino_firmware/src/app.h | 24 ++++------ andino_firmware/src/shell.cpp | 86 +++++++++++++++-------------------- andino_firmware/src/shell.h | 44 ++++++++---------- 4 files changed, 102 insertions(+), 107 deletions(-) diff --git a/andino_firmware/src/app.cpp b/andino_firmware/src/app.cpp index f7d97361..82d3c900 100644 --- a/andino_firmware/src/app.cpp +++ b/andino_firmware/src/app.cpp @@ -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) { @@ -155,25 +155,33 @@ 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()); @@ -181,9 +189,13 @@ void App::cmd_reset_encoders_cb(const char*, const char*) { 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(); @@ -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(); @@ -222,7 +238,12 @@ 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; + } + static constexpr int kSizePidArgs{4}; int i = 0; char arg[20]; @@ -230,7 +251,7 @@ void App::cmd_set_pid_tuning_gains_cb(const char* arg1, const char*) { int pid_args[kSizePidArgs]{0, 0, 0, 0}; // Example: "u 30:20:10:50". - strcpy(arg, arg1); + strcpy(arg, argv[1]); char* p = arg; while ((str = strtok_r(p, ":", &p)) != NULL && i < kSizePidArgs) { pid_args[i] = atoi(str); diff --git a/andino_firmware/src/app.h b/andino_firmware/src/app.h index 1c125695..b0ed9aa6 100644 --- a/andino_firmware/src/app.h +++ b/andino_firmware/src/app.h @@ -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_; diff --git a/andino_firmware/src/shell.cpp b/andino_firmware/src/shell.cpp index 736c4f3b..6c8dfdc0 100644 --- a/andino_firmware/src/shell.cpp +++ b/andino_firmware/src/shell.cpp @@ -48,69 +48,57 @@ 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); } } diff --git a/andino_firmware/src/shell.h b/andino_firmware/src/shell.h index 3b988f83..b4a8212b 100644 --- a/andino_firmware/src/shell.h +++ b/andino_firmware/src/shell.h @@ -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. /// @@ -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 { @@ -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}; @@ -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 From eb40c172d1600ba4441b4cf183b35e37dd7a03f3 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Thu, 23 Nov 2023 12:24:13 -0300 Subject: [PATCH 03/23] Add dependencies for BNO055 Signed-off-by: Ivan Santiago Paunovic --- andino_firmware/platformio.ini | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/andino_firmware/platformio.ini b/andino_firmware/platformio.ini index b6cbfd70..f07c409c 100644 --- a/andino_firmware/platformio.ini +++ b/andino_firmware/platformio.ini @@ -13,11 +13,12 @@ platform = atmelavr framework = arduino monitor_speed = 57600 - -; Base configuration for build tools. -[base_build] -build_flags = - -Wall -Wextra +lib_deps = + Wire + SPI + adafruit/Adafruit BNO055 + adafruit/Adafruit BusIO + adafruit/Adafruit Unified Sensor ; Environment for Arduino Uno. [env:uno] From 2cad5d28fe1258bf526dd34a358f9c9e50605147 Mon Sep 17 00:00:00 2001 From: Gonzalo de Pedro Date: Thu, 23 Nov 2023 14:42:27 -0300 Subject: [PATCH 04/23] Added arduino code Signed-off-by: Gonzalo de Pedro Remove space Signed-off-by: Gonzalo de Pedro Added Imu to ros control Signed-off-by: Gonzalo de Pedro --- .../include/andino_base/motor_driver.h | 17 + andino_base/src/diffdrive_andino.cpp | 23 +- andino_base/src/motor_driver.cpp | 32 +- andino_firmware/src/app.cpp | 16 + andino_firmware/src/commands.h | 4 + andino_firmware/src/hw.h | 4 +- andino_firmware/src/main.cpp | 302 ++++++++++++++++++ 7 files changed, 386 insertions(+), 12 deletions(-) diff --git a/andino_base/include/andino_base/motor_driver.h b/andino_base/include/andino_base/motor_driver.h index 5189fe36..81d4453b 100644 --- a/andino_base/include/andino_base/motor_driver.h +++ b/andino_base/include/andino_base/motor_driver.h @@ -45,6 +45,12 @@ class MotorDriver { public: /// @brief Type to store the encoder values. using Encoders = std::array; + using Imu = std::array; + + struct EncodersAndImu { + Encoders encoders; + Imu imu; + }; /// @brief Default constructor. MotorDriver() = default; @@ -58,6 +64,17 @@ class MotorDriver { /// ensure that the motor driver is ready to receive a new command. void SendEmptyMsg(); + /// @brief Checks if there is an Imu. + /// @returns True if Imu is present. + bool HasImu(); + + /// @brief Read the encoder and Imu values from the motor driver. + /// First value is the left encoder, second value is the right encoder, next values + /// are IMU orientation (qx, qy, qz, qw), angular velocity (x, y, z), + /// linear acceleration (x, y, z) + /// @returns The encoder values. + EncodersAndImu ReadEncoderAndImuValues(); + /// @brief Read the encoder values from the motor driver. /// First value is the left encoder, second value is the right encoder. /// @returns The encoder values. diff --git a/andino_base/src/diffdrive_andino.cpp b/andino_base/src/diffdrive_andino.cpp index c39d3980..3d85dd59 100644 --- a/andino_base/src/diffdrive_andino.cpp +++ b/andino_base/src/diffdrive_andino.cpp @@ -145,10 +145,25 @@ hardware_interface::return_type DiffDriveAndino::read(const rclcpp::Time& /* tim return hardware_interface::return_type::ERROR; } - const MotorDriver::Encoders encoders = motor_driver_.ReadEncoderValues(); - - left_wheel_.enc_ = encoders[0]; - right_wheel_.enc_ = encoders[1]; + if (motor_driver_.HasImu()) { + const MotorDriver::EncodersAndImu eai = motor_driver_.ReadEncoderAndImuValues(); + left_wheel_.enc_ = eai.encoders[0]; + right_wheel_.enc_ = eai.encoders[1]; + imu_.orientation_x = eai.imu[0]; + imu_.orientation_y = eai.imu[1]; + imu_.orientation_z = eai.imu[2]; + imu_.orientation_w = eai.imu[3]; + imu_.angular_velocity_x_ = eai.imu[4]; + imu_.angular_velocity_y_ = eai.imu[5]; + imu_.angular_velocity_z_ = eai.imu[6]; + imu_.linear_acceleration_x_ = eai.imu[7]; + imu_.linear_acceleration_y_ = eai.imu[8]; + imu_.linear_acceleration_z_ = eai.imu[9]; + } else { + const MotorDriver::Encoders encoders = motor_driver_.ReadEncoderValues(); + left_wheel_.enc_ = encoders[0]; + right_wheel_.enc_ = encoders[1]; + } const double left_pos_prev = left_wheel_.pos_; left_wheel_.pos_ = left_wheel_.Angle(); diff --git a/andino_base/src/motor_driver.cpp b/andino_base/src/motor_driver.cpp index f337294a..9f9ced73 100644 --- a/andino_base/src/motor_driver.cpp +++ b/andino_base/src/motor_driver.cpp @@ -62,14 +62,34 @@ bool MotorDriver::is_connected() const { return serial_port_.IsOpen(); } void MotorDriver::SendEmptyMsg() { std::string response = SendMsg(""); } +bool MotorDriver::HasImu() { + const std::string response = SendMsg("h"); + return response != "0"; +} + +MotorDriver::EncodersAndImu MotorDriver::ReadEncoderAndImuValues() { + std::istringstream is(SendMsg("i")); + + MotorDriver::EncodersAndImu eai; + for(int &val : eai.encoders) { + is >> val; + } + for(double &val : eai.imu) { + is >> val; + } + + return eai; +} + MotorDriver::Encoders MotorDriver::ReadEncoderValues() { - static const std::string delimiter = " "; + std::istringstream is(SendMsg("e")); + + MotorDriver::Encoders enc; + for(int &val : enc) { + is >> val; + } - const std::string response = SendMsg("e"); - const size_t del_pos = response.find(delimiter); - const std::string token_1 = response.substr(0, del_pos).c_str(); - const std::string token_2 = response.substr(del_pos + delimiter.length()).c_str(); - return {std::atoi(token_1.c_str()), std::atoi(token_2.c_str())}; + return enc; } void MotorDriver::SetMotorValues(int val_1, int val_2) { diff --git a/andino_firmware/src/app.cpp b/andino_firmware/src/app.cpp index 82d3c900..baef96ff 100644 --- a/andino_firmware/src/app.cpp +++ b/andino_firmware/src/app.cpp @@ -73,6 +73,12 @@ #include "pid.h" #include "shell.h" +/*BNO055 Imu */ +#include +#include +#include +#include + // TODO(jballoffet): Move this variables to a different module. /* Track the next time we make a PID calculation */ @@ -80,6 +86,8 @@ unsigned long nextPID = andino::Constants::kPidPeriod; long lastMotorCommand = andino::Constants::kAutoStopWindow; +bool HAS_IMU = true; + namespace andino { Shell App::shell_; @@ -123,6 +131,14 @@ void App::setup() { 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); + /* Initialise the IMU sensor */ + if(!bno.begin()) + { + /* There was a problem detecting the BNO055 ... check your connections */ + Serial.print("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!"); + HAS_IMU = false; + } + bno.setExtCrystalUse(true); } void App::loop() { diff --git a/andino_firmware/src/commands.h b/andino_firmware/src/commands.h index ff3e5ac5..e7c1e749 100644 --- a/andino_firmware/src/commands.h +++ b/andino_firmware/src/commands.h @@ -74,6 +74,10 @@ struct Commands { static constexpr const char* kReadDigitalGpio{"d"}; /// @brief Reads the encoders tick count values. static constexpr const char* kReadEncoders{"e"}; + /// @brief Reads if there is an IMU connected. + static constexpr const char* kHasImu{"h"}; + /// @brief Reads the encoders tick count values and IMU data. + static constexpr const char* kReadEncodersAndImu{"i"}; /// @brief Sets the encoders ticks count to zero. static constexpr const char* kResetEncoders{"r"}; /// @brief Sets the motors speed [ticks/s]. diff --git a/andino_firmware/src/hw.h b/andino_firmware/src/hw.h index 0b50b2d9..e665dced 100644 --- a/andino_firmware/src/hw.h +++ b/andino_firmware/src/hw.h @@ -39,9 +39,9 @@ struct Hw { static constexpr int kLeftEncoderChannelBGpioPin{3}; /// @brief Right encoder channel A pin. Connected to PC4 (digital pin 18, analog pin A4). - static constexpr int kRightEncoderChannelAGpioPin{18}; + static constexpr int kRightEncoderChannelAGpioPin{14}; /// @brief Right encoder channel B pin. Connected to PC5 (digital pin 19, analog pin A5). - static constexpr int kRightEncoderChannelBGpioPin{19}; + static constexpr int kRightEncoderChannelBGpioPin{15}; /// @brief Left motor driver backward pin. Connected to PD6 (digital pin 6). static constexpr int kLeftMotorBackwardGpioPin{6}; diff --git a/andino_firmware/src/main.cpp b/andino_firmware/src/main.cpp index 6892a5a0..7dc0c5cb 100644 --- a/andino_firmware/src/main.cpp +++ b/andino_firmware/src/main.cpp @@ -36,9 +36,311 @@ int main(void) { // Application configuration. andino::App::setup(); +<<<<<<< HEAD // Application main run loop. while (1) { andino::App::loop(); +======= +#include "Arduino.h" +#include "hw.h" + +/* Include definition of serial commands */ +#include "commands.h" + +/* Motor driver function definitions */ +#include "motor.h" + +/* Encoder driver function definitions */ +#include "encoder.h" + +/* PID parameters and functions */ +#include "pid.h" + +/*BNO055 Imu */ +#include +#include +#include +#include + +/* Run the PID loop at 30 times per second */ +#define PID_RATE 30 // Hz + +/* Convert the rate into an interval in milliseconds */ +const int PID_INTERVAL = 1000 / PID_RATE; + +/* Track the next time we make a PID calculation */ +unsigned long nextPID = PID_INTERVAL; + +/* Stop the robot if it hasn't received a movement command + in this number of milliseconds */ +#define AUTO_STOP_INTERVAL 3000 +long lastMotorCommand = AUTO_STOP_INTERVAL; + +bool HAS_IMU = true; + +/* Variable initialization */ + +// 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]; + +// The arguments converted to integers +long arg1; +long arg2; + +// TODO(jballoffet): Make these objects local to the main function. +andino::Motor left_motor(LEFT_MOTOR_ENABLE_GPIO_PIN, LEFT_MOTOR_FORWARD_GPIO_PIN, + LEFT_MOTOR_BACKWARD_GPIO_PIN); +andino::Motor right_motor(RIGHT_MOTOR_ENABLE_GPIO_PIN, RIGHT_MOTOR_FORWARD_GPIO_PIN, + RIGHT_MOTOR_BACKWARD_GPIO_PIN); + +// TODO(jballoffet): Make these objects local to the main function. +andino::PID left_pid_controller(30, 10, 0, 10, -MAX_PWM, MAX_PWM); +andino::PID right_pid_controller(30, 10, 0, 10, -MAX_PWM, MAX_PWM); + +// TODO(jballoffet): Make these objects local to the main function. +andino::Encoder left_encoder(LEFT_ENCODER_A_GPIO_PIN, LEFT_ENCODER_B_GPIO_PIN); +andino::Encoder right_encoder(RIGHT_ENCODER_A_GPIO_PIN, RIGHT_ENCODER_B_GPIO_PIN); + +// Check I2C device address and correct line below (by default address is 0x29 or 0x28) +// id, address +Adafruit_BNO055 bno = Adafruit_BNO055(55, BNO055_ADDRESS_A, &Wire); + +/* Clear the current command parameters */ +void resetCommand() { + cmd = 0; + memset(argv1, 0, sizeof(argv1)); + memset(argv2, 0, sizeof(argv2)); + arg1 = 0; + arg2 = 0; + arg = 0; + index = 0; +} + +/* Run a command. Commands are defined in commands.h */ +int runCommand() { + int i = 0; + char* p = argv1; + char* str; + int pid_args[4]; + arg1 = atoi(argv1); + arg2 = atoi(argv2); + + switch (cmd) { + case GET_BAUDRATE: + Serial.println(BAUDRATE); + break; + case ANALOG_READ: + Serial.println(analogRead(arg1)); + break; + case DIGITAL_READ: + Serial.println(digitalRead(arg1)); + break; + case ANALOG_WRITE: + analogWrite(arg1, arg2); + Serial.println("OK"); + break; + case DIGITAL_WRITE: + if (arg2 == 0) + digitalWrite(arg1, LOW); + else if (arg2 == 1) + digitalWrite(arg1, HIGH); + Serial.println("OK"); + break; + case PIN_MODE: + if (arg2 == 0) + pinMode(arg1, INPUT); + else if (arg2 == 1) + pinMode(arg1, OUTPUT); + Serial.println("OK"); + break; + case READ_ENCODERS: + Serial.print(left_encoder.read()); + Serial.print(" "); + Serial.println(right_encoder.read()); + break; + case RESET_ENCODERS: + left_encoder.reset(); + right_encoder.reset(); + left_pid_controller.reset(left_encoder.read()); + right_pid_controller.reset(right_encoder.read()); + Serial.println("OK"); + break; + case MOTOR_SPEEDS: + /* Reset the auto stop timer */ + lastMotorCommand = millis(); + if (arg1 == 0 && arg2 == 0) { + left_motor.set_speed(0); + right_motor.set_speed(0); + left_pid_controller.reset(left_encoder.read()); + right_pid_controller.reset(right_encoder.read()); + left_pid_controller.enable(false); + right_pid_controller.enable(false); + } else { + left_pid_controller.enable(true); + right_pid_controller.enable(true); + } + // The target speeds are in ticks per second, so we need to convert them + // to ticks per PID_INTERVAL + left_pid_controller.set_setpoint(arg1 / PID_RATE); + right_pid_controller.set_setpoint(arg2 / PID_RATE); + Serial.println("OK"); + break; + case MOTOR_RAW_PWM: + /* Reset the auto stop timer */ + lastMotorCommand = millis(); + left_pid_controller.reset(left_encoder.read()); + right_pid_controller.reset(right_encoder.read()); + // Sneaky way to temporarily disable the PID + left_pid_controller.enable(false); + right_pid_controller.enable(false); + left_motor.set_speed(arg1); + right_motor.set_speed(arg2); + Serial.println("OK"); + break; + case UPDATE_PID: + /* Example: "u 30:20:10:50" */ + while ((str = strtok_r(p, ":", &p)) != NULL) { + pid_args[i] = atoi(str); + i++; + } + left_pid_controller.set_tunings(pid_args[0], pid_args[1], pid_args[2], pid_args[3]); + right_pid_controller.set_tunings(pid_args[0], pid_args[1], pid_args[2], pid_args[3]); + Serial.print("PID Updated: "); + Serial.print(pid_args[0]); + Serial.print(" "); + Serial.print(pid_args[1]); + Serial.print(" "); + Serial.print(pid_args[2]); + Serial.print(" "); + Serial.println(pid_args[3]); + Serial.println("OK"); + break; + case HAVE_IMU: + Serial.println(HAS_IMU); + break; + case READ_ENCODERS_AND_IMU: + { + Serial.print(left_encoder.read()); + Serial.print(" "); + Serial.print(right_encoder.read()); + Serial.print(" "); + // Quaternion data + imu::Quaternion quat = bno.getQuat(); + Serial.print(quat.x(), 4); + Serial.print(" "); + Serial.print(quat.y(), 4); + Serial.print(" "); + Serial.print(quat.z(), 4); + Serial.print(" "); + Serial.print(quat.w(), 4); + Serial.print(" "); + + /* Display the floating point data */ + imu::Vector<3> euler_angvel = bno.getVector(Adafruit_BNO055::VECTOR_GYROSCOPE); + Serial.print(euler_angvel.x()); + Serial.print(" "); + Serial.print(euler_angvel.y()); + Serial.print(" "); + Serial.print(euler_angvel.z()); + Serial.print(" "); + + /* Display the floating point data */ + imu::Vector<3> euler_linearaccel = bno.getVector(Adafruit_BNO055::VECTOR_LINEARACCEL); + Serial.print(euler_linearaccel.x()); + Serial.print(" "); + Serial.print(euler_linearaccel.y()); + Serial.print(" "); + Serial.print(euler_linearaccel.z()); + Serial.print("\t\t"); + + Serial.println("OK"); + } + break; + default: + Serial.println("Invalid Command"); + break; + } +} + +/* Setup function--runs once at startup. */ +void setup() { + Serial.begin(BAUDRATE); + + left_encoder.init(); + right_encoder.init(); + + // Enable motors. + left_motor.set_state(true); + right_motor.set_state(true); + + left_pid_controller.reset(left_encoder.read()); + right_pid_controller.reset(right_encoder.read()); + + /* Initialise the IMU sensor */ + if(!bno.begin()) + { + /* There was a problem detecting the BNO055 ... check your connections */ + Serial.print("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!"); + HAS_IMU = false; + } + bno.setExtCrystalUse(true); +} + +/* Enter the main loop. Read and parse input from the serial port + and run any valid commands. Run a PID calculation at the target + interval and check for auto-stop conditions. +*/ +void 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; + runCommand(); + resetCommand(); + } + // 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++; + } + } +>>>>>>> a86f0c4 (Added arduino code) } return 0; From 4f7afd26e3cce402732a6c441faeba0dc517201d Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Thu, 23 Nov 2023 16:59:23 -0300 Subject: [PATCH 05/23] Add ros2_control state interfaces for IMU Signed-off-by: Ivan Santiago Paunovic --- .../include/andino_base/diffdrive_andino.h | 5 ++ andino_base/include/andino_base/imu.h | 57 +++++++++++++++++++ andino_base/src/diffdrive_andino.cpp | 28 ++++++++- andino_control/config/andino_controllers.yaml | 11 ++++ andino_control/package.xml | 1 + .../urdf/include/andino_control.urdf.xacro | 12 ++++ 6 files changed, 113 insertions(+), 1 deletion(-) create mode 100644 andino_base/include/andino_base/imu.h diff --git a/andino_base/include/andino_base/diffdrive_andino.h b/andino_base/include/andino_base/diffdrive_andino.h index f304b53c..9eb4fa4e 100644 --- a/andino_base/include/andino_base/diffdrive_andino.h +++ b/andino_base/include/andino_base/diffdrive_andino.h @@ -40,6 +40,7 @@ #include #include +#include "andino_base/imu.h" #include "andino_base/motor_driver.h" #include "andino_base/wheel.h" @@ -70,6 +71,7 @@ class DiffDriveAndino : public hardware_interface::SystemInterface { hardware_interface::return_type write(const rclcpp::Time& time, const rclcpp::Duration& period) override; private: + const std::string kImuSensorName{"imu_sensor_name"}; const std::string kLeftWheelNameParam{"left_wheel_name"}; const std::string kRightWheelNameParam{"right_wheel_name"}; const std::string kSerialDeviceParam{"serial_device"}; @@ -80,6 +82,7 @@ class DiffDriveAndino : public hardware_interface::SystemInterface { // Configuration parameters for the DiffDriveAndino class. struct Config { // Name of the left and right wheels. + std::string imu_sensor_name = "imu_sensor"; std::string left_wheel_name = "left_wheel"; std::string right_wheel_name = "right_wheel"; // Encoder parameters. @@ -98,6 +101,8 @@ class DiffDriveAndino : public hardware_interface::SystemInterface { Wheel left_wheel_; // Right wheel of the robot. Wheel right_wheel_; + // Imu sensor + Imu imu_; // Logger. rclcpp::Logger logger_{rclcpp::get_logger("DiffDriveAndino")}; }; diff --git a/andino_base/include/andino_base/imu.h b/andino_base/include/andino_base/imu.h new file mode 100644 index 00000000..de54b4d3 --- /dev/null +++ b/andino_base/include/andino_base/imu.h @@ -0,0 +1,57 @@ +// 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. +#pragma once + +#include + +namespace andino_base { + +struct Imu { + /// @brief Default constructor for the Wheel class + Imu() = default; + + /// @brief Setup the wheel. + /// @param imu_name name of the imu sensor. + void Setup(const std::string& imu_name); + + std::string name_ = ""; + double orientation_x = 0.; + double orientation_y = 0.; + double orientation_z = 0.; + double orientation_w = 0.; + double angular_velocity_x_ = 0; + double angular_velocity_y_ = 0; + double angular_velocity_z_ = 0; + double linear_acceleration_x_ = 0; + double linear_acceleration_y_ = 0; + double linear_acceleration_z_ = 0; +}; + +} // namespace andino_base diff --git a/andino_base/src/diffdrive_andino.cpp b/andino_base/src/diffdrive_andino.cpp index 3d85dd59..3920bb69 100644 --- a/andino_base/src/diffdrive_andino.cpp +++ b/andino_base/src/diffdrive_andino.cpp @@ -41,6 +41,7 @@ hardware_interface::CallbackReturn DiffDriveAndino::on_init(const hardware_inter RCLCPP_INFO(logger_, "On init..."); + config_.imu_sensor_name = info_.hardware_parameters[kImuSensorName]; config_.left_wheel_name = info_.hardware_parameters[kLeftWheelNameParam]; RCLCPP_DEBUG(logger_, (kLeftWheelNameParam + static_cast(": ") + config_.left_wheel_name).c_str()); config_.right_wheel_name = info_.hardware_parameters[kRightWheelNameParam]; @@ -81,7 +82,7 @@ hardware_interface::CallbackReturn DiffDriveAndino::on_init(const hardware_inter return hardware_interface::CallbackReturn::SUCCESS; } -hardware_interface::CallbackReturn DiffDriveAndino::on_configure(const rclcpp_lifecycle::State& /*previous_state*/) { +hardware_interface::CallbackReturn DiffDriveAinterface_names_ndino::on_configure(const rclcpp_lifecycle::State& /*previous_state*/) { RCLCPP_INFO(logger_, "On configure..."); // Set up communication with motor driver controller. @@ -106,6 +107,31 @@ std::vector DiffDriveAndino::export_state_in hardware_interface::StateInterface(right_wheel_.name_, hardware_interface::HW_IF_VELOCITY, &right_wheel_.vel_)); state_interfaces.emplace_back( hardware_interface::StateInterface(right_wheel_.name_, hardware_interface::HW_IF_POSITION, &right_wheel_.pos_)); + state_interfaces.emplace_back( + hardware_interface::StateInterface(config_.imu_sensor_name, config_.imu_sensor_name + "/1", &right_wheel_.pos_)); + state_interfaces.emplace_back( + hardware_interface::StateInterface(config_.imu_sensor_name, config_.imu_sensor_name + "/2", &right_wheel_.pos_)); + state_interfaces.emplace_back( + hardware_interface::StateInterface(config_.imu_sensor_name, config_.imu_sensor_name + "/3", &right_wheel_.pos_)); + state_interfaces.emplace_back( + hardware_interface::StateInterface(config_.imu_sensor_name, config_.imu_sensor_name + "/4", &right_wheel_.pos_)); + state_interfaces.emplace_back( + hardware_interface::StateInterface(config_.imu_sensor_name, config_.imu_sensor_name + "/5", &right_wheel_.pos_)); + state_interfaces.emplace_back( + hardware_interface::StateInterface(config_.imu_sensor_name, config_.imu_sensor_name + "/6", &right_wheel_.pos_)); + state_interfaces.emplace_back( + hardware_interface::StateInterface(config_.imu_sensor_name, config_.imu_sensor_name + "/7", &right_wheel_.pos_)); + state_interfaces.emplace_back( + hardware_interface::StateInterface(config_.imu_sensor_name, config_.imu_sensor_name + "/8", &right_wheel_.pos_)); + state_interfaces.emplace_back( + hardware_interface::StateInterface(config_.imu_sensor_name, config_.imu_sensor_name + "/9", &right_wheel_.pos_)); + state_interfaces.emplace_back( + hardware_interface::StateInterface(config_.imu_sensor_name, config_.imu_sensor_name + "/10", &right_wheel_.pos_)); + + for (const auto &name : imu_sensor.get_state_interface_names()) { + state_interfaces.emplace_back( + hardware_interface::StateInterface(config_.imu_sensor_name, name, &right_wheel_.pos_)); + } return state_interfaces; } diff --git a/andino_control/config/andino_controllers.yaml b/andino_control/config/andino_controllers.yaml index 41c42a47..2f73e78f 100644 --- a/andino_control/config/andino_controllers.yaml +++ b/andino_control/config/andino_controllers.yaml @@ -8,6 +8,17 @@ controller_manager: joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster + imu_sensor_broadcaster: + type: imu_sensor_broadcaster/IMUSensorBroadcaster + +imu_sensor_broadcaster: + ros__parameters: + sensor_name: 'imu_sensor' + frame_id: 'base_link' + static_covariance_orientation: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + static_covariance_angular_velocity: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + static_covariance_linear_acceleration: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + diff_controller: ros__parameters: publish_rate: 50.0 diff --git a/andino_control/package.xml b/andino_control/package.xml index fba9b6ed..d1d42241 100644 --- a/andino_control/package.xml +++ b/andino_control/package.xml @@ -17,6 +17,7 @@ controller_manager diff_drive_controller joint_state_broadcaster + imu_sensor_broadcaster ros2controlcli ament_cmake_clang_format diff --git a/andino_description/urdf/include/andino_control.urdf.xacro b/andino_description/urdf/include/andino_control.urdf.xacro index 8dc8949a..195037d8 100644 --- a/andino_description/urdf/include/andino_control.urdf.xacro +++ b/andino_description/urdf/include/andino_control.urdf.xacro @@ -29,6 +29,18 @@ + + + + + + + + + + + + From d24be5e5b552a78088136a2b02149fdd29f08e45 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Thu, 23 Nov 2023 17:06:53 -0300 Subject: [PATCH 06/23] fix random typo Signed-off-by: Ivan Santiago Paunovic --- andino_base/src/diffdrive_andino.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/andino_base/src/diffdrive_andino.cpp b/andino_base/src/diffdrive_andino.cpp index 3920bb69..657a4b67 100644 --- a/andino_base/src/diffdrive_andino.cpp +++ b/andino_base/src/diffdrive_andino.cpp @@ -82,7 +82,7 @@ hardware_interface::CallbackReturn DiffDriveAndino::on_init(const hardware_inter return hardware_interface::CallbackReturn::SUCCESS; } -hardware_interface::CallbackReturn DiffDriveAinterface_names_ndino::on_configure(const rclcpp_lifecycle::State& /*previous_state*/) { +hardware_interface::CallbackReturn DiffDriveAndino::on_configure(const rclcpp_lifecycle::State& /*previous_state*/) { RCLCPP_INFO(logger_, "On configure..."); // Set up communication with motor driver controller. From 1d32955875ccf0611d2a972eab669b3acf3757d8 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Thu, 23 Nov 2023 17:25:59 -0300 Subject: [PATCH 07/23] remove code not needed Signed-off-by: Ivan Santiago Paunovic --- andino_base/src/diffdrive_andino.cpp | 5 ----- 1 file changed, 5 deletions(-) diff --git a/andino_base/src/diffdrive_andino.cpp b/andino_base/src/diffdrive_andino.cpp index 657a4b67..9dc68d60 100644 --- a/andino_base/src/diffdrive_andino.cpp +++ b/andino_base/src/diffdrive_andino.cpp @@ -128,11 +128,6 @@ std::vector DiffDriveAndino::export_state_in state_interfaces.emplace_back( hardware_interface::StateInterface(config_.imu_sensor_name, config_.imu_sensor_name + "/10", &right_wheel_.pos_)); - for (const auto &name : imu_sensor.get_state_interface_names()) { - state_interfaces.emplace_back( - hardware_interface::StateInterface(config_.imu_sensor_name, name, &right_wheel_.pos_)); - } - return state_interfaces; } From 984362e09daaac71587563735942aca5753bb5b0 Mon Sep 17 00:00:00 2001 From: fbaglivo Date: Thu, 23 Nov 2023 18:45:14 -0300 Subject: [PATCH 08/23] Fix sensor tag closure in xacro file. --- andino_description/urdf/include/andino_control.urdf.xacro | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/andino_description/urdf/include/andino_control.urdf.xacro b/andino_description/urdf/include/andino_control.urdf.xacro index 195037d8..e7b574e8 100644 --- a/andino_description/urdf/include/andino_control.urdf.xacro +++ b/andino_description/urdf/include/andino_control.urdf.xacro @@ -40,7 +40,7 @@ - + From 96fa7556de43fa48d88dd05eb2441309dd76f7d0 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Fri, 24 Nov 2023 10:10:11 -0300 Subject: [PATCH 09/23] Add missing param Signed-off-by: Ivan Santiago Paunovic --- andino_description/urdf/include/andino_control.urdf.xacro | 1 + 1 file changed, 1 insertion(+) diff --git a/andino_description/urdf/include/andino_control.urdf.xacro b/andino_description/urdf/include/andino_control.urdf.xacro index e7b574e8..6584f670 100644 --- a/andino_description/urdf/include/andino_control.urdf.xacro +++ b/andino_description/urdf/include/andino_control.urdf.xacro @@ -12,6 +12,7 @@ 57600 1000 585 + imu_sensor From 1d653415496fc2a0b94f2244c2e236d4cb0da46d Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Fri, 24 Nov 2023 10:13:26 -0300 Subject: [PATCH 10/23] Fix state interface imu data to read Signed-off-by: Ivan Santiago Paunovic --- andino_base/src/diffdrive_andino.cpp | 30 ++++++++++++++++++---------- 1 file changed, 20 insertions(+), 10 deletions(-) diff --git a/andino_base/src/diffdrive_andino.cpp b/andino_base/src/diffdrive_andino.cpp index 9dc68d60..4d289b64 100644 --- a/andino_base/src/diffdrive_andino.cpp +++ b/andino_base/src/diffdrive_andino.cpp @@ -107,26 +107,36 @@ std::vector DiffDriveAndino::export_state_in hardware_interface::StateInterface(right_wheel_.name_, hardware_interface::HW_IF_VELOCITY, &right_wheel_.vel_)); state_interfaces.emplace_back( hardware_interface::StateInterface(right_wheel_.name_, hardware_interface::HW_IF_POSITION, &right_wheel_.pos_)); + double orientation_x = 0.; + double orientation_y = 0.; + double orientation_z = 0.; + double orientation_w = 0.; + double angular_velocity_x_ = 0; + double angular_velocity_y_ = 0; + double angular_velocity_z_ = 0; + double linear_acceleration_x_ = 0; + double linear_acceleration_y_ = 0; + double linear_acceleration_z_ = 0; state_interfaces.emplace_back( - hardware_interface::StateInterface(config_.imu_sensor_name, config_.imu_sensor_name + "/1", &right_wheel_.pos_)); + hardware_interface::StateInterface(config_.imu_sensor_name, config_.imu_sensor_name + "/1", &imu_.orientation_x)); state_interfaces.emplace_back( - hardware_interface::StateInterface(config_.imu_sensor_name, config_.imu_sensor_name + "/2", &right_wheel_.pos_)); + hardware_interface::StateInterface(config_.imu_sensor_name, config_.imu_sensor_name + "/2", &imu_.orientation_y)); state_interfaces.emplace_back( - hardware_interface::StateInterface(config_.imu_sensor_name, config_.imu_sensor_name + "/3", &right_wheel_.pos_)); + hardware_interface::StateInterface(config_.imu_sensor_name, config_.imu_sensor_name + "/3", &imu_.orientation_z)); state_interfaces.emplace_back( - hardware_interface::StateInterface(config_.imu_sensor_name, config_.imu_sensor_name + "/4", &right_wheel_.pos_)); + hardware_interface::StateInterface(config_.imu_sensor_name, config_.imu_sensor_name + "/4", &imu_.orientation_w)); state_interfaces.emplace_back( - hardware_interface::StateInterface(config_.imu_sensor_name, config_.imu_sensor_name + "/5", &right_wheel_.pos_)); + hardware_interface::StateInterface(config_.imu_sensor_name, config_.imu_sensor_name + "/5", &imu_.angular_velocity_x_)); state_interfaces.emplace_back( - hardware_interface::StateInterface(config_.imu_sensor_name, config_.imu_sensor_name + "/6", &right_wheel_.pos_)); + hardware_interface::StateInterface(config_.imu_sensor_name, config_.imu_sensor_name + "/6", &imu_.angular_velocity_y_)); state_interfaces.emplace_back( - hardware_interface::StateInterface(config_.imu_sensor_name, config_.imu_sensor_name + "/7", &right_wheel_.pos_)); + hardware_interface::StateInterface(config_.imu_sensor_name, config_.imu_sensor_name + "/7", &imu_.angular_velocity_z_)); state_interfaces.emplace_back( - hardware_interface::StateInterface(config_.imu_sensor_name, config_.imu_sensor_name + "/8", &right_wheel_.pos_)); + hardware_interface::StateInterface(config_.imu_sensor_name, config_.imu_sensor_name + "/8", &imu_.linear_acceleration_x_)); state_interfaces.emplace_back( - hardware_interface::StateInterface(config_.imu_sensor_name, config_.imu_sensor_name + "/9", &right_wheel_.pos_)); + hardware_interface::StateInterface(config_.imu_sensor_name, config_.imu_sensor_name + "/9", &imu_.linear_acceleration_y_)); state_interfaces.emplace_back( - hardware_interface::StateInterface(config_.imu_sensor_name, config_.imu_sensor_name + "/10", &right_wheel_.pos_)); + hardware_interface::StateInterface(config_.imu_sensor_name, config_.imu_sensor_name + "/10", &imu_.linear_acceleration_z_)); return state_interfaces; } From 58fd6abd1c7ea98198f06fdc5e71ca201153a1e1 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Fri, 24 Nov 2023 10:17:20 -0300 Subject: [PATCH 11/23] remove copied code Signed-off-by: Ivan Santiago Paunovic --- andino_base/src/diffdrive_andino.cpp | 10 ---------- 1 file changed, 10 deletions(-) diff --git a/andino_base/src/diffdrive_andino.cpp b/andino_base/src/diffdrive_andino.cpp index 4d289b64..87a72d6c 100644 --- a/andino_base/src/diffdrive_andino.cpp +++ b/andino_base/src/diffdrive_andino.cpp @@ -107,16 +107,6 @@ std::vector DiffDriveAndino::export_state_in hardware_interface::StateInterface(right_wheel_.name_, hardware_interface::HW_IF_VELOCITY, &right_wheel_.vel_)); state_interfaces.emplace_back( hardware_interface::StateInterface(right_wheel_.name_, hardware_interface::HW_IF_POSITION, &right_wheel_.pos_)); - double orientation_x = 0.; - double orientation_y = 0.; - double orientation_z = 0.; - double orientation_w = 0.; - double angular_velocity_x_ = 0; - double angular_velocity_y_ = 0; - double angular_velocity_z_ = 0; - double linear_acceleration_x_ = 0; - double linear_acceleration_y_ = 0; - double linear_acceleration_z_ = 0; state_interfaces.emplace_back( hardware_interface::StateInterface(config_.imu_sensor_name, config_.imu_sensor_name + "/1", &imu_.orientation_x)); state_interfaces.emplace_back( From 6153f7cd52b67701ba14ba64813818103ea8510b Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Fri, 24 Nov 2023 10:25:26 -0300 Subject: [PATCH 12/23] Spawn imu controller Signed-off-by: Ivan Santiago Paunovic --- andino_control/launch/andino_control.launch.py | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/andino_control/launch/andino_control.launch.py b/andino_control/launch/andino_control.launch.py index b3c66c7a..58ddbc3e 100644 --- a/andino_control/launch/andino_control.launch.py +++ b/andino_control/launch/andino_control.launch.py @@ -70,6 +70,12 @@ def generate_launch_description(): arguments=["diff_controller", "--controller-manager", "/controller_manager"], ) + diff_drive_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["imu_sensor_broadcaster", "--controller-manager", "/controller_manager"], + ) + # Delay start of diff_drive_controller_spawner after `joint_state_broadcaster` delay_diff_drive_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler( event_handler=OnProcessExit( From e2d8bbc09090651c85564835264a932b8aa5d52a Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Fri, 24 Nov 2023 10:28:37 -0300 Subject: [PATCH 13/23] fix Signed-off-by: Ivan Santiago Paunovic --- andino_control/launch/andino_control.launch.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/andino_control/launch/andino_control.launch.py b/andino_control/launch/andino_control.launch.py index 58ddbc3e..8b41d3fc 100644 --- a/andino_control/launch/andino_control.launch.py +++ b/andino_control/launch/andino_control.launch.py @@ -70,7 +70,7 @@ def generate_launch_description(): arguments=["diff_controller", "--controller-manager", "/controller_manager"], ) - diff_drive_controller_spawner = Node( + imu_sensor_broadcaster_spawner = Node( package="controller_manager", executable="spawner", arguments=["imu_sensor_broadcaster", "--controller-manager", "/controller_manager"], @@ -80,7 +80,7 @@ def generate_launch_description(): delay_diff_drive_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler( event_handler=OnProcessExit( target_action=joint_state_broadcaster_spawner, - on_exit=[diff_drive_controller_spawner], + on_exit=[diff_drive_controller_spawner, imu_sensor_broadcaster_spawner], ) ) From 3c4878e50409d751fb2682a467cb18d69db1e5ec Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Fri, 24 Nov 2023 10:42:12 -0300 Subject: [PATCH 14/23] Fix state interface names (maybe) Signed-off-by: Ivan Santiago Paunovic --- andino_base/src/diffdrive_andino.cpp | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/andino_base/src/diffdrive_andino.cpp b/andino_base/src/diffdrive_andino.cpp index 87a72d6c..6620a801 100644 --- a/andino_base/src/diffdrive_andino.cpp +++ b/andino_base/src/diffdrive_andino.cpp @@ -108,25 +108,25 @@ std::vector DiffDriveAndino::export_state_in state_interfaces.emplace_back( hardware_interface::StateInterface(right_wheel_.name_, hardware_interface::HW_IF_POSITION, &right_wheel_.pos_)); state_interfaces.emplace_back( - hardware_interface::StateInterface(config_.imu_sensor_name, config_.imu_sensor_name + "/1", &imu_.orientation_x)); + hardware_interface::StateInterface(config_.imu_sensor_name, "orientation.x", &imu_.orientation_x)); state_interfaces.emplace_back( - hardware_interface::StateInterface(config_.imu_sensor_name, config_.imu_sensor_name + "/2", &imu_.orientation_y)); + hardware_interface::StateInterface(config_.imu_sensor_name, "orientation.y", &imu_.orientation_y)); state_interfaces.emplace_back( - hardware_interface::StateInterface(config_.imu_sensor_name, config_.imu_sensor_name + "/3", &imu_.orientation_z)); + hardware_interface::StateInterface(config_.imu_sensor_name, "orientation.z", &imu_.orientation_z)); state_interfaces.emplace_back( - hardware_interface::StateInterface(config_.imu_sensor_name, config_.imu_sensor_name + "/4", &imu_.orientation_w)); + hardware_interface::StateInterface(config_.imu_sensor_name, "orientation.w", &imu_.orientation_w)); state_interfaces.emplace_back( - hardware_interface::StateInterface(config_.imu_sensor_name, config_.imu_sensor_name + "/5", &imu_.angular_velocity_x_)); + hardware_interface::StateInterface(config_.imu_sensor_name, "angular_velocity.x", &imu_.angular_velocity_x_)); state_interfaces.emplace_back( - hardware_interface::StateInterface(config_.imu_sensor_name, config_.imu_sensor_name + "/6", &imu_.angular_velocity_y_)); + hardware_interface::StateInterface(config_.imu_sensor_name, "angular_velocity.y", &imu_.angular_velocity_y_)); state_interfaces.emplace_back( - hardware_interface::StateInterface(config_.imu_sensor_name, config_.imu_sensor_name + "/7", &imu_.angular_velocity_z_)); + hardware_interface::StateInterface(config_.imu_sensor_name, "angular_velocity.z", &imu_.angular_velocity_z_)); state_interfaces.emplace_back( - hardware_interface::StateInterface(config_.imu_sensor_name, config_.imu_sensor_name + "/8", &imu_.linear_acceleration_x_)); + hardware_interface::StateInterface(config_.imu_sensor_name, "linear_acceleration.x", &imu_.linear_acceleration_x_)); state_interfaces.emplace_back( - hardware_interface::StateInterface(config_.imu_sensor_name, config_.imu_sensor_name + "/9", &imu_.linear_acceleration_y_)); + hardware_interface::StateInterface(config_.imu_sensor_name, "linear_acceleration.y", &imu_.linear_acceleration_y_)); state_interfaces.emplace_back( - hardware_interface::StateInterface(config_.imu_sensor_name, config_.imu_sensor_name + "/10", &imu_.linear_acceleration_z_)); + hardware_interface::StateInterface(config_.imu_sensor_name, "linear_acceleration.z", &imu_.linear_acceleration_z_)); return state_interfaces; } From 708e2e7161ef0ec0ed150b9393f3a665b1b85941 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Fri, 24 Nov 2023 10:45:39 -0300 Subject: [PATCH 15/23] Same for ros control urdf Signed-off-by: Ivan Santiago Paunovic --- .../urdf/include/andino_control.urdf.xacro | 20 +++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/andino_description/urdf/include/andino_control.urdf.xacro b/andino_description/urdf/include/andino_control.urdf.xacro index 6584f670..9ccea3b6 100644 --- a/andino_description/urdf/include/andino_control.urdf.xacro +++ b/andino_description/urdf/include/andino_control.urdf.xacro @@ -31,16 +31,16 @@ - - - - - - - - - - + + + + + + + + + + From dc526b56c3a6df5567c188761050d47f520209a4 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Fri, 24 Nov 2023 14:54:10 -0300 Subject: [PATCH 16/23] Fix post rebasing errors Signed-off-by: Ivan Santiago Paunovic --- andino_firmware/src/app.cpp | 48 +++++- andino_firmware/src/app.h | 9 ++ andino_firmware/src/main.cpp | 302 ----------------------------------- 3 files changed, 55 insertions(+), 304 deletions(-) diff --git a/andino_firmware/src/app.cpp b/andino_firmware/src/app.cpp index baef96ff..8517182f 100644 --- a/andino_firmware/src/app.cpp +++ b/andino_firmware/src/app.cpp @@ -105,6 +105,8 @@ PID App::left_pid_controller_(Constants::kPidKp, Constants::kPidKd, Constants::k PID App::right_pid_controller_(Constants::kPidKp, Constants::kPidKd, Constants::kPidKi, Constants::kPidKo, -Constants::kPwmMax, Constants::kPwmMax); +Adafruit_BNO055 App::bno_{55, BNO055_ADDRESS_A, &Wire}; + void App::setup() { // Required by Arduino libraries to work. init(); @@ -132,13 +134,13 @@ void App::setup() { shell_.register_command(Commands::kSetMotorsPwm, cmd_set_motors_pwm_cb); shell_.register_command(Commands::kSetPidsTuningGains, cmd_set_pid_tuning_gains_cb); /* Initialise the IMU sensor */ - if(!bno.begin()) + if(!bno_.begin()) { /* There was a problem detecting the BNO055 ... check your connections */ Serial.print("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!"); HAS_IMU = false; } - bno.setExtCrystalUse(true); + bno_.setExtCrystalUse(true); } void App::loop() { @@ -197,6 +199,48 @@ void App::cmd_read_encoders_cb(int, char**) { Serial.println(right_encoder_.read()); } +void App::cmd_read_has_imu_cb(int argc, char** argv) { + Serial.println(HAS_IMU); +} + +void App::cmd_read_encoders_and_imu_cb(int argc, char** argv) { + Serial.print(left_encoder_.read()); + Serial.print(" "); + Serial.print(right_encoder_.read()); + Serial.print(" "); + + // Quaternion data + imu::Quaternion quat = bno_.getQuat(); + Serial.print(quat.x(), 4); + Serial.print(" "); + Serial.print(quat.y(), 4); + Serial.print(" "); + Serial.print(quat.z(), 4); + Serial.print(" "); + Serial.print(quat.w(), 4); + Serial.print(" "); + + /* Display the floating point data */ + imu::Vector<3> euler_angvel = bno_.getVector(Adafruit_BNO055::VECTOR_GYROSCOPE); + Serial.print(euler_angvel.x()); + Serial.print(" "); + Serial.print(euler_angvel.y()); + Serial.print(" "); + Serial.print(euler_angvel.z()); + Serial.print(" "); + + /* Display the floating point data */ + imu::Vector<3> euler_linearaccel = bno_.getVector(Adafruit_BNO055::VECTOR_LINEARACCEL); + Serial.print(euler_linearaccel.x()); + Serial.print(" "); + Serial.print(euler_linearaccel.y()); + Serial.print(" "); + Serial.print(euler_linearaccel.z()); + Serial.print("\t\t"); + + Serial.println("OK"); +} + void App::cmd_reset_encoders_cb(int, char**) { left_encoder_.reset(); right_encoder_.reset(); diff --git a/andino_firmware/src/app.h b/andino_firmware/src/app.h index b0ed9aa6..4b9ea02e 100644 --- a/andino_firmware/src/app.h +++ b/andino_firmware/src/app.h @@ -33,6 +33,7 @@ #include "motor.h" #include "pid.h" #include "shell.h" +#include namespace andino { @@ -61,6 +62,12 @@ class App { /// Callback method for the `Commands::kReadEncoders` command. static void cmd_read_encoders_cb(int argc, char** argv); + /// Callback method for the `Commands::kReadHasImu` command. + static void cmd_read_has_imu_cb(int argc, char** argv); + + /// Callback method for the `Commands::kReadEncodersAndImu` command. + static void cmd_read_encoders_and_imu_cb(int argc, char** argv); + /// Callback method for the `Commands::kResetEncoders` command. static void cmd_reset_encoders_cb(int argc, char** argv); @@ -87,6 +94,8 @@ class App { /// PID controllers (one per wheel). static PID left_pid_controller_; static PID right_pid_controller_; + + static Adafruit_BNO055 bno_; }; } // namespace andino diff --git a/andino_firmware/src/main.cpp b/andino_firmware/src/main.cpp index 7dc0c5cb..6892a5a0 100644 --- a/andino_firmware/src/main.cpp +++ b/andino_firmware/src/main.cpp @@ -36,311 +36,9 @@ int main(void) { // Application configuration. andino::App::setup(); -<<<<<<< HEAD // Application main run loop. while (1) { andino::App::loop(); -======= -#include "Arduino.h" -#include "hw.h" - -/* Include definition of serial commands */ -#include "commands.h" - -/* Motor driver function definitions */ -#include "motor.h" - -/* Encoder driver function definitions */ -#include "encoder.h" - -/* PID parameters and functions */ -#include "pid.h" - -/*BNO055 Imu */ -#include -#include -#include -#include - -/* Run the PID loop at 30 times per second */ -#define PID_RATE 30 // Hz - -/* Convert the rate into an interval in milliseconds */ -const int PID_INTERVAL = 1000 / PID_RATE; - -/* Track the next time we make a PID calculation */ -unsigned long nextPID = PID_INTERVAL; - -/* Stop the robot if it hasn't received a movement command - in this number of milliseconds */ -#define AUTO_STOP_INTERVAL 3000 -long lastMotorCommand = AUTO_STOP_INTERVAL; - -bool HAS_IMU = true; - -/* Variable initialization */ - -// 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]; - -// The arguments converted to integers -long arg1; -long arg2; - -// TODO(jballoffet): Make these objects local to the main function. -andino::Motor left_motor(LEFT_MOTOR_ENABLE_GPIO_PIN, LEFT_MOTOR_FORWARD_GPIO_PIN, - LEFT_MOTOR_BACKWARD_GPIO_PIN); -andino::Motor right_motor(RIGHT_MOTOR_ENABLE_GPIO_PIN, RIGHT_MOTOR_FORWARD_GPIO_PIN, - RIGHT_MOTOR_BACKWARD_GPIO_PIN); - -// TODO(jballoffet): Make these objects local to the main function. -andino::PID left_pid_controller(30, 10, 0, 10, -MAX_PWM, MAX_PWM); -andino::PID right_pid_controller(30, 10, 0, 10, -MAX_PWM, MAX_PWM); - -// TODO(jballoffet): Make these objects local to the main function. -andino::Encoder left_encoder(LEFT_ENCODER_A_GPIO_PIN, LEFT_ENCODER_B_GPIO_PIN); -andino::Encoder right_encoder(RIGHT_ENCODER_A_GPIO_PIN, RIGHT_ENCODER_B_GPIO_PIN); - -// Check I2C device address and correct line below (by default address is 0x29 or 0x28) -// id, address -Adafruit_BNO055 bno = Adafruit_BNO055(55, BNO055_ADDRESS_A, &Wire); - -/* Clear the current command parameters */ -void resetCommand() { - cmd = 0; - memset(argv1, 0, sizeof(argv1)); - memset(argv2, 0, sizeof(argv2)); - arg1 = 0; - arg2 = 0; - arg = 0; - index = 0; -} - -/* Run a command. Commands are defined in commands.h */ -int runCommand() { - int i = 0; - char* p = argv1; - char* str; - int pid_args[4]; - arg1 = atoi(argv1); - arg2 = atoi(argv2); - - switch (cmd) { - case GET_BAUDRATE: - Serial.println(BAUDRATE); - break; - case ANALOG_READ: - Serial.println(analogRead(arg1)); - break; - case DIGITAL_READ: - Serial.println(digitalRead(arg1)); - break; - case ANALOG_WRITE: - analogWrite(arg1, arg2); - Serial.println("OK"); - break; - case DIGITAL_WRITE: - if (arg2 == 0) - digitalWrite(arg1, LOW); - else if (arg2 == 1) - digitalWrite(arg1, HIGH); - Serial.println("OK"); - break; - case PIN_MODE: - if (arg2 == 0) - pinMode(arg1, INPUT); - else if (arg2 == 1) - pinMode(arg1, OUTPUT); - Serial.println("OK"); - break; - case READ_ENCODERS: - Serial.print(left_encoder.read()); - Serial.print(" "); - Serial.println(right_encoder.read()); - break; - case RESET_ENCODERS: - left_encoder.reset(); - right_encoder.reset(); - left_pid_controller.reset(left_encoder.read()); - right_pid_controller.reset(right_encoder.read()); - Serial.println("OK"); - break; - case MOTOR_SPEEDS: - /* Reset the auto stop timer */ - lastMotorCommand = millis(); - if (arg1 == 0 && arg2 == 0) { - left_motor.set_speed(0); - right_motor.set_speed(0); - left_pid_controller.reset(left_encoder.read()); - right_pid_controller.reset(right_encoder.read()); - left_pid_controller.enable(false); - right_pid_controller.enable(false); - } else { - left_pid_controller.enable(true); - right_pid_controller.enable(true); - } - // The target speeds are in ticks per second, so we need to convert them - // to ticks per PID_INTERVAL - left_pid_controller.set_setpoint(arg1 / PID_RATE); - right_pid_controller.set_setpoint(arg2 / PID_RATE); - Serial.println("OK"); - break; - case MOTOR_RAW_PWM: - /* Reset the auto stop timer */ - lastMotorCommand = millis(); - left_pid_controller.reset(left_encoder.read()); - right_pid_controller.reset(right_encoder.read()); - // Sneaky way to temporarily disable the PID - left_pid_controller.enable(false); - right_pid_controller.enable(false); - left_motor.set_speed(arg1); - right_motor.set_speed(arg2); - Serial.println("OK"); - break; - case UPDATE_PID: - /* Example: "u 30:20:10:50" */ - while ((str = strtok_r(p, ":", &p)) != NULL) { - pid_args[i] = atoi(str); - i++; - } - left_pid_controller.set_tunings(pid_args[0], pid_args[1], pid_args[2], pid_args[3]); - right_pid_controller.set_tunings(pid_args[0], pid_args[1], pid_args[2], pid_args[3]); - Serial.print("PID Updated: "); - Serial.print(pid_args[0]); - Serial.print(" "); - Serial.print(pid_args[1]); - Serial.print(" "); - Serial.print(pid_args[2]); - Serial.print(" "); - Serial.println(pid_args[3]); - Serial.println("OK"); - break; - case HAVE_IMU: - Serial.println(HAS_IMU); - break; - case READ_ENCODERS_AND_IMU: - { - Serial.print(left_encoder.read()); - Serial.print(" "); - Serial.print(right_encoder.read()); - Serial.print(" "); - // Quaternion data - imu::Quaternion quat = bno.getQuat(); - Serial.print(quat.x(), 4); - Serial.print(" "); - Serial.print(quat.y(), 4); - Serial.print(" "); - Serial.print(quat.z(), 4); - Serial.print(" "); - Serial.print(quat.w(), 4); - Serial.print(" "); - - /* Display the floating point data */ - imu::Vector<3> euler_angvel = bno.getVector(Adafruit_BNO055::VECTOR_GYROSCOPE); - Serial.print(euler_angvel.x()); - Serial.print(" "); - Serial.print(euler_angvel.y()); - Serial.print(" "); - Serial.print(euler_angvel.z()); - Serial.print(" "); - - /* Display the floating point data */ - imu::Vector<3> euler_linearaccel = bno.getVector(Adafruit_BNO055::VECTOR_LINEARACCEL); - Serial.print(euler_linearaccel.x()); - Serial.print(" "); - Serial.print(euler_linearaccel.y()); - Serial.print(" "); - Serial.print(euler_linearaccel.z()); - Serial.print("\t\t"); - - Serial.println("OK"); - } - break; - default: - Serial.println("Invalid Command"); - break; - } -} - -/* Setup function--runs once at startup. */ -void setup() { - Serial.begin(BAUDRATE); - - left_encoder.init(); - right_encoder.init(); - - // Enable motors. - left_motor.set_state(true); - right_motor.set_state(true); - - left_pid_controller.reset(left_encoder.read()); - right_pid_controller.reset(right_encoder.read()); - - /* Initialise the IMU sensor */ - if(!bno.begin()) - { - /* There was a problem detecting the BNO055 ... check your connections */ - Serial.print("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!"); - HAS_IMU = false; - } - bno.setExtCrystalUse(true); -} - -/* Enter the main loop. Read and parse input from the serial port - and run any valid commands. Run a PID calculation at the target - interval and check for auto-stop conditions. -*/ -void 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; - runCommand(); - resetCommand(); - } - // 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++; - } - } ->>>>>>> a86f0c4 (Added arduino code) } return 0; From 027f15b247efffc336a142a3ad5703cf5b10e3b9 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Fri, 24 Nov 2023 15:25:27 -0300 Subject: [PATCH 17/23] Add Arduino IDE dependencies in README Signed-off-by: Ivan Santiago Paunovic --- andino_firmware/README.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/andino_firmware/README.md b/andino_firmware/README.md index c8995ae9..8e2fab0b 100644 --- a/andino_firmware/README.md +++ b/andino_firmware/README.md @@ -8,6 +8,9 @@ Check `encoder_driver.h` and `motor_driver.h` files to check the expected pins f ## Installation +In Arduino IDE, go to `tools->Manage Libraries ...` and install: +- "Adafruit BNO055" + Verify and Upload `andino_firmware.ino` to your arduino board. ## Description From ebd2f6863ee5b1227cfe6ff8900124828b06bb2e Mon Sep 17 00:00:00 2001 From: Gonzalo de Pedro Date: Fri, 24 Nov 2023 17:26:06 -0300 Subject: [PATCH 18/23] Gazebo changes Signed-off-by: Gonzalo de Pedro --- andino_description/config/andino/sensors.yaml | 8 +++ .../urdf/include/common_sensors.urdf.xacro | 32 +++++++++++ .../urdf/andino_gz_classic.xacro | 56 +++++++++++++++++++ 3 files changed, 96 insertions(+) diff --git a/andino_description/config/andino/sensors.yaml b/andino_description/config/andino/sensors.yaml index c7371452..56ff428b 100644 --- a/andino_description/config/andino/sensors.yaml +++ b/andino_description/config/andino/sensors.yaml @@ -14,3 +14,11 @@ camera: dx: 0.085 dy: 0.0 dz: 0.025 + +imu: + mass: 0.05 # mass in Kg + box_size: 0.01 + box_size_height: 0.0025 + dx: 0.0 + dy: 0.0 + dz: 0.0513 \ No newline at end of file diff --git a/andino_description/urdf/include/common_sensors.urdf.xacro b/andino_description/urdf/include/common_sensors.urdf.xacro index 89701cc9..76bcc263 100644 --- a/andino_description/urdf/include/common_sensors.urdf.xacro +++ b/andino_description/urdf/include/common_sensors.urdf.xacro @@ -92,6 +92,38 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/andino_gz_classic/urdf/andino_gz_classic.xacro b/andino_gz_classic/urdf/andino_gz_classic.xacro index 22911df7..032da9c7 100644 --- a/andino_gz_classic/urdf/andino_gz_classic.xacro +++ b/andino_gz_classic/urdf/andino_gz_classic.xacro @@ -148,6 +148,62 @@ + + + true + 100 + + + + + 0.0 + 2e-4 + + + + + 0.0 + 2e-4 + + + + + 0.0 + 2e-4 + + + + + + + 0.0 + 1.7e-2 + + + + + 0.0 + 1.7e-2 + + + + + 0.0 + 1.7e-2 + + + + + + + imu + ~/out:=data + + false + + + + From 1f4eadb5a40defd4ebbb403a3aa4ffe933ad5bf3 Mon Sep 17 00:00:00 2001 From: Gonzalo de Pedro Date: Fri, 24 Nov 2023 17:40:45 -0300 Subject: [PATCH 19/23] andino urdf Signed-off-by: Gonzalo de Pedro --- andino_description/urdf/andino.urdf.xacro | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/andino_description/urdf/andino.urdf.xacro b/andino_description/urdf/andino.urdf.xacro index 099b5716..175bf58a 100644 --- a/andino_description/urdf/andino.urdf.xacro +++ b/andino_description/urdf/andino.urdf.xacro @@ -39,13 +39,18 @@ - + +<<<<<<< HEAD +======= + + +>>>>>>> 737a061 (andino urdf) @@ -79,13 +84,16 @@ - + + + From 67dbca16f311bdcd552714adcb5f214b2e5c8b99 Mon Sep 17 00:00:00 2001 From: Gonzalo de Pedro Date: Fri, 24 Nov 2023 17:47:34 -0300 Subject: [PATCH 20/23] Fixed dangling conflicts Signed-off-by: Gonzalo de Pedro --- andino_description/urdf/andino.urdf.xacro | 7 +------ 1 file changed, 1 insertion(+), 6 deletions(-) diff --git a/andino_description/urdf/andino.urdf.xacro b/andino_description/urdf/andino.urdf.xacro index 175bf58a..52377037 100644 --- a/andino_description/urdf/andino.urdf.xacro +++ b/andino_description/urdf/andino.urdf.xacro @@ -39,18 +39,13 @@ - + -<<<<<<< HEAD -======= - - ->>>>>>> 737a061 (andino urdf) From 22286880d2b4a4bf0f10a65a698dbede11a50c78 Mon Sep 17 00:00:00 2001 From: Gonzalo de Pedro Date: Tue, 16 Jan 2024 17:08:07 -0300 Subject: [PATCH 21/23] Added commands for imu Signed-off-by: Gonzalo de Pedro --- andino_firmware/src/app.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/andino_firmware/src/app.cpp b/andino_firmware/src/app.cpp index 8517182f..89cff20b 100644 --- a/andino_firmware/src/app.cpp +++ b/andino_firmware/src/app.cpp @@ -129,6 +129,8 @@ void App::setup() { 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::kReadHasImu, cmd_read_has_imu_cb); + shell_.register_command(Commands::kReadEncodersAndImu, cmd_read_encoders_and_imu_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); From 5de88d16f189b020e61d7e88ca88a497f37a73a2 Mon Sep 17 00:00:00 2001 From: Gonzalo de Pedro Date: Tue, 16 Jan 2024 17:17:16 -0300 Subject: [PATCH 22/23] Fixed has imu command variable name Signed-off-by: Gonzalo de Pedro --- andino_firmware/src/commands.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/andino_firmware/src/commands.h b/andino_firmware/src/commands.h index e7c1e749..6cf1c960 100644 --- a/andino_firmware/src/commands.h +++ b/andino_firmware/src/commands.h @@ -75,7 +75,7 @@ struct Commands { /// @brief Reads the encoders tick count values. static constexpr const char* kReadEncoders{"e"}; /// @brief Reads if there is an IMU connected. - static constexpr const char* kHasImu{"h"}; + static constexpr const char* kReadHasImu{"h"}; /// @brief Reads the encoders tick count values and IMU data. static constexpr const char* kReadEncodersAndImu{"i"}; /// @brief Sets the encoders ticks count to zero. From 6904fd57f2868afd4b951a95876e81ab8ec18450 Mon Sep 17 00:00:00 2001 From: Gonzalo de Pedro Date: Wed, 24 Jan 2024 16:21:08 -0300 Subject: [PATCH 23/23] Remove andino firmware changes Signed-off-by: Gonzalo de Pedro --- andino_firmware/README.md | 3 - andino_firmware/platformio.ini | 11 +- andino_firmware/src/app.cpp | 212 ++++++++++++++++----------------- andino_firmware/src/app.h | 45 +++---- andino_firmware/src/commands.h | 18 ++- andino_firmware/src/hw.h | 4 +- andino_firmware/src/shell.cpp | 105 ---------------- andino_firmware/src/shell.h | 110 ----------------- 8 files changed, 139 insertions(+), 369 deletions(-) delete mode 100644 andino_firmware/src/shell.cpp delete mode 100644 andino_firmware/src/shell.h diff --git a/andino_firmware/README.md b/andino_firmware/README.md index 8e2fab0b..c8995ae9 100644 --- a/andino_firmware/README.md +++ b/andino_firmware/README.md @@ -8,9 +8,6 @@ Check `encoder_driver.h` and `motor_driver.h` files to check the expected pins f ## Installation -In Arduino IDE, go to `tools->Manage Libraries ...` and install: -- "Adafruit BNO055" - Verify and Upload `andino_firmware.ino` to your arduino board. ## Description diff --git a/andino_firmware/platformio.ini b/andino_firmware/platformio.ini index f07c409c..b6cbfd70 100644 --- a/andino_firmware/platformio.ini +++ b/andino_firmware/platformio.ini @@ -13,12 +13,11 @@ platform = atmelavr framework = arduino monitor_speed = 57600 -lib_deps = - Wire - SPI - adafruit/Adafruit BNO055 - adafruit/Adafruit BusIO - adafruit/Adafruit Unified Sensor + +; Base configuration for build tools. +[base_build] +build_flags = + -Wall -Wextra ; Environment for Arduino Uno. [env:uno] diff --git a/andino_firmware/src/app.cpp b/andino_firmware/src/app.cpp index 89cff20b..faf41ef7 100644 --- a/andino_firmware/src/app.cpp +++ b/andino_firmware/src/app.cpp @@ -71,13 +71,6 @@ #include "hw.h" #include "motor.h" #include "pid.h" -#include "shell.h" - -/*BNO055 Imu */ -#include -#include -#include -#include // TODO(jballoffet): Move this variables to a different module. @@ -86,11 +79,21 @@ unsigned long nextPID = andino::Constants::kPidPeriod; long lastMotorCommand = andino::Constants::kAutoStopWindow; -bool HAS_IMU = true; +// A pair of varibles to help parse serial commands +int arg = 0; +int index = 0; -namespace andino { +// Variable to hold an input character +char chr; + +// Variable to hold the current single-character command +char cmd; -Shell App::shell_; +// Character arrays to hold the first and second arguments +char argv1[16]; +char argv2[16]; + +namespace andino { Motor App::left_motor_(Hw::kLeftMotorEnableGpioPin, Hw::kLeftMotorForwardGpioPin, Hw::kLeftMotorBackwardGpioPin); @@ -105,8 +108,6 @@ PID App::left_pid_controller_(Constants::kPidKp, Constants::kPidKd, Constants::k PID App::right_pid_controller_(Constants::kPidKp, Constants::kPidKd, Constants::kPidKi, Constants::kPidKo, -Constants::kPwmMax, Constants::kPwmMax); -Adafruit_BNO055 App::bno_{55, BNO055_ADDRESS_A, &Wire}; - void App::setup() { // Required by Arduino libraries to work. init(); @@ -122,32 +123,47 @@ 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::kReadHasImu, cmd_read_has_imu_cb); - shell_.register_command(Commands::kReadEncodersAndImu, cmd_read_encoders_and_imu_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); - /* Initialise the IMU sensor */ - if(!bno_.begin()) - { - /* There was a problem detecting the BNO055 ... check your connections */ - Serial.print("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!"); - HAS_IMU = false; - } - bno_.setExtCrystalUse(true); } void App::loop() { - // Process command prompt input. - shell_.process_input(); + 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++; + } + } + } // Run a PID calculation at the appropriate intervals if (millis() > nextPID) { @@ -175,75 +191,62 @@ void App::loop() { } } -void App::cmd_unknown_cb(int, char**) { Serial.println("Unknown command."); } +void App::reset_command() { + cmd = 0; + memset(argv1, 0, sizeof(argv1)); + memset(argv2, 0, sizeof(argv2)); + arg = 0; + index = 0; +} -void App::cmd_read_analog_gpio_cb(int argc, char** argv) { - if (argc < 2) { - return; +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."); } - const int pin = atoi(argv[1]); +void App::cmd_read_analog_gpio(const char* arg1, const char*) { + const int pin = atoi(arg1); Serial.println(analogRead(pin)); } -void App::cmd_read_digital_gpio_cb(int argc, char** argv) { - if (argc < 2) { - return; - } - - const int pin = atoi(argv[1]); +void App::cmd_read_digital_gpio(const char* arg1, const char*) { + const int pin = atoi(arg1); Serial.println(digitalRead(pin)); } -void App::cmd_read_encoders_cb(int, char**) { +void App::cmd_read_encoders(const char*, const char*) { Serial.print(left_encoder_.read()); Serial.print(" "); Serial.println(right_encoder_.read()); } -void App::cmd_read_has_imu_cb(int argc, char** argv) { - Serial.println(HAS_IMU); -} - -void App::cmd_read_encoders_and_imu_cb(int argc, char** argv) { - Serial.print(left_encoder_.read()); - Serial.print(" "); - Serial.print(right_encoder_.read()); - Serial.print(" "); - - // Quaternion data - imu::Quaternion quat = bno_.getQuat(); - Serial.print(quat.x(), 4); - Serial.print(" "); - Serial.print(quat.y(), 4); - Serial.print(" "); - Serial.print(quat.z(), 4); - Serial.print(" "); - Serial.print(quat.w(), 4); - Serial.print(" "); - - /* Display the floating point data */ - imu::Vector<3> euler_angvel = bno_.getVector(Adafruit_BNO055::VECTOR_GYROSCOPE); - Serial.print(euler_angvel.x()); - Serial.print(" "); - Serial.print(euler_angvel.y()); - Serial.print(" "); - Serial.print(euler_angvel.z()); - Serial.print(" "); - - /* Display the floating point data */ - imu::Vector<3> euler_linearaccel = bno_.getVector(Adafruit_BNO055::VECTOR_LINEARACCEL); - Serial.print(euler_linearaccel.x()); - Serial.print(" "); - Serial.print(euler_linearaccel.y()); - Serial.print(" "); - Serial.print(euler_linearaccel.z()); - Serial.print("\t\t"); - - Serial.println("OK"); -} - -void App::cmd_reset_encoders_cb(int, char**) { +void App::cmd_reset_encoders(const char*, const char*) { left_encoder_.reset(); right_encoder_.reset(); left_pid_controller_.reset(left_encoder_.read()); @@ -251,13 +254,9 @@ void App::cmd_reset_encoders_cb(int, char**) { Serial.println("OK"); } -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]); +void App::cmd_set_motors_speed(const char* arg1, const char* arg2) { + const int left_motor_speed = atoi(arg1); + const int right_motor_speed = atoi(arg2); // Reset the auto stop timer. lastMotorCommand = millis(); @@ -280,13 +279,9 @@ void App::cmd_set_motors_speed_cb(int argc, char** argv) { Serial.println("OK"); } -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]); +void App::cmd_set_motors_pwm(const char* arg1, const char* arg2) { + const int left_motor_pwm = atoi(arg1); + const int right_motor_pwm = atoi(arg2); // Reset the auto stop timer. lastMotorCommand = millis(); @@ -300,12 +295,7 @@ void App::cmd_set_motors_pwm_cb(int argc, char** argv) { Serial.println("OK"); } -void App::cmd_set_pid_tuning_gains_cb(int argc, char** argv) { - // TODO(jballoffet): Refactor to expect command multiple arguments. - if (argc < 2) { - return; - } - +void App::cmd_set_pid_tuning_gains(const char* arg1, const char*) { static constexpr int kSizePidArgs{4}; int i = 0; char arg[20]; @@ -313,7 +303,7 @@ void App::cmd_set_pid_tuning_gains_cb(int argc, char** argv) { int pid_args[kSizePidArgs]{0, 0, 0, 0}; // Example: "u 30:20:10:50". - strcpy(arg, argv[1]); + strcpy(arg, arg1); char* p = arg; while ((str = strtok_r(p, ":", &p)) != NULL && i < kSizePidArgs) { pid_args[i] = atoi(str); diff --git a/andino_firmware/src/app.h b/andino_firmware/src/app.h index 4b9ea02e..b0745f6e 100644 --- a/andino_firmware/src/app.h +++ b/andino_firmware/src/app.h @@ -32,8 +32,6 @@ #include "encoder.h" #include "motor.h" #include "pid.h" -#include "shell.h" -#include namespace andino { @@ -50,38 +48,45 @@ 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). - static void cmd_unknown_cb(int argc, char** argv); + // TODO(jballoffet): Parse arguments within callback method. + static void cmd_unknown(const char* arg1, const char* arg2); /// Callback method for the `Commands::kReadAnalogGpio` command. - static void cmd_read_analog_gpio_cb(int argc, char** argv); + // TODO(jballoffet): Parse arguments within callback method. + static void cmd_read_analog_gpio(const char* arg1, const char* arg2); /// Callback method for the `Commands::kReadDigitalGpio` command. - static void cmd_read_digital_gpio_cb(int argc, char** argv); + // TODO(jballoffet): Parse arguments within callback method. + static void cmd_read_digital_gpio(const char* arg1, const char* arg2); /// Callback method for the `Commands::kReadEncoders` command. - static void cmd_read_encoders_cb(int argc, char** argv); - - /// Callback method for the `Commands::kReadHasImu` command. - static void cmd_read_has_imu_cb(int argc, char** argv); - - /// Callback method for the `Commands::kReadEncodersAndImu` command. - static void cmd_read_encoders_and_imu_cb(int argc, char** argv); + // TODO(jballoffet): Parse arguments within callback method. + static void cmd_read_encoders(const char* arg1, const char* arg2); /// Callback method for the `Commands::kResetEncoders` command. - static void cmd_reset_encoders_cb(int argc, char** argv); + // TODO(jballoffet): Parse arguments within callback method. + static void cmd_reset_encoders(const char* arg1, const char* arg2); /// Callback method for the `Commands::kSetMotorsSpeed` command. - static void cmd_set_motors_speed_cb(int argc, char** argv); + // TODO(jballoffet): Parse arguments within callback method. + static void cmd_set_motors_speed(const char* arg1, const char* arg2); /// Callback method for the `Commands::kSetMotorsPwm` command. - static void cmd_set_motors_pwm_cb(int argc, char** argv); + // TODO(jballoffet): Parse arguments within callback method. + static void cmd_set_motors_pwm(const char* arg1, const char* arg2); /// Callback method for the `Commands::kSetPidsTuningGains` command. - static void cmd_set_pid_tuning_gains_cb(int argc, char** argv); - - /// Application command shell. - static Shell shell_; + // TODO(jballoffet): Parse arguments within callback method. + static void cmd_set_pid_tuning_gains(const char* arg1, const char* arg2); /// Motors (one per wheel). static Motor left_motor_; @@ -94,8 +99,6 @@ class App { /// PID controllers (one per wheel). static PID left_pid_controller_; static PID right_pid_controller_; - - static Adafruit_BNO055 bno_; }; } // namespace andino diff --git a/andino_firmware/src/commands.h b/andino_firmware/src/commands.h index 6cf1c960..43efa111 100644 --- a/andino_firmware/src/commands.h +++ b/andino_firmware/src/commands.h @@ -69,23 +69,19 @@ namespace andino { /// @brief CLI commands. struct Commands { /// @brief Reads an analog GPIO. - static constexpr const char* kReadAnalogGpio{"a"}; + static constexpr char kReadAnalogGpio{'a'}; /// @brief Reads a digital GPIO. - static constexpr const char* kReadDigitalGpio{"d"}; + static constexpr char kReadDigitalGpio{'d'}; /// @brief Reads the encoders tick count values. - static constexpr const char* kReadEncoders{"e"}; - /// @brief Reads if there is an IMU connected. - static constexpr const char* kReadHasImu{"h"}; - /// @brief Reads the encoders tick count values and IMU data. - static constexpr const char* kReadEncodersAndImu{"i"}; + static constexpr char kReadEncoders{'e'}; /// @brief Sets the encoders ticks count to zero. - static constexpr const char* kResetEncoders{"r"}; + static constexpr char kResetEncoders{'r'}; /// @brief Sets the motors speed [ticks/s]. - static constexpr const char* kSetMotorsSpeed{"m"}; + static constexpr char kSetMotorsSpeed{'m'}; /// @brief Sets the motors PWM value [duty range: 0-255]. - static constexpr const char* kSetMotorsPwm{"o"}; + static constexpr char kSetMotorsPwm{'o'}; /// @brief Sets the PIDs tuning gains [format: "kp:kd:ki:ko"]. - static constexpr const char* kSetPidsTuningGains{"u"}; + static constexpr char kSetPidsTuningGains{'u'}; }; } // namespace andino diff --git a/andino_firmware/src/hw.h b/andino_firmware/src/hw.h index e665dced..0b50b2d9 100644 --- a/andino_firmware/src/hw.h +++ b/andino_firmware/src/hw.h @@ -39,9 +39,9 @@ struct Hw { static constexpr int kLeftEncoderChannelBGpioPin{3}; /// @brief Right encoder channel A pin. Connected to PC4 (digital pin 18, analog pin A4). - static constexpr int kRightEncoderChannelAGpioPin{14}; + static constexpr int kRightEncoderChannelAGpioPin{18}; /// @brief Right encoder channel B pin. Connected to PC5 (digital pin 19, analog pin A5). - static constexpr int kRightEncoderChannelBGpioPin{15}; + static constexpr int kRightEncoderChannelBGpioPin{19}; /// @brief Left motor driver backward pin. Connected to PD6 (digital pin 6). static constexpr int kLeftMotorBackwardGpioPin{6}; diff --git a/andino_firmware/src/shell.cpp b/andino_firmware/src/shell.cpp deleted file mode 100644 index 6c8dfdc0..00000000 --- a/andino_firmware/src/shell.cpp +++ /dev/null @@ -1,105 +0,0 @@ -// 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 - -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; -} - -void Shell::process_input() { - while (stream_->available() > 0) { - 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::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); -} - -void Shell::execute_callback(int argc, char** argv) { - for (size_t i = 0; i < commands_count_; i++) { - 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_(argc, argv); - } -} - -} // namespace andino diff --git a/andino_firmware/src/shell.h b/andino_firmware/src/shell.h deleted file mode 100644 index b4a8212b..00000000 --- a/andino_firmware/src/shell.h +++ /dev/null @@ -1,110 +0,0 @@ -// 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. -#pragma once - -#include - -#include "Arduino.h" - -namespace andino { - -/// @brief This class interprets and processes the commands entered by the user. -class Shell { - public: - /// @brief Command callback type. - typedef void (*CommandCallback)(int argc, char** argv); - - /// @brief Initializes the shell. - /// - /// @param stream Data stream. - void init(Stream& stream); - - /// @brief Sets the default callback for unknown commands. - /// - /// @param callback Callback function. - void set_default_callback(CommandCallback callback); - - /// @brief Adds a command registry entry to the shell. - /// - /// @param name Command name. - /// @param callback Callback function. - void register_command(const char* name, CommandCallback callback); - - /// @brief Processes the available input at the command prompt (if any). Meant to be called - /// continously. - void process_input(); - - private: - /// Maximum command name length. - static constexpr int kCommandNameLengthMax{8}; - - /// Command registry entry definition. - struct Command { - /// Command name. - char name[kCommandNameLengthMax]; - /// Callback function. - CommandCallback callback; - }; - - /// 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(int argc, char** argv); - - /// Data stream. - Stream* stream_{nullptr}; - - /// Default callback for unknown commands. - CommandCallback default_callback_{nullptr}; - - /// Command registry. - Command commands_[kCommandsMax]; - - /// Number of registered commands. - size_t commands_count_{0}; - - /// Command prompt message. - char message_buffer_[kCommandPromptLengthMax]; - - /// Command prompt message index. - int message_index_{0}; -}; - -} // namespace andino