Changes to add IMU to andino firmware (#215)

* Changes to add IMU to andino firmware

Signed-off-by: Gonzalo de Pedro <[email protected]>

* Address comments

Signed-off-by: Javier Balloffet <[email protected]>

---------

Signed-off-by: Gonzalo de Pedro <[email protected]>
Signed-off-by: Javier Balloffet <[email protected]>
Co-authored-by: Gonzalo de Pedro <[email protected]>
Co-authored-by: Javier Balloffet <[email protected]>

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

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]

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 <Adafruit_BNO055.h>
+#include <Adafruit_Sensor.h>
 #include <Arduino.h>
+#include <Wire.h>
+#include <utility/imumaths.h>
 
 #include "commands.h"
 #include "constants.h"
@@ -115,6 +119,10 @@ unsigned long App::last_pid_computation_{0};
 
 unsigned long App::last_set_motors_speed_cmd_{0};
 
+bool App::is_imu_connected{false};
+
+Adafruit_BNO055 App::bno055_imu_{/*sensorID=*/55, BNO055_ADDRESS_A, &Wire};
+
 void App::setup() {
   // Required by Arduino libraries to work.
   init();
@@ -142,6 +150,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);
+  shell_.register_command(Commands::kGetIsImuConnected, cmd_get_is_imu_connected_cb);
+  shell_.register_command(Commands::kReadEncodersAndImu, cmd_read_encoders_and_imu_cb);
+
+  // Initialize IMU sensor.
+  if (bno055_imu_.begin()) {
+    bno055_imu_.setExtCrystalUse(true);
+    is_imu_connected = true;
+  }
 }
 
 void App::loop() {
@@ -171,10 +187,10 @@ 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);
   }
 }
@@ -299,4 +315,47 @@ void App::cmd_set_pid_tuning_gains_cb(int argc, char** argv) {
   Serial.println("OK");
 }
 
+void App::cmd_get_is_imu_connected_cb(int, char**) { Serial.println(is_imu_connected); }
+
+void App::cmd_read_encoders_and_imu_cb(int, char**) {
+  Serial.print(left_encoder_.read());
+  Serial.print(" ");
+  Serial.print(right_encoder_.read());
+  Serial.print(" ");
+
+  // Retrieve absolute orientation (quaternion). See
+  // https://learn.adafruit.com/adafruit-bno055-absolute-orientation-sensor/overview for further
+  // information.
+  imu::Quaternion orientation = bno055_imu_.getQuat();
+  Serial.print(orientation.x(), 4);
+  Serial.print(" ");
+  Serial.print(orientation.y(), 4);
+  Serial.print(" ");
+  Serial.print(orientation.z(), 4);
+  Serial.print(" ");
+  Serial.print(orientation.w(), 4);
+  Serial.print(" ");
+
+  // Retrive angular velocity (rad/s). See
+  // https://learn.adafruit.com/adafruit-bno055-absolute-orientation-sensor/overview for further
+  // information.
+  imu::Vector<3> angular_velocity = bno055_imu_.getVector(Adafruit_BNO055::VECTOR_GYROSCOPE);
+  Serial.print(angular_velocity.x());
+  Serial.print(" ");
+  Serial.print(angular_velocity.y());
+  Serial.print(" ");
+  Serial.print(angular_velocity.z());
+  Serial.print(" ");
+
+  // Retrive linear acceleration (m/s^2). See
+  // https://learn.adafruit.com/adafruit-bno055-absolute-orientation-sensor/overview for further
+  // information.
+  imu::Vector<3> linear_acceleration = bno055_imu_.getVector(Adafruit_BNO055::VECTOR_LINEARACCEL);
+  Serial.print(linear_acceleration.x());
+  Serial.print(" ");
+  Serial.print(linear_acceleration.y());
+  Serial.print(" ");
+  Serial.print(linear_acceleration.z());
+}
+
 }  // namespace andino

andino_firmware/src/app.h

@@ -29,6 +29,8 @@
 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #pragma once
 
+#include <Adafruit_BNO055.h>
+
 #include "digital_out_arduino.h"
 #include "encoder.h"
 #include "interrupt_in_arduino.h"
@@ -83,6 +85,12 @@ class App {
   /// Callback method for the `Commands::kSetPidsTuningGains` command.
   static void cmd_set_pid_tuning_gains_cb(int argc, char** argv);
 
+  /// Callback method for the `Commands::kGetIsImuConnected` command.
+  static void cmd_get_is_imu_connected_cb(int argc, char** argv);
+
+  /// Callback method for the `Commands::kReadEncodersAndImu` command.
+  static void cmd_read_encoders_and_imu_cb(int argc, char** argv);
+
   /// Serial stream.
   static SerialStreamArduino serial_stream_;
 
@@ -115,11 +123,17 @@ class App {
   static Pid left_pid_controller_;
   static Pid right_pid_controller_;
 
+  /// Adafruit BNO055 IMU sensor.
+  static Adafruit_BNO055 bno055_imu_;
+
   /// Tracks the last time the PID computation was made.
   static unsigned long last_pid_computation_;
 
   /// Tracks the last time a `Commands::kSetMotorsSpeed` command was received.
   static unsigned long last_set_motors_speed_cmd_;
+
+  /// Tracks whether there is an IMU sensor connected.
+  static bool is_imu_connected;
 };
 
 }  // namespace andino

andino_firmware/src/commands.h

@@ -82,6 +82,10 @@ struct Commands {
   static constexpr const char* kSetMotorsPwm{"o"};
   /// @brief Sets the PIDs tuning gains [format: "kp:kd:ki:ko"].
   static constexpr const char* kSetPidsTuningGains{"u"};
+  /// @brief Gets whether there is an IMU sensor connected.
+  static constexpr const char* kGetIsImuConnected{"h"};
+  /// @brief Reads the encoders tick count values and IMU sensor data.
+  static constexpr const char* kReadEncodersAndImu{"i"};
 };
 
 }  // namespace andino

andino_firmware/src/hw.h

@@ -38,10 +38,10 @@ struct Hw {
   /// @brief Left encoder channel B pin. Connected to PD3 (digital pin 3).
   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};
-  /// @brief Right encoder channel B pin. Connected to PC5 (digital pin 19, analog pin A5).
-  static constexpr int kRightEncoderChannelBGpioPin{19};
+  /// @brief Right encoder channel A pin. Connected to PC2 (digital pin 16, analog pin A2).
+  static constexpr int kRightEncoderChannelAGpioPin{16};
+  /// @brief Right encoder channel B pin. Connected to PC3 (digital pin 17, analog pin A3).
+  static constexpr int kRightEncoderChannelBGpioPin{17};
 
   /// @brief Left motor driver backward pin. Connected to PD6 (digital pin 6).
   static constexpr int kLeftMotorBackwardGpioPin{6};
@@ -60,6 +60,11 @@ struct Hw {
   /// @note The enable input of the L298N motor driver may be directly jumped to 5V if the board has
   /// a jumper to do so.
   static constexpr int kRightMotorEnableGpioPin{12};
+
+  /// @brief IMU sensor I2C SCL pin. Connected to PC5 (digital pin 19, analog pin A5).
+  static constexpr int kImuI2cSclPin{19};
+  /// @brief IMU sensor I2C SDA pin. Connected to PC4 (digital pin 18, analog pin A4).
+  static constexpr int kImuI2cSdaPin{18};
 };
 
 }  // namespace andino