Changes to add IMU to andino firmware

Signed-off-by: Gonzalo de Pedro <[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,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

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"
@@ -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

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].

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};