From 14e9bef8d472f76b8212627e32195b2b1683d4f8 Mon Sep 17 00:00:00 2001 From: Gonzalo de Pedro Date: Wed, 24 Jan 2024 16:19:49 -0300 Subject: [PATCH] Changes to add IMU to andino firmware Signed-off-by: Gonzalo de Pedro --- andino_firmware/README.md | 3 + andino_firmware/platformio.ini | 6 + andino_firmware/src/app.cpp | 255 ++++++++++++++++++++------------- andino_firmware/src/app.h | 10 ++ andino_firmware/src/commands.h | 4 + andino_firmware/src/hw.h | 4 +- 6 files changed, 180 insertions(+), 102 deletions(-) diff --git a/andino_firmware/README.md b/andino_firmware/README.md index f4337c8b..cb1e2d8e 100644 --- a/andino_firmware/README.md +++ b/andino_firmware/README.md @@ -9,6 +9,9 @@ Check `encoder_driver.h` and `motor_driver.h` files to check the expected pins f ## Installation ### Arduino +In Arduino IDE, go to `tools->Manage Libraries ...` and install: +- "Adafruit BNO055" + Verify and Upload `andino_firmware.ino` to your arduino board. ### PlatformIO diff --git a/andino_firmware/platformio.ini b/andino_firmware/platformio.ini index f3b40bbe..65256063 100644 --- a/andino_firmware/platformio.ini +++ b/andino_firmware/platformio.ini @@ -22,6 +22,12 @@ platform = atmelavr framework = arduino monitor_speed = 57600 test_ignore = desktop/* +lib_deps = + Wire + SPI + adafruit/Adafruit BNO055 + adafruit/Adafruit BusIO + adafruit/Adafruit Unified Sensor ; Base configuration for desktop platforms (for unit testing). [base_desktop] diff --git a/andino_firmware/src/app.cpp b/andino_firmware/src/app.cpp index f9577468..1b4dab65 100644 --- a/andino_firmware/src/app.cpp +++ b/andino_firmware/src/app.cpp @@ -64,7 +64,11 @@ // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "app.h" +#include +#include #include +#include +#include #include "commands.h" #include "constants.h" @@ -115,6 +119,8 @@ unsigned long App::last_pid_computation_{0}; unsigned long App::last_set_motors_speed_cmd_{0}; +Adafruit_BNO055 App::bno_{55, BNO055_ADDRESS_A, &Wire}; + void App::setup() { // Required by Arduino libraries to work. init(); @@ -138,10 +144,20 @@ 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); 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() { @@ -171,132 +187,171 @@ void App::adjust_motors_speed() { 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); - if (left_pid_controller_.enabled()){ + if (left_pid_controller_.enabled()) { left_motor_.set_speed(left_motor_speed); } - if (right_pid_controller_.enabled()){ + if (right_pid_controller_.enabled()) { right_motor_.set_speed(right_motor_speed); } -} -void App::stop_motors() { - left_motor_.set_speed(0); - right_motor_.set_speed(0); - left_pid_controller_.disable(); - right_pid_controller_.disable(); -} - -void App::cmd_unknown_cb(int, char**) { Serial.println("Unknown command."); } - -void App::cmd_read_analog_gpio_cb(int argc, char** argv) { - if (argc < 2) { - return; + void App::stop_motors() { + left_motor_.set_speed(0); + right_motor_.set_speed(0); + left_pid_controller_.disable(); + right_pid_controller_.disable(); } - const int pin = atoi(argv[1]); - Serial.println(analogRead(pin)); -} + void App::cmd_unknown_cb(int, char**) { Serial.println("Unknown command."); } -void App::cmd_read_digital_gpio_cb(int argc, char** argv) { - if (argc < 2) { - return; - } + void App::cmd_read_analog_gpio_cb(int argc, char** argv) { + if (argc < 2) { + return; + } - const int pin = atoi(argv[1]); - Serial.println(digitalRead(pin)); -} + const int pin = atoi(argv[1]); + Serial.println(analogRead(pin)); + } -void App::cmd_read_encoders_cb(int, char**) { - Serial.print(left_encoder_.read()); - Serial.print(" "); - Serial.println(right_encoder_.read()); -} + void App::cmd_read_digital_gpio_cb(int argc, char** argv) { + if (argc < 2) { + return; + } -void App::cmd_reset_encoders_cb(int, char**) { - left_encoder_.reset(); - right_encoder_.reset(); - left_pid_controller_.reset(left_encoder_.read()); - right_pid_controller_.reset(right_encoder_.read()); - Serial.println("OK"); -} + const int pin = atoi(argv[1]); + Serial.println(digitalRead(pin)); + } -void App::cmd_set_motors_speed_cb(int argc, char** argv) { - if (argc < 3) { - return; + void App::cmd_read_encoders_cb(int, char**) { + Serial.print(left_encoder_.read()); + Serial.print(" "); + Serial.println(right_encoder_.read()); } - const int left_motor_speed = atoi(argv[1]); - const int right_motor_speed = atoi(argv[2]); + 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"); + } - // Reset the auto stop timer. - last_set_motors_speed_cmd_ = millis(); - if (left_motor_speed == 0 && right_motor_speed == 0) { - left_motor_.set_speed(0); - right_motor_.set_speed(0); + void App::cmd_reset_encoders_cb(int, char**) { + left_encoder_.reset(); + right_encoder_.reset(); left_pid_controller_.reset(left_encoder_.read()); right_pid_controller_.reset(right_encoder_.read()); - left_pid_controller_.disable(); - right_pid_controller_.disable(); - } else { - left_pid_controller_.enable(); - right_pid_controller_.enable(); + Serial.println("OK"); } - // 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_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_pwm_cb(int argc, char** argv) { - if (argc < 3) { - return; + // Reset the auto stop timer. + last_set_motors_speed_cmd_ = 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_.disable(); + right_pid_controller_.disable(); + } else { + left_pid_controller_.enable(); + right_pid_controller_.enable(); + } + + // 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"); } - const int left_motor_pwm = atoi(argv[1]); - const int right_motor_pwm = atoi(argv[2]); + void App::cmd_set_motors_pwm_cb(int argc, char** argv) { + if (argc < 3) { + return; + } - left_pid_controller_.reset(left_encoder_.read()); - right_pid_controller_.reset(right_encoder_.read()); - // Sneaky way to temporarily disable the PID. - left_pid_controller_.disable(); - right_pid_controller_.disable(); - left_motor_.set_speed(left_motor_pwm); - right_motor_.set_speed(right_motor_pwm); - Serial.println("OK"); -} + const int left_motor_pwm = atoi(argv[1]); + const int right_motor_pwm = atoi(argv[2]); -void App::cmd_set_pid_tuning_gains_cb(int argc, char** argv) { - // TODO(jballoffet): Refactor to expect command multiple arguments. - if (argc < 2) { - return; + left_pid_controller_.reset(left_encoder_.read()); + right_pid_controller_.reset(right_encoder_.read()); + // Sneaky way to temporarily disable the PID. + left_pid_controller_.disable(); + right_pid_controller_.disable(); + left_motor_.set_speed(left_motor_pwm); + right_motor_.set_speed(right_motor_pwm); + Serial.println("OK"); } - static constexpr int kSizePidArgs{4}; - int i = 0; - char arg[20]; - char* str; - int pid_args[kSizePidArgs]{0, 0, 0, 0}; - - // Example: "u 30:20:10:50". - strcpy(arg, argv[1]); - char* p = arg; - while ((str = strtok_r(p, ":", &p)) != NULL && i < kSizePidArgs) { - pid_args[i] = atoi(str); - i++; + 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]; + char* str; + int pid_args[kSizePidArgs]{0, 0, 0, 0}; + + // Example: "u 30:20:10:50". + strcpy(arg, argv[1]); + char* p = arg; + while ((str = strtok_r(p, ":", &p)) != NULL && i < kSizePidArgs) { + 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"); } - 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 937c7827..ed7569b4 100644 --- a/andino_firmware/src/app.h +++ b/andino_firmware/src/app.h @@ -29,6 +29,8 @@ // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once +#include + #include "digital_out_arduino.h" #include "encoder.h" #include "interrupt_in_arduino.h" @@ -71,6 +73,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); @@ -120,6 +128,8 @@ class App { /// Tracks the last time a `Commands::kSetMotorsSpeed` command was received. static unsigned long last_set_motors_speed_cmd_; + + static Adafruit_BNO055 bno_; }; } // namespace andino diff --git a/andino_firmware/src/commands.h b/andino_firmware/src/commands.h index ff3e5ac5..6cf1c960 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* 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. 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};