From 27fdd85ad42bbe9029684b9e58e2ee1a5eebaf66 Mon Sep 17 00:00:00 2001 From: Javier Balloffet Date: Sun, 3 Dec 2023 23:38:55 +0100 Subject: [PATCH] Add command methods Signed-off-by: Javier Balloffet --- andino_firmware/src/app.cpp | 216 ++++++++++++++++++--------------- andino_firmware/src/app.h | 32 +++++ andino_firmware/src/commands.h | 38 +++--- 3 files changed, 172 insertions(+), 114 deletions(-) diff --git a/andino_firmware/src/app.cpp b/andino_firmware/src/app.cpp index 03c0e554..47f85ff1 100644 --- a/andino_firmware/src/app.cpp +++ b/andino_firmware/src/app.cpp @@ -93,10 +93,6 @@ char cmd; char argv1[16]; char argv2[16]; -// The arguments converted to integers -long arg1; -long arg2; - namespace andino { Motor App::left_motor_(Hw::kLeftMotorEnableGpioPin, Hw::kLeftMotorForwardGpioPin, @@ -199,114 +195,142 @@ void App::reset_command() { cmd = 0; memset(argv1, 0, sizeof(argv1)); memset(argv2, 0, sizeof(argv2)); - arg1 = 0; - arg2 = 0; arg = 0; index = 0; } void App::run_command() { - 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(Constants::kBaudrate); - break; - case ANALOG_READ: - Serial.println(analogRead(arg1)); + case Commands::kreadAnalogGpio: + cmd_read_analog_gpio(argv1, argv2); break; - case DIGITAL_READ: - Serial.println(digitalRead(arg1)); + case Commands::kreadDigitalGpio: + cmd_read_digital_gpio(argv1, argv2); break; - case ANALOG_WRITE: - analogWrite(arg1, arg2); - Serial.println("OK"); + case Commands::kreadEncoders: + cmd_read_encoders(argv1, argv2); break; - case DIGITAL_WRITE: - if (arg2 == 0) - digitalWrite(arg1, LOW); - else if (arg2 == 1) - digitalWrite(arg1, HIGH); - Serial.println("OK"); + case Commands::kresetEncoders: + cmd_reset_encoders(argv1, argv2); break; - case PIN_MODE: - if (arg2 == 0) - pinMode(arg1, INPUT); - else if (arg2 == 1) - pinMode(arg1, OUTPUT); - Serial.println("OK"); + case Commands::ksetMotorsSpeed: + cmd_set_motors_speed(argv1, argv2); break; - case READ_ENCODERS: - Serial.print(left_encoder_.read()); - Serial.print(" "); - Serial.println(right_encoder_.read()); + case Commands::ksetMotorsPwm: + cmd_set_motors_pwm(argv1, argv2); 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 / Constants::kPidRate); - right_pid_controller_.set_setpoint(arg2 / Constants::kPidRate); - 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"); + case Commands::ksetPidsTuningGains: + cmd_set_pid_tuning_gains(argv1, argv2); break; default: - Serial.println("Invalid Command"); + cmd_unknown(argv1, argv2); break; } } +void App::cmd_unknown(const char* arg1, const char* arg2) { + (void)arg1; + (void)arg2; + Serial.println("Unknown command."); +} + +void App::cmd_read_analog_gpio(const char* arg1, const char* arg2) { + (void)arg2; + int pin = atoi(arg1); + Serial.println(analogRead(pin)); +} + +void App::cmd_read_digital_gpio(const char* arg1, const char* arg2) { + (void)arg2; + int pin = atoi(arg1); + Serial.println(digitalRead(pin)); +} + +void App::cmd_read_encoders(const char* arg1, const char* arg2) { + (void)arg1; + (void)arg2; + Serial.print(left_encoder_.read()); + Serial.print(" "); + Serial.println(right_encoder_.read()); +} + +void App::cmd_reset_encoders(const char* arg1, const char* arg2) { + (void)arg1; + (void)arg2; + left_encoder_.reset(); + right_encoder_.reset(); + left_pid_controller_.reset(left_encoder_.read()); + right_pid_controller_.reset(right_encoder_.read()); + Serial.println("OK"); +} + +void App::cmd_set_motors_speed(const char* arg1, const char* arg2) { + int left_motor_speed = atoi(arg1); + int right_motor_speed = atoi(arg2); + + // Reset the auto stop timer. + lastMotorCommand = millis(); + if (left_motor_speed == 0 && right_motor_speed == 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 + // Constants::kPidRate. + left_pid_controller_.set_setpoint(left_motor_speed / Constants::kPidRate); + right_pid_controller_.set_setpoint(right_motor_speed / Constants::kPidRate); + Serial.println("OK"); +} + +void App::cmd_set_motors_pwm(const char* arg1, const char* arg2) { + int left_motor_pwm = atoi(arg1); + int right_motor_pwm = atoi(arg2); + + // 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(left_motor_pwm); + right_motor_.set_speed(right_motor_pwm); + Serial.println("OK"); +} + +void App::cmd_set_pid_tuning_gains(const char* arg1, const char* arg2) { + (void)arg2; + + int i = 0; + char arg[20]; + char* str; + int pid_args[4]; + + // Example: "u 30:20:10:50". + strcpy(arg, arg1); + char* p = arg; + 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"); +} + } // namespace andino diff --git a/andino_firmware/src/app.h b/andino_firmware/src/app.h index ffdc0388..46b5c4c1 100644 --- a/andino_firmware/src/app.h +++ b/andino_firmware/src/app.h @@ -56,6 +56,38 @@ class App { // 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); + + /// 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); + + /// 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); + + /// 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); + + /// 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); + + /// 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); + + /// 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); + + /// 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); + /// Motors (one per wheel). static Motor left_motor_; static Motor right_motor_; diff --git a/andino_firmware/src/commands.h b/andino_firmware/src/commands.h index bacd2027..81662765 100644 --- a/andino_firmware/src/commands.h +++ b/andino_firmware/src/commands.h @@ -62,24 +62,26 @@ // 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 -/* Define single-letter commands that will be sent by the PC over the - serial link. -*/ +namespace andino { -#ifndef COMMANDS_H -#define COMMANDS_H +/// @brief CLI commands. +struct Commands { + /// @brief Reads an analog GPIO. + static constexpr char kreadAnalogGpio{'a'}; + /// @brief Reads a digital GPIO. + static constexpr char kreadDigitalGpio{'d'}; + /// @brief Reads the encoders tick count values. + static constexpr char kreadEncoders{'e'}; + /// @brief Sets the encoders ticks count to zero. + static constexpr char kresetEncoders{'r'}; + /// @brief Sets the motors speed [ticks/s]. + static constexpr char ksetMotorsSpeed{'m'}; + /// @brief Sets the motors PWM value [duty range: 0-255]. + static constexpr char ksetMotorsPwm{'o'}; + /// @brief Sets the PIDs tuning gains [format: "kp:kd:ki:ko"]. + static constexpr char ksetPidsTuningGains{'u'}; +}; -#define ANALOG_READ 'a' -#define GET_BAUDRATE 'b' -#define PIN_MODE 'c' -#define DIGITAL_READ 'd' -#define READ_ENCODERS 'e' -#define MOTOR_SPEEDS 'm' -#define MOTOR_RAW_PWM 'o' -#define RESET_ENCODERS 'r' -#define UPDATE_PID 'u' -#define DIGITAL_WRITE 'w' -#define ANALOG_WRITE 'x' - -#endif +} // namespace andino