From 1db6993e70558a718d5d67457524365b6e6e41ba Mon Sep 17 00:00:00 2001 From: Javier Balloffet Date: Sun, 19 Nov 2023 19:51:44 +0100 Subject: [PATCH] Add App class Signed-off-by: Javier Balloffet --- andino_firmware/src/app.cpp | 272 ++++++++++++++++------------------- andino_firmware/src/app.h | 40 +++++- andino_firmware/src/main.cpp | 6 +- 3 files changed, 167 insertions(+), 151 deletions(-) diff --git a/andino_firmware/src/app.cpp b/andino_firmware/src/app.cpp index e4ac8957..f42646a4 100644 --- a/andino_firmware/src/app.cpp +++ b/andino_firmware/src/app.cpp @@ -64,25 +64,19 @@ // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "app.h" -/* Serial port baud rate */ -#define BAUDRATE 57600 -/* Maximum PWM signal */ -#define MAX_PWM 255 - #include "Arduino.h" -#include "hw.h" - -/* Include definition of serial commands */ #include "commands.h" - -/* Motor driver function definitions */ +#include "encoder.h" +#include "hw.h" #include "motor.h" +#include "pid.h" -/* Encoder driver function definitions */ -#include "encoder.h" +// TODO(jballoffet): Move this variables and constants to a different module. -/* PID parameters and functions */ -#include "pid.h" +/* Serial port baud rate */ +#define BAUDRATE 57600 +/* Maximum PWM signal */ +#define MAX_PWM 255 /* Run the PID loop at 30 times per second */ #define PID_RATE 30 // Hz @@ -98,8 +92,6 @@ unsigned long nextPID = PID_INTERVAL; #define AUTO_STOP_INTERVAL 3000 long lastMotorCommand = AUTO_STOP_INTERVAL; -/* Variable initialization */ - // A pair of varibles to help parse serial commands int arg = 0; int index = 0; @@ -118,22 +110,103 @@ char argv2[16]; 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); +namespace andino { + +Motor App::left_motor_(LEFT_MOTOR_ENABLE_GPIO_PIN, LEFT_MOTOR_FORWARD_GPIO_PIN, + LEFT_MOTOR_BACKWARD_GPIO_PIN); +Motor App::right_motor_(RIGHT_MOTOR_ENABLE_GPIO_PIN, RIGHT_MOTOR_FORWARD_GPIO_PIN, + RIGHT_MOTOR_BACKWARD_GPIO_PIN); + +Encoder App::left_encoder_(LEFT_ENCODER_A_GPIO_PIN, LEFT_ENCODER_B_GPIO_PIN); +Encoder App::right_encoder_(RIGHT_ENCODER_A_GPIO_PIN, RIGHT_ENCODER_B_GPIO_PIN); + +PID App::left_pid_controller_(30, 10, 0, 10, -MAX_PWM, MAX_PWM); +PID App::right_pid_controller_(30, 10, 0, 10, -MAX_PWM, MAX_PWM); + +void App::setup() { + // Required by Arduino libraries to work. + init(); + + 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()); +} + +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++; + } + } + } + + // Run a PID calculation at the appropriate intervals + if (millis() > nextPID) { + int left_motor_speed = 0; + int right_motor_speed = 0; + left_pid_controller_.compute(left_encoder_.read(), left_motor_speed); + right_pid_controller_.compute(right_encoder_.read(), right_motor_speed); + left_motor_.set_speed(left_motor_speed); + right_motor_.set_speed(right_motor_speed); + nextPID += PID_INTERVAL; + } -// 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); + // Check to see if we have exceeded the auto-stop interval + if ((millis() - lastMotorCommand) > AUTO_STOP_INTERVAL) { + lastMotorCommand = millis(); + left_motor_.set_speed(0); + right_motor_.set_speed(0); + left_pid_controller_.enable(false); + right_pid_controller_.enable(false); + } -// 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); + // Required by Arduino libraries to work. + if (serialEventRun) { + serialEventRun(); + } +} -/* Clear the current command parameters */ -void resetCommand() { +void App::reset_command() { cmd = 0; memset(argv1, 0, sizeof(argv1)); memset(argv2, 0, sizeof(argv2)); @@ -143,8 +216,7 @@ void resetCommand() { index = 0; } -/* Run a command. Commands are defined in commands.h */ -int runCommand() { +void App::run_command() { int i = 0; char* p = argv1; char* str; @@ -181,47 +253,47 @@ int runCommand() { Serial.println("OK"); break; case READ_ENCODERS: - Serial.print(left_encoder.read()); + Serial.print(left_encoder_.read()); Serial.print(" "); - Serial.println(right_encoder.read()); + 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()); + 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); + 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); + 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); + 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()); + 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); + 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: @@ -230,8 +302,8 @@ int runCommand() { 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]); + 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(" "); @@ -248,94 +320,4 @@ int runCommand() { } } -namespace andino { - -/* Setup function--runs once at startup. */ -void setup() { - // Required by Arduino libraries to work. - init(); - - 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()); -} - -/* 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++; - } - } - } - - // Run a PID calculation at the appropriate intervals - if (millis() > nextPID) { - int left_motor_speed = 0; - int right_motor_speed = 0; - left_pid_controller.compute(left_encoder.read(), left_motor_speed); - right_pid_controller.compute(right_encoder.read(), right_motor_speed); - left_motor.set_speed(left_motor_speed); - right_motor.set_speed(right_motor_speed); - nextPID += PID_INTERVAL; - } - - // Check to see if we have exceeded the auto-stop interval - if ((millis() - lastMotorCommand) > AUTO_STOP_INTERVAL) { - lastMotorCommand = millis(); - left_motor.set_speed(0); - right_motor.set_speed(0); - left_pid_controller.enable(false); - right_pid_controller.enable(false); - } - - // Required by Arduino libraries to work. - if (serialEventRun) { - serialEventRun(); - } -} - } // namespace andino diff --git a/andino_firmware/src/app.h b/andino_firmware/src/app.h index 50e8a834..ffdc0388 100644 --- a/andino_firmware/src/app.h +++ b/andino_firmware/src/app.h @@ -29,10 +29,44 @@ // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once +#include "encoder.h" +#include "motor.h" +#include "pid.h" + namespace andino { -// TODO(jballoffet): Turn this into a class. -void setup(); -void loop(); +/// @brief This class wraps the MCU main application. +class App { + public: + /// This class only contains static members. + App() = delete; + + /// @brief Configures and sets the application up. Meant to be called once at startup. + static void setup(); + + /// @brief Application main run loop. Meant to be called continously. + 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(); + + /// Motors (one per wheel). + static Motor left_motor_; + static Motor right_motor_; + + /// Encoders (one per wheel). + static Encoder left_encoder_; + static Encoder right_encoder_; + + /// PID controllers (one per wheel). + static PID left_pid_controller_; + static PID right_pid_controller_; +}; } // namespace andino diff --git a/andino_firmware/src/main.cpp b/andino_firmware/src/main.cpp index 5334b04f..dc2e9621 100644 --- a/andino_firmware/src/main.cpp +++ b/andino_firmware/src/main.cpp @@ -33,12 +33,12 @@ /// /// @return Execution final status (never reached). int main(void) { - // MCU configuration. - andino::setup(); + // MCU and application configuration. + andino::App::setup(); // Main run loop. while (1) { - andino::loop(); + andino::App::loop(); } return 0;