Publish files

software/cube-controller/cube-controller.ino

@@ -0,0 +1,244 @@
+// Toggle over-the-air (WiFi) programming functionality (0 = disabled, 1 = enabled)
+#define OTA 1
+
+// Toggle between use of nonlinear or linear controller (0 = linear, 1 = nonlinear)
+#define USE_NONLINEAR_CONTROLLER 1
+
+// TickTwo provides ESP32-compatible timers
+#include <TickTwo.h>
+
+#if OTA
+  // Libraries related to wireless functionality
+  #include <WiFi.h>
+  #include <ESPmDNS.h>
+  #include <WiFiUdp.h>
+  #include <ArduinoOTA.h>
+#endif
+
+// Cube controller library
+#include "src/cube-controller.h"
+
+#if OTA
+  // WiFi settings
+  const char* wifi_ssid = "YOUR_SSID";
+  const char* wifi_password = "YOUR_PASSWORD";
+#endif
+
+// Instantiation of objects
+Motor motor1(M1_ENABLE, M1_CURRENT), motor2(M2_ENABLE, M2_CURRENT), motor3(M3_ENABLE, M3_CURRENT);
+WheelEstimator whe_est_1(M1_SPEED), whe_est_2(M2_SPEED), whe_est_3(M3_SPEED);
+AttitudeEstimator att_est(IMU_SDA, IMU_SCL);
+AttitudeWheelController cont(USE_NONLINEAR_CONTROLLER);
+AttitudeTrajectory att_tra;
+
+// Run cube controller at the frequency as specified in parameter file
+void controller();
+TickTwo timer(controller, dt_us, 0, MICROS_MICROS);
+
+// Status LED control: LED is either solid or blinking at 2.5 Hz
+void control_led();
+TickTwo timer_led(control_led, 200, 0, MILLIS);
+
+#if OTA
+  // Handle OTA flashing requests every 5 seconds
+  void handle_ota();
+  TickTwo timer_ota(handle_ota, 5000, 0, MILLIS);
+#endif
+
+// Quaternion and angle error
+float qe0, qe1, qe2, qe3;
+float phi;
+float phi_lim = phi_min;
+
+// Trajectory initialization flag
+bool flag_tra = false;
+
+// LED status when blinking
+bool led_status = false;
+
+// Security flags
+bool flag_arm = false;
+bool flag_terminate = false;
+
+// Torques
+float tau_1 = 0, tau_2 = 0, tau_3 = 0;
+
+void setup() {
+  // Open serial connection
+  Serial.begin(921600);
+  Serial.println("This is the ESP32 cube controller.");
+
+  #if OTA
+    // Set up WiFi connection
+    WiFi.mode(WIFI_STA);
+    WiFi.begin(wifi_ssid, wifi_password);
+
+    unsigned int nAttempts = 0;
+    while (WiFi.waitForConnectResult() != WL_CONNECTED && nAttempts < 3) {
+      delay(1000);
+      nAttempts++;
+    }
+
+    // If WiFi connection failed, indicate this using six yellow flashes
+    if(WiFi.waitForConnectResult() != WL_CONNECTED) {
+      for(int i = 0; i < 5; i++) {
+        neopixelWrite(RGB_BUILTIN, 255, 255, 0);
+        delay(250);
+        neopixelWrite(RGB_BUILTIN, 0, 0, 0);
+        delay(250);
+      }
+    }
+
+    // Set hostname
+    ArduinoOTA.setHostname("Cube ESP32");
+
+    // Print WiFi status
+    Serial.print("Connected to WiFi (local IP is ");
+    Serial.print(WiFi.localIP());
+    Serial.println(").");
+
+    // Set up OTA flashing using ArduinoOTA
+    ArduinoOTA.begin();
+  #endif
+
+  // Delay for one second to allow Maxon ESCON drivers to fully initialize
+  delay(1000);
+
+  // Motor setup (also spins each motor in positive direction very briefly)
+  motor1.init();
+  motor2.init();
+  motor3.init();
+
+  // Wheel estimator initialization (also calibrates the hall sensor for each wheel)
+  whe_est_1.init();
+  whe_est_2.init();
+  whe_est_3.init();
+
+  // Attitude estimator initialization (also calibrates the gyroscope)
+  att_est.init();
+
+  // Start controller and LED timers
+  timer.start();
+  timer_led.start();
+
+  #if OTA
+    // OTA handler timer
+    timer_ota.start();
+  #endif
+}
+
+void loop() {
+  // Update controller timer
+  timer.update();
+  timer_led.update();
+
+  #if OTA
+    // Handle OTA updates
+    timer_ota.update();
+  #endif
+}
+
+#if OTA
+  void handle_ota(){
+    ArduinoOTA.handle();
+  }
+#endif
+
+void controller() {
+  // Estimate wheel velocities
+  whe_est_1.estimate(tau_1);
+  whe_est_2.estimate(tau_2);
+  whe_est_3.estimate(tau_3);
+
+  // Estimate cube attitude
+  att_est.estimate();
+
+  // Calculate rotation quaternion error
+  qe0 = att_est.q0 * att_tra.qr0 + att_est.q1 * att_tra.qr1 + att_est.q2 * att_tra.qr2 + att_est.q3 * att_tra.qr3;
+  qe1 = att_est.q0 * att_tra.qr1 - att_est.q1 * att_tra.qr0 - att_est.q2 * att_tra.qr3 + att_est.q3 * att_tra.qr2;
+  qe2 = att_est.q0 * att_tra.qr2 - att_est.q2 * att_tra.qr0 + att_est.q1 * att_tra.qr3 - att_est.q3 * att_tra.qr1;
+  qe3 = att_est.q0 * att_tra.qr3 - att_est.q1 * att_tra.qr2 + att_est.q2 * att_tra.qr1 - att_est.q3 * att_tra.qr0;
+
+  // Normalize rotation quaternion error (real part only since we don't need the rest)
+  qe0 /= sqrt(qe0 * qe0 + qe1 * qe1 + qe2 * qe2 + qe3 * qe3);
+
+  // Calculate error angle
+  phi = 2.0 * acos(qe0);
+
+  // Controller enable and disable logic. The controller only activates once its orientation is such that the error with
+  // respect to the desired orientation is smaller than phi_min. As soon as this is reached, the error tolerance phi_lim
+  // is increased to phi_max. If this error is exceeded, the controller is disabled until the chip is reset.
+  if(abs(phi) <= phi_lim && !flag_terminate) {
+    // Controller is active
+    flag_arm = true;
+
+    // Increase error range to the maximum limit
+    phi_lim = phi_max;
+
+    // Generate trajectory
+    // Comment out the generate() method if the cube is balancing on a side instead of a corner
+    if(!flag_tra) {
+      flag_tra = true;
+      att_tra.init();
+    }
+    att_tra.generate();
+
+    // Controller calculates motor torques based on cube and wheel states
+    cont.control(att_tra.qr0, att_tra.qr1, att_tra.qr2, att_tra.qr3, att_est.q0, att_est.q1, att_est.q2, att_est.q3,
+      att_tra.omega_r_x, att_tra.omega_r_y, att_tra.omega_r_z, att_est.omega_x, att_est.omega_y, att_est.omega_z, 
+      att_tra.alpha_r_x, att_tra.alpha_r_y, att_tra.alpha_r_z, whe_est_1.theta_w, whe_est_2.theta_w, 
+      whe_est_3.theta_w, whe_est_1.omega_w, whe_est_2.omega_w, whe_est_3.omega_w);
+
+    // Get motor torques from controller
+    tau_1 = cont.tau_1;
+    tau_2 = cont.tau_2;
+    tau_3 = cont.tau_3;
+  } else {
+    // Outside safe limits: reset motor torques to zero
+    tau_1 = 0.0;
+    tau_2 = 0.0;
+    tau_3 = 0.0;
+
+    // Disarm mechanism
+    if(flag_arm) {
+      flag_arm = false;
+      flag_terminate = true;
+    }
+  }
+
+  // Apply torques to motors
+  // When balancing on the x side, comment out motor 2 and 3. When balancing on the y side, comment out 1 and 3.
+  motor1.set_torque(tau_1);
+  motor2.set_torque(tau_2);
+  motor3.set_torque(tau_3);
+}
+
+void control_led() {
+  if(flag_arm) {
+    if(abs(phi) <= phi_lim - 10 * pi / 180) {
+      // Cube is not close to error limit: solid green LED
+      neopixelWrite(RGB_BUILTIN, 0, 255, 0);
+    } else {
+      // Cube is close to error limit: alternate between red and green. Cube should be manually put to rest.
+      if(led_status) {
+        neopixelWrite(RGB_BUILTIN, 255, 0, 0);
+      } else {
+        neopixelWrite(RGB_BUILTIN, 0, 255, 0);
+      }
+      led_status = !led_status;
+    }
+  } else {
+    if(flag_tra) {
+      // A trajectory was already generated so the cube must now be halted due to a limit error
+      if(led_status) {
+        neopixelWrite(RGB_BUILTIN, 0, 0, 0);
+      } else {
+        neopixelWrite(RGB_BUILTIN, 255, 0, 0);
+      }
+      led_status = !led_status;
+    } else {
+      // Show a solid red LED to indicate readiness to start
+      neopixelWrite(RGB_BUILTIN, 255, 0, 0);
+    }
+  }
+}

software/cube-controller/src/classes/controller_attitude_wheel.cpp

@@ -0,0 +1,169 @@
+#include "controller_attitude_wheel.h"
+
+// Constructor
+AttitudeWheelController::AttitudeWheelController(bool useNonlinear) : useNonlinear(useNonlinear) {
+    // Set initial quaternion error
+    qe0 = 0.0;
+    qe1 = 0.0;
+    qe2 = 0.0;
+    qe3 = 0.0;
+
+    // Set initial torque
+    tau_1 = 0.0;
+    tau_2 = 0.0;
+    tau_3 = 0.0;
+
+    // Set initial linearized input
+    u_1 = 0.0;
+    u_2 = 0.0;
+    u_3 = 0.0;
+}
+
+// Control step
+void AttitudeWheelController::control(float qr0, float qr1, float qr2, float qr3, float q0, float q1, float q2, 
+        float q3, float omega_r_x, float omega_r_y, float omega_r_z, float omega_x, float omega_y, float omega_z, 
+        float alpha_r_x, float alpha_r_y, float alpha_r_z, float theta_1, float theta_2, float theta_3, float omega_1,
+        float omega_2, float omega_3) {
+    if(useNonlinear) {
+        state_regulator(qr0, qr1, qr2, qr3, q0, q1, q2, q3, omega_r_x, omega_r_y, omega_r_z, omega_x, omega_y, omega_z,
+            alpha_r_x, alpha_r_y, alpha_r_z, theta_1, theta_2, theta_3, omega_1, omega_2, omega_3);
+        feedback_linearization(q0, q1, q2, q3, omega_x, omega_y, omega_z, omega_1, omega_2, omega_3);
+    } else {
+        linear_regulator(qr0, qr1, qr2, qr3, q0, q1, q2, q3, omega_r_x, omega_r_y, omega_r_z, omega_x, omega_y, omega_z, 
+            alpha_r_x, alpha_r_y, alpha_r_z, theta_1, theta_2, theta_3, omega_1, omega_2, omega_3);
+    }
+}
+
+// State regulator step
+void AttitudeWheelController::state_regulator(float qr0, float qr1, float qr2, float qr3, float q0, float q1, float q2,
+        float q3, float omega_r_x, float omega_r_y, float omega_r_z, float omega_x, float omega_y, float omega_z, 
+        float alpha_r_x, float alpha_r_y, float alpha_r_z, float theta_1, float theta_2, float theta_3, float omega_1,
+        float omega_2, float omega_3) {
+    // Calculate rotation quaternion error
+    qe0 = q0 * qr0 + q1 * qr1 + q2 * qr2 + q3 * qr3;
+    qe1 = q0 * qr1 - q1 * qr0 - q2 * qr3 + q3 * qr2;
+    qe2 = q0 * qr2 - q2 * qr0 + q1 * qr3 - q3 * qr1;
+    qe3 = q0 * qr3 - q1 * qr2 + q2 * qr1 - q3 * qr0;
+
+    // Normalize rotation quaternion error
+    float qe_norm = sqrt(qe0 * qe0 + qe1 * qe1 + qe2 * qe2 + qe3 * qe3);
+    qe0 /= qe_norm;
+    qe1 /= qe_norm;
+    qe2 /= qe_norm;
+    qe3 /= qe_norm;
+
+    // Auxiliary variables to avoid computing the same term multiple times
+    float qe0qe1 = qe0 * qe1;
+    float qe0qe2 = qe0 * qe2;
+    float qe0qe3 = qe0 * qe3;
+    float qe1qe1 = qe1 * qe1;
+    float qe1qe2 = qe1 * qe2;
+    float qe1qe3 = qe1 * qe3;
+    float qe2qe2 = qe2 * qe2;
+    float qe2qe3 = qe2 * qe3;
+    float qe3qe3 = qe3 * qe3;
+
+    // Calculate angular velocity error
+    float omega_e_x = omega_r_x + 2.0 * (omega_r_x * (-qe2qe2 - qe3qe3) + omega_r_y * (-qe0qe3 + qe1qe2) + omega_r_z * 
+        ( qe0qe2 + qe1qe3)) - omega_x;
+    float omega_e_y = omega_r_y + 2.0 * (omega_r_x * ( qe0qe3 + qe1qe2) + omega_r_y * (-qe1qe1 - qe3qe3) + omega_r_z * 
+        (-qe0qe1 + qe2qe3)) - omega_y;
+    float omega_e_z = omega_r_z + 2.0 * (omega_r_x * (-qe0qe2 + qe1qe3) + omega_r_y * ( qe0qe1 + qe2qe3) + omega_r_z * 
+        (-qe1qe1 - qe2qe2)) - omega_z;
+
+    // Auxiliary variable to avoid computing the same term multiple times
+    float _2_kp_omega_e_omega_e_4 = 2.0 * (kp - (omega_e_x * omega_e_x + omega_e_y * omega_e_y + omega_e_z * omega_e_z)
+        / 4.0);
+
+    // Attitude feedback
+    u_1 = _2_kp_omega_e_omega_e_4 * qe1 / qe0 + kd * omega_e_x;
+    u_2 = _2_kp_omega_e_omega_e_4 * qe2 / qe0 + kd * omega_e_y;
+    u_3 = _2_kp_omega_e_omega_e_4 * qe3 / qe0 + kd * omega_e_z;
+
+    // Attitude feedforward
+    u_1 += omega_e_y * omega_z - omega_e_z * omega_y + alpha_r_x + 2.0 * (alpha_r_x *(-qe2qe2 - qe3qe3) + alpha_r_y * 
+        (-qe0qe3 + qe1qe2) + alpha_r_z * ( qe0qe2 + qe1qe3));
+    u_2 += omega_e_z * omega_x - omega_e_x * omega_z + alpha_r_y + 2.0 * (alpha_r_x *( qe0qe3 + qe1qe2) + alpha_r_y * 
+        (-qe1qe1 - qe3qe3) + alpha_r_z * (-qe0qe1 + qe2qe3));
+    u_3 += omega_e_x * omega_y - omega_e_y * omega_x + alpha_r_z + 2.0 * (alpha_r_x *(-qe0qe2 + qe1qe3) + alpha_r_y * 
+        ( qe0qe1 + qe2qe3) + alpha_r_z * (-qe1qe1 - qe2qe2));
+
+    // Wheel feedback
+    u_1 += -kpw * theta_1 - kdw * omega_1;
+    u_2 += -kpw * theta_2 - kdw * omega_2;
+    u_3 += -kpw * theta_3 - kdw * omega_3;
+}
+
+// Feedback linearization step
+void AttitudeWheelController::feedback_linearization(float q0, float q1, float q2, float q3, float omega_x, 
+        float omega_y, float omega_z, float omega_1, float omega_2, float omega_3) {
+    // Calculate friction torque
+    float sign_1 = (0.0 < omega_1) - (omega_1 < 0.0);
+    float sign_2 = (0.0 < omega_2) - (omega_2 < 0.0);
+    float sign_3 = (0.0 < omega_3) - (omega_3 < 0.0);
+    tau_f_1 = sign_1 * (tau_c + bw * abs(omega_1));
+    tau_f_2 = sign_2 * (tau_c + bw * abs(omega_2));
+    tau_f_3 = sign_3 * (tau_c + bw * abs(omega_3));
+
+    // Auxiliary variable to avoid computing the same term multiple times
+    float omega_x_omega_y_omega_z = omega_x + omega_y + omega_z;
+
+    // Feedback linearization
+    tau_1 = -I_c_xy_bar * (omega_y - omega_z) * omega_x_omega_y_omega_z - I_w_xx * (omega_3 * omega_y - omega_2 * 
+        omega_z) + m_c_bar_g_l * (0.5 - q0 * q0 + q0 * q1 - q3 * q3 + q2 * q3) + tau_f_1 - I_c_xx_bar * u_1 - 
+        I_c_xy_bar * (u_2 + u_3);
+    tau_2 = -I_c_xy_bar * (omega_z - omega_x) * omega_x_omega_y_omega_z - I_w_xx * (omega_1 * omega_z - omega_3 * 
+        omega_x) + m_c_bar_g_l * (0.5 + q0 * q2 - q1 * q1 - q1 * q3 - q2 * q2) + tau_f_2 - I_c_xx_bar * u_2 - 
+        I_c_xy_bar * (u_1 + u_3);
+    tau_3 = -I_c_xy_bar * (omega_x - omega_y) * omega_x_omega_y_omega_z - I_w_xx * (omega_2 * omega_x - omega_1 * 
+        omega_y) - m_c_bar_g_l * (      q0 * q1 + q0 * q2 - q1 * q3 + q2 * q3) + tau_f_3 - I_c_xx_bar * u_3 - 
+        I_c_xy_bar * (u_1 + u_2);
+}
+
+void AttitudeWheelController::linear_regulator(float qr0, float qr1, float qr2, float qr3, float q0, float q1, 
+    float q2, float q3, float omega_r_x, float omega_r_y, float omega_r_z, float omega_x, float omega_y, float omega_z, 
+        float alpha_r_x, float alpha_r_y, float alpha_r_z, float theta_1, float theta_2, float theta_3, float omega_1, 
+        float omega_2, float omega_3) {
+    // Calculate rotation quaternion error
+    qe0 = q0 * qr0 + q1 * qr1 + q2 * qr2 + q3 * qr3;
+    qe1 = q0 * qr1 - q1 * qr0 - q2 * qr3 + q3 * qr2;
+    qe2 = q0 * qr2 - q2 * qr0 + q1 * qr3 - q3 * qr1;
+    qe3 = q0 * qr3 - q1 * qr2 + q2 * qr1 - q3 * qr0;
+
+    // Normalize rotation quaternion error
+    float qe_norm = sqrt(qe0 * qe0 + qe1 * qe1 + qe2 * qe2 + qe3 * qe3);
+    qe0 /= qe_norm;
+    qe1 /= qe_norm;
+    qe2 /= qe_norm;
+    qe3 /= qe_norm;
+
+    // Calculate angle error
+    float theta_e_x = 2 * asin(qe1);
+    float theta_e_y = 2 * asin(qe2);
+    float theta_e_z = 2 * asin(qe3);
+
+    // Calculate angular velocity error
+    float omega_e_x = omega_r_x - omega_x;
+    float omega_e_y = omega_r_y - omega_y;
+    float omega_e_z = omega_r_z - omega_z;
+
+    // Attitude feedback (basically a PD controller)
+    u_1 = kp * theta_e_x + kd * omega_e_x;
+    u_2 = kp * theta_e_y + kd * omega_e_y;
+    u_3 = kp * theta_e_z + kd * omega_e_z;
+
+    // Attitude feedforward
+    u_1 += alpha_r_x;
+    u_2 += alpha_r_y;
+    u_3 += alpha_r_z;
+
+    // Wheel feedback
+    u_1 += -kpw * theta_1 - kdw * omega_1;
+    u_2 += -kpw * theta_2 - kdw * omega_2;
+    u_3 += -kpw * theta_3 - kdw * omega_3;
+
+    // Convert inputs into torques
+    tau_1 = -I_c_xx_bar * u_1 - I_c_xy_bar * (u_2 + u_3);
+    tau_2 = -I_c_xx_bar * u_2 - I_c_xy_bar * (u_1 + u_3);
+    tau_3 = -I_c_xx_bar * u_3 - I_c_xy_bar * (u_1 + u_2);
+}

software/cube-controller/src/classes/controller_attitude_wheel.h

@@ -0,0 +1,45 @@
+#ifndef controller_attitude_wheel_h
+#define controller_attitude_wheel_h
+
+#include "Arduino.h"
+
+#include "../definitions/parameters.h"
+
+// Attitude and wheel controller class
+class AttitudeWheelController {
+    public:
+        // Constructor
+        AttitudeWheelController(bool useNonlinear);
+        // Control step
+        void control(float qr0, float qr1, float qr2, float qr3, float q0, float q1, float q2, float q3, 
+            float omega_r_x, float omega_r_y, float omega_r_z, float omega_x, float omega_y, float omega_z, 
+            float alpha_r_x, float alpha_r_y, float alpha_r_z, float theta_1, float theta_2, float theta_3, 
+            float omega_1, float omega_2, float omega_3);
+        // Quaternion error
+        float qe0, qe1, qe2, qe3;
+        // Torque (Nm)
+        float tau_1, tau_2, tau_3;
+        // Friction torques
+        float tau_f_1, tau_f_2, tau_f_3;
+
+    private:
+        // State regulator step
+        void state_regulator(float qr0, float qr1, float qr2, float qr3, float q0, float q1, float q2, float q3, 
+            float omega_r_x, float omega_r_y, float omega_r_z, float omega_x, float omega_y, float omega_z, 
+            float alpha_r_x, float alpha_r_y, float alpha_r_z, float theta_1, float theta_2, float theta_3, 
+            float omega_1, float omega_2, float omega_3);
+        // Feedback linearization step
+        void feedback_linearization(float q0, float q1, float q2, float q3, float omega_x, float omega_y, 
+            float omega_z, float omega_1, float omega_2, float omega_3);
+        // Linear regulator
+        void linear_regulator(float qr0, float qr1, float qr2, float qr3, float q0, float q1, float q2, float q3, 
+            float omega_r_x, float omega_r_y, float omega_r_z, float omega_x, float omega_y, float omega_z, 
+            float alpha_r_x, float alpha_r_y, float alpha_r_z, float theta_1, float theta_2, float theta_3, 
+            float omega_1, float omega_2, float omega_3);
+        // Linearized input
+        float u_1, u_2, u_3;
+        // Controller type
+        bool useNonlinear;
+};
+
+#endif

software/cube-controller/src/classes/estimator_attitude.cpp

@@ -0,0 +1,152 @@
+#include "estimator_attitude.h"
+
+// Constructor
+AttitudeEstimator::AttitudeEstimator(int pin_sda, int pin_scl) : imu(pin_sda, pin_scl) {
+    // Set initial rotation quaternion
+    q0 = 1.0;
+    q1 = 0.0;
+    q2 = 0.0;
+    q3 = 0.0;
+
+    // Set initial angular velocity
+    omega_x = 0.0;
+    omega_y = 0.0;
+    omega_z = 0.0;
+
+    // Set initial angular velocity bias
+    b_omega_x = 0.0;
+    b_omega_y = 0.0;
+    b_omega_z = 0.0;
+}
+
+// Initializer
+void AttitudeEstimator::init() {
+    // Initialize IMU sensor object
+    imu.init();
+
+    // Angular velocity bias calibration
+    calibrate();
+}
+
+// Angular velocity bias calibration 
+void AttitudeEstimator::calibrate() {
+    // Calculate angular velocity bias by averaging one thousand samples or 5 seconds worth of gyroscope data
+    // This is done on top of the one-time calibration that was done manually in advance
+    for(int i = 0; i < 1000; i++) {
+        // Read sensor values
+        imu.read();
+
+        // Add 1/f-th part of the current reading to the bias
+        b_omega_x += imu.gx / 1000;
+        b_omega_y += imu.gy / 1000;
+        b_omega_z += imu.gz / 1000;
+
+        delay(5);
+    }
+}
+
+// Estimate step
+void AttitudeEstimator::estimate() {
+    // Read values from IMU
+    imu.read();
+
+    // Get angular velocity from IMU gyroscope data and apply second offset correction
+    omega_x = imu.gx - b_omega_x;
+    omega_y = imu.gy - b_omega_y;
+    omega_z = imu.gz - b_omega_z;
+
+    // Predict step
+    predict(omega_x, omega_y, omega_z);
+
+    // Get linear acceleration from IMU accelerometer data
+    float ax = imu.ax;
+    float ay = imu.ay;
+    float az = imu.az;
+
+    // Normalize linear acceleration
+    float a_norm = sqrt(ax * ax + ay * ay + az * az);
+    ax /= a_norm;
+    ay /= a_norm;
+    az /= a_norm;
+
+    // Correct step
+    correct(ax, ay, az);
+
+    // Normalize rotation quaternion
+    float q_norm = sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
+    q0 /= q_norm;
+    q1 /= q_norm;
+    q2 /= q_norm;
+    q3 /= q_norm;
+}
+
+// Estimate step
+void AttitudeEstimator::predict(float omega_x, float omega_y, float omega_z) {   
+    // Predict rotation quaternion time derivative
+    float q0_dot = 0.5 * (-q1 * omega_x - q2 * omega_y - q3 * omega_z);
+    float q1_dot = 0.5 * ( q0 * omega_x - q3 * omega_y + q2 * omega_z);
+    float q2_dot = 0.5 * ( q3 * omega_x + q0 * omega_y - q1 * omega_z);
+    float q3_dot = 0.5 * (-q2 * omega_x + q0 * omega_z + q1 * omega_y);
+
+    // Predict rotation quaternion
+    q0 += q0_dot * dt;
+    q1 += q1_dot * dt;
+    q2 += q2_dot * dt;
+    q3 += q3_dot * dt;
+}
+
+// Correct step
+void AttitudeEstimator::correct(float ax, float ay, float az) {    
+    // Calculate rotation quaternion measurement
+    float qm0 =  ax * q2 - ay * q1 - az * q0;
+    float qm1 = -ax * q3 - ay * q0 + az * q1;
+    float qm2 =  ax * q0 - ay * q3 + az * q2;
+    float qm3 = -ax * q1 - ay * q2 - az * q3;
+
+    // Calculate rotation quaternion error
+    float qe0 = q0 * qm0 + q1 * qm1 + q2 * qm2 + q3 * qm3;
+    float qe1 = q0 * qm1 - q1 * qm0 - q2 * qm3 + q3 * qm2;
+    float qe2 = q0 * qm2 + q1 * qm3 - q2 * qm0 - q3 * qm1;
+    float qe3 = q0 * qm3 - q1 * qm2 + q2 * qm1 - q3 * qm0;
+
+    // Calculate rotation Gibbs-vector error
+    float se1 = qe1 / qe0;
+    float se2 = qe2 / qe0;
+    float se3 = qe3 / qe0;
+
+    // Calculate rotation quaternion error time derivative
+    float qe0_dot = -q1 * se1 - q2 * se2 - q3 * se3;
+    float qe1_dot =  q0 * se1 - q3 * se2 + q2 * se3;
+    float qe2_dot =  q3 * se1 + q0 * se2 - q1 * se3;
+    float qe3_dot = -q2 * se1 + q1 * se2 + q0 * se3;
+
+    // Correct rotation quaternion
+    q0 += lds * dt * qe0_dot;
+    q1 += lds * dt * qe1_dot;
+    q2 += lds * dt * qe2_dot;
+    q3 += lds * dt * qe3_dot;
+}
+
+float AttitudeEstimator::ax() {
+    return imu.ax;
+}
+
+float AttitudeEstimator::ay() {
+    return imu.ay;
+}
+
+float AttitudeEstimator::az() {
+    return imu.az;
+}
+
+float AttitudeEstimator::gx() {
+    return imu.gx;
+}
+
+float AttitudeEstimator::gy() {
+    return imu.gy;
+}
+
+float AttitudeEstimator::gz() {
+    return imu.gz;
+}

software/cube-controller/src/classes/estimator_attitude.h

@@ -0,0 +1,38 @@
+#ifndef estimator_attitude_h
+#define estimator_attitude_h
+
+#include "Arduino.h"
+
+#include "../definitions/parameters.h"
+#include "../classes/icm20948.h"
+
+// Attitude estimator class
+class AttitudeEstimator {
+    public:
+        // Constructor
+        AttitudeEstimator(int pin_sda, int pin_scl);
+        // Initializer
+        void init();
+        // Estimate step
+        void estimate();
+        // Rotation quaternion estimations
+        float q0, q1, q2, q3;
+        // Angular velocity (rad/s) estimations
+        float omega_x, omega_y, omega_z;
+        // Get acceleration and gyroscope values directly from IMU
+        float ax(), ay(), az(), gx(), gy(), gz();
+
+    private:
+        // IMU sensor object
+        ICM20948 imu;
+        // Angular velocity bias calibration
+        void calibrate();
+        // Predict step
+        void predict(float omega_x, float omega_y, float omega_z);
+        // Correct step
+        void correct(float ax, float ay, float az);
+        // Angular velocity (rad/s) bias
+        float b_omega_x, b_omega_y, b_omega_z;
+};
+
+#endif

software/cube-controller/src/classes/estimator_wheel.cpp

@@ -0,0 +1,51 @@
+#include "estimator_wheel.h"
+
+// Constructor
+WheelEstimator::WheelEstimator(int pin_speed) : hall(pin_speed) {
+    // Set initial angular displacement and angular velocity
+    theta_w = 0.0;
+    omega_w = 0.0;
+}
+
+// Initializer
+void WheelEstimator::init() {
+    // Initialize and calibrate hall sensor
+    hall.init();
+}
+
+// Estimate step
+void WheelEstimator::estimate(float tau) {
+    // Predict step
+    predict(tau);
+
+    // Get angular velocity measurement from hall sensor
+    hall.read();
+
+    // Correct step
+    correct(hall.omega);
+}
+
+// Predict step
+void WheelEstimator::predict(float tau) {
+    // Calculate friction torque
+    float sign = (0.0 < omega_w) - (omega_w < 0.0);
+    float tau_f = sign * (tau_c + bw * abs(omega_w));
+
+    // Calculate angular acceleration
+    omega_w_dot = (1.0 / I_w_xx) * (-tau_f + tau);
+
+    // Predict angular displacement and angular velocity
+    theta_w += omega_w * dt + omega_w_dot * dt * dt / 2.0;
+    omega_w += omega_w_dot * dt;
+}
+
+// Correct step
+void WheelEstimator::correct(float omega_w_m) {
+    // Correct angular velocity with measurement
+    omega_w += ldw * dt * (omega_w_m - omega_w);
+}
+
+float WheelEstimator::omega() {
+    // Get hall sensor reading
+    return hall.omega;
+}

software/cube-controller/src/classes/estimator_wheel.h

@@ -0,0 +1,32 @@
+#ifndef estimator_wheel_h
+#define estimator_wheel_h
+
+#include "Arduino.h"
+
+#include "../definitions/parameters.h"
+#include "../classes/hall.h"
+
+// Wheel estimator class
+class WheelEstimator {
+    public:
+        // Constructor
+        WheelEstimator(int pin_speed);
+        // Initializer
+        void init();
+        // Estimate step
+        void estimate(float tau = 0.0);
+        // Angular displacement (rad) and angular velocity (rad/s) estimations
+        float theta_w, omega_w, omega_w_dot;
+        // Get hall sensor reading directly from hall sensor
+        float omega();
+
+    private:
+        // Motor hall sensor object
+        Hall hall;
+        // Predict step
+        void predict(float tau);
+        // Correct step
+        void correct(float omega_w_m);
+};
+
+#endif

software/cube-controller/src/classes/hall.cpp

@@ -0,0 +1,50 @@
+#include "hall.h"
+
+// Class constructor
+Hall::Hall(int pin_speed) : pin_speed(pin_speed) {
+}
+
+void Hall::init() {
+    // Set analog read resolution
+    analogReadResolution(12);
+
+    // Indicate wheel calibration in progress using blue LED
+    neopixelWrite(RGB_BUILTIN, 0, 0, 255);
+    
+    // Logging
+    Serial.print("Starting hall sensor calibration (pin ");
+    Serial.print(pin_speed);
+    Serial.print(")... ");
+
+    // Calibrate reading by averaging 100 samples over one second
+    for(int i = 0; i < 100; i++) {
+        bias += ((float) analogReadMilliVolts(pin_speed) / 1000) / 100;
+        delay(10);
+    }
+
+    // Remove nominal voltage (wheel at rest) from bias calculation
+    bias -= 1.65;
+
+    // Flash LED and then disable it
+    digitalWrite(RGB_BUILTIN, LOW);
+    delay(250);
+    neopixelWrite(RGB_BUILTIN, 0, 0, 255);
+    delay(250);
+    digitalWrite(RGB_BUILTIN, LOW);
+
+    // Logging
+    Serial.print("Done. Offset is ");
+    Serial.print(bias);
+    Serial.println(" V.");
+}
+
+// Read angular velocity
+void Hall::read() {
+    // Read voltage using ADC (with 12-bit resolution)
+    float voltage = (float) analogReadMilliVolts(pin_speed) / 1000;
+
+    // Convert voltage reading to speed in rad/s. Speed is -6500 rpm at 0.3 V and 6500 rpm at 3.0 V. The range between
+    // 0 and 0.3 V and 3.0 V and 3.3 V is not used due to slight nonlinearities in that domain. The sign of the output
+    // is changed to align with the coordinate system and rotation direction definitions.
+    omega = -1 * (-6500 + (voltage - bias - 0.3) * 13000 / 2.7) *  pi / 30;
+}

software/cube-controller/src/classes/hall.h

@@ -0,0 +1,27 @@
+#ifndef hall_h
+#define hall_h
+
+#include "Arduino.h"
+
+#include "../definitions/parameters.h"
+
+// Hall class
+class Hall {
+    public:
+        // Class constructor
+        Hall(int pin_speed);
+        // Initialize and calibrate
+        void init();
+        // Read angular velocity
+        void read();
+        // Angular velocity (rad/s)
+        float omega;
+
+    private:
+        // Input pin
+        int pin_speed;
+        // Bias (calibration offset)
+        float bias;
+};
+
+#endif

software/cube-controller/src/classes/icm20948.cpp

@@ -0,0 +1,66 @@
+#include "icm20948.h"
+
+// Class constructor 
+ICM20948::ICM20948(int pin_sda, int pin_scl) : pin_sda(pin_sda), pin_scl(pin_scl), wirePort(0) {
+}
+
+// Initialize sensor
+void ICM20948::init() {
+    // Set up i2c communication at 400 kHz
+    wirePort.begin(pin_sda, pin_scl, 400000);
+
+    // Setup IMU
+    imu = ICM20948_WE(&wirePort);
+
+    // Verify that sensor works correctly
+    if(imu.init()) {
+        // Sensor is responding correctly
+        Serial.println("ICM20948 is responding.");
+
+        for(int i = 0; i < 3; i++) {
+            digitalWrite(RGB_BUILTIN, HIGH);
+            delay(250);
+            digitalWrite(RGB_BUILTIN, LOW);
+            delay(250);
+        }
+    } else {
+        // Sensor is not responding
+        Serial.println("ICM20948 is not responding.");
+
+        for(int i = 0; i < 10; i++) {
+            neopixelWrite(RGB_BUILTIN, 255, 0, 0);
+            delay(100);
+            digitalWrite(RGB_BUILTIN, LOW);
+            delay(100);
+        }
+    }
+
+    // Set measurement ranges for accelerometer (2 m/s/s) and gyroscope (2000 dps)
+    imu.setAccRange(ICM20948_ACC_RANGE_2G);
+    imu.setGyrRange(ICM20948_GYRO_RANGE_2000);
+    
+    // Configure digital low-pass filter (DLPF)
+    imu.setAccDLPF(ICM20948_DLPF_6);
+    imu.setGyrDLPF(ICM20948_DLPF_6);
+    
+    // Disable divider to get maximum possible output data rate (approximately 1.1 kHz)
+    imu.setAccSampleRateDivider(0);
+    imu.setGyrSampleRateDivider(0);
+}
+
+// Read sensor data 
+void ICM20948::read() {
+    // Get values from sensor
+    imu.readSensor();
+    acc = imu.getAccRawValues();
+    gyr = imu.getGyrValues();
+
+    // Apply calibration offsets and gains and swap axes
+    ax = -(acc.x - b_acc_x) / f_acc_x;
+    ay = -(acc.z - b_acc_z) / f_acc_z;
+    az = -(acc.y - b_acc_y) / f_acc_y;
+
+    gx = -(gyr.x - b_gyr_x) * pi / 180;
+    gy = -(gyr.z - b_gyr_z) * pi / 180;
+    gz = -(gyr.y - b_gyr_y) * pi / 180;
+}

software/cube-controller/src/classes/icm20948.h

@@ -0,0 +1,35 @@
+#ifndef icm20948_h
+#define icm20948_h
+
+#include "Arduino.h"
+#include "Wire.h"
+#include "ICM20948_WE.h"
+
+#include "../definitions/parameters.h"
+
+// ICM20948 class
+class ICM20948 {
+    public:
+        // Class constructor
+        ICM20948(int pin_sda, int pin_scl);
+        // Initialize sensor
+        void init();
+        // Read sensor data
+        void read();
+        // Gyroscope data in x, y and z axis (rad/s)
+        float gx, gy, gz;
+        // Accelerometer data x, y and z axis (m/s^2)
+        float ax, ay, az;
+        
+    private:
+        // i2c bus
+        TwoWire wirePort;
+        // i2c pins
+        int pin_sda, pin_scl;
+        // IMU object
+        ICM20948_WE imu;
+        // Measurement stores
+        xyzFloat acc, gyr;
+};
+
+#endif

software/cube-controller/src/classes/motor.cpp

@@ -0,0 +1,72 @@
+#include "motor.h"
+
+// Class constructor
+Motor::Motor(int pin_enable, int pin_current) : pin_enable(pin_enable), pin_current(pin_current) {
+    // Configure enable pin as digital output
+    pinMode(pin_enable, OUTPUT);
+
+    // Map pin number to channel (these must be updated if mx_current in pins.h is changed...)
+    if(pin_current == 14) {
+        channel = 0;
+    } else if(pin_current == 16) {
+        channel = 1;
+    } else {
+        channel = 2;
+    }
+
+    // Configure current pin as 1 kHz 12-bit PWM output
+    ledcSetup(channel, 1000, 12);
+    ledcAttachPin(pin_current, channel);
+}
+
+void Motor::init() {
+    // Briefly set current to verify motor behaviour
+    set_current(0.5);
+    delay(250);
+    set_current(-0.4);
+    delay(250);
+    set_current(0);
+}
+
+void Motor::set_current(float ia) {
+    // Variables
+    bool enable;
+    float current;
+
+    // Reverse motor current because of definitions (positive corresponds to CCW rotation)
+    ia = -ia;
+
+    if (ia == 0.0) {
+        // If current is zero, disable motor and set corresponding PWM value
+        enable = false;
+        current = 0.5;   
+    } else {
+        // Enable motor
+        enable = true;
+
+        // Set PWM value according to current input. Upper and lower 10% are not used for reliability. ESCON drivers are
+        // configured accordingly.
+        if(ia > ia_max) {
+            // Saturation at upper limit
+            current = 0.9;
+        } else if (ia < -ia_max) {
+            // Saturation at lower limit
+            current = 0.1;
+        } else {
+            // Interpolation in valid range
+            current = 0.5 + ia * (0.8 / (2.0 * ia_max));
+        }
+    }
+
+    // Set enable pin
+    digitalWrite(pin_enable, enable);
+
+    // Map 0-1 PWM value to 12-bit range (0-4095) and write duty
+    uint32_t duty = current * 4095;
+    ledcWrite(channel, duty);
+}
+
+void Motor::set_torque(float tau) {
+    // Convert torque to current using motor torque constant Km
+    set_current(tau / Km);
+}

software/cube-controller/src/classes/motor.h

@@ -0,0 +1,27 @@
+#ifndef motor_h
+#define motor_h
+
+#include "Arduino.h"
+
+#include "../definitions/parameters.h"
+
+// Motor class
+class Motor {
+    public:
+        // Class constructor
+        Motor(int pin_enable, int pin_current);
+        // Initialize
+        void init();
+        // Set current (A)
+        void set_current(float ia);
+        // Set torque (Nm)
+        void set_torque(float tau);
+
+    private:
+        // PWM channel
+        int channel;
+        // Pins
+        int pin_enable, pin_current;
+};
+
+#endif

software/cube-controller/src/classes/trajectory_attitude.cpp

@@ -0,0 +1,160 @@
+#include "trajectory_attitude.h"
+
+// Constructor
+AttitudeTrajectory::AttitudeTrajectory() {
+    // Initial reference quaternion
+    qr0 = qu0;
+    qr1 = qu1;
+    qr2 = qu2;
+    qr3 = qu3;
+
+    // Reference angular velocities for cube
+    omega_r_x = 0.0;
+    omega_r_y = 0.0;
+    omega_r_z = 0.0;
+
+    // Reference angular accelerations for cube
+    alpha_r_x = 0.0;
+    alpha_r_y = 0.0;
+    alpha_r_z = 0.0;
+
+    // Algorithm flags
+    flag_rot1 = true;
+    flag_res1 = true;
+    flag_rot2 = true;
+    flag_res2 = true;
+
+    // Initialization of position and its derivatives
+    pos = 0.0;
+    vel = 0.0;
+    acc = 0.0;
+    jer = 0.0;
+    sna = 0.0;
+    cra = 0.0;
+}
+
+// Initializer
+void AttitudeTrajectory::init() {
+    // Start timer
+    timer = micros();
+}
+
+// Generate step
+void AttitudeTrajectory::generate() {
+    // Get current time in seconds since start of timer
+    float t = (micros() - timer) / 1e6;
+    
+    // Minimum jerk trajectory algorithm
+    if((t >= t_rest / 2.0) && flag_rot1) {
+        flag_rot1 = false;
+        jer =  jer_0;
+        sna = -sna_0;
+        cra =  cra_0;
+    }
+
+    if((t >= t_traj + t_rest / 2.0) && flag_res1) {
+        flag_res1 = false;
+        pos = pos_traj;
+        vel = 0.0;
+        acc = 0.0;
+        jer = 0.0;
+        sna = 0.0;
+        cra = 0.0;
+    }
+
+    if((t >= t_traj + 3.0 * t_rest / 2.0) && flag_rot2) {
+        flag_rot2 = false;
+        jer = -jer_0;
+        sna =  sna_0;
+        cra = -cra_0;
+    }
+
+    if((t >= 2.0 * t_traj + 3.0 * t_rest / 2.0) && flag_res2) {
+        flag_res2 = false;
+        pos = 0.0;
+        vel = 0.0;
+        acc = 0.0;
+        jer = 0.0;
+        sna = 0.0;
+        cra = 0.0;
+    }
+
+    if(t >= 2.0 * t_traj + 2.0 * t_rest) {
+        timer = micros();
+        flag_rot1 = true;
+        flag_res1 = true;
+        flag_rot2 = true;
+        flag_res2 = true;
+    }
+
+    pos += vel * dt + acc * pow(dt, 2) / 2.0 + jer * pow(dt, 3) / 6.0 + sna * pow(dt, 4) / 24.0 + cra * pow(dt, 5) / 120.0;
+    vel += acc * dt + jer * pow(dt, 2) / 2.0 + sna * pow(dt, 3) / 6.0 + cra * pow(dt, 4) / 24.0;
+    acc += jer * dt + sna * pow(dt, 2) / 2.0 + cra * pow(dt, 3) / 6.0;
+    jer += sna * dt + cra * pow(dt, 2) / 2.0;
+    sna += cra * dt;
+ 
+    // Euler angles and its time derivatives
+    float phi = pos;
+    float phi_dot = vel;
+    float phi_ddot = acc;
+    float theta = 0.0;
+    float theta_dot = 0.0;
+    float theta_ddot = 0.0;
+    float psi = 0.0;
+    float psi_dot = 0.0;
+    float psi_ddot = 0.0;
+
+    // Auxiliary varaibles to avoid double arithmetic
+    float sin_phi = sin(phi);
+    float cos_phi = cos(phi);
+    float sin_theta = sin(theta);
+    float cos_theta = cos(theta);
+    float sin_psi = sin(psi);
+    float cos_psi = cos(psi);
+    float sin_phi_2 = sin(phi / 2.0);
+    float cos_phi_2 = cos(phi / 2.0);
+    float sin_theta_2 = sin(theta / 2.0);
+    float cos_theta_2 = cos(theta / 2.0);
+    float sin_psi_2 = sin(psi / 2.0);
+    float cos_psi_2 = cos(psi / 2.0);
+
+    // Calculate orientation quaternion in terms of Euler angles
+    float q0 = cos_phi_2 * cos_psi_2 * cos_theta_2 - cos_theta_2 * sin_phi_2 * sin_psi_2;
+    float q1 = cos_psi_2 * sin_phi_2 * sin_theta_2 - cos_phi_2 * sin_psi_2 * sin_theta_2;
+    float q2 = cos_phi_2 * cos_psi_2 * sin_theta_2 + sin_phi_2 * sin_psi_2 * sin_theta_2;
+    float q3 = cos_phi_2 * cos_theta_2 * sin_psi_2 + cos_psi_2 * cos_theta_2 * sin_phi_2;
+
+    // Calculate angular velocity in terms of Euler angles and its derivatives
+    float omega_x = sin_phi * theta_dot - cos_phi * sin_theta * psi_dot;
+    float omega_y = cos_phi * theta_dot + sin_phi * sin_theta * psi_dot;
+    float omega_z = phi_dot + cos_theta * psi_dot;
+
+    // Calculate angular acceleration in terms of Euler angles and its time derivatives
+    float alpha_x = cos_phi * phi_dot * theta_dot + sin_phi * theta_ddot - sin_phi * phi_dot * sin_theta * psi_dot - 
+        cos_phi * cos_theta * theta_dot * psi_dot - cos_phi * sin_theta * psi_ddot;
+    float alpha_y = -sin_phi * phi_dot * theta_dot + cos_phi * theta_ddot + cos_phi * phi_dot * sin_theta * psi_dot + 
+        sin_phi * cos_theta * theta_dot * psi_dot + sin_phi * sin_theta * psi_ddot;
+    float alpha_z = phi_ddot - sin_theta * theta_dot * psi_dot + cos_theta * psi_ddot;  
+
+    // Calculate orientation quaternion reference with Cubli in vertex
+    qr0 = q0 * qu0 - q1 * qu1 - q2 * qu2 - q3 * qu3;
+    qr1 = q0 * qu1 + q1 * qu0 + q2 * qu3 - q3 * qu2;
+    qr2 = q0 * qu2 - q1 * qu3 + q2 * qu0 + q3 * qu1;
+    qr3 = q0 * qu3 + q1 * qu2 - q2 * qu1 + q3 * qu0;
+
+    // Calculate angular velocity reference with Cubli in vertex
+    omega_r_x = omega_x + 2.0 * (omega_x * (-qu2_qu2 - qu3_qu3) + omega_y * ( qu0_qu3 + qu1_qu2) + omega_z * 
+        (-qu0_qu2 + qu1_qu3));
+    omega_r_y = omega_y + 2.0 * (omega_x * (-qu0_qu3 + qu1_qu2) + omega_y * (-qu1_qu1 - qu3_qu3) + omega_z * 
+        ( qu0_qu1 + qu2_qu3));
+    omega_r_z = omega_z + 2.0 * (omega_x * ( qu0_qu2 + qu1_qu3) + omega_y * (-qu0_qu1 + qu2_qu3) + omega_z * 
+        (-qu1_qu1 - qu2_qu2));
+
+    // Calculate angular acceleration reference with Cubli in vertex
+    alpha_r_x = alpha_x + 2.0 * (alpha_x * (-qu2_qu2 - qu3_qu3) + alpha_y * ( qu0_qu3 + qu1_qu2) + alpha_z * 
+        (-qu0_qu2 + qu1_qu3));
+    alpha_r_y = alpha_y + 2.0 * (alpha_x * (-qu0_qu3 + qu1_qu2) + alpha_y * (-qu1_qu1 - qu3_qu3) + alpha_z * 
+        ( qu0_qu1 + qu2_qu3));
+    alpha_r_z = alpha_z + 2.0 * (alpha_x * ( qu0_qu2 + qu1_qu3) + alpha_y * (-qu0_qu1 + qu2_qu3) + alpha_z * 
+        (-qu1_qu1 - qu2_qu2));
+}

software/cube-controller/src/classes/trajectory_attitude.h

@@ -0,0 +1,33 @@
+#ifndef trajectory_attitude_h
+#define trajectory_attitude_h
+
+#include "Arduino.h"
+
+#include "../definitions/parameters.h"
+
+// Attitude trajectory class
+class AttitudeTrajectory {
+    public:
+        // Constructor
+        AttitudeTrajectory();
+        // Initializer
+        void init();
+        // Generate step
+        void generate();
+        //
+        float qr0, qr1, qr2, qr3;
+        //
+        float omega_r_x, omega_r_y, omega_r_z;
+        //
+        float alpha_r_x, alpha_r_y, alpha_r_z;
+
+    private:
+        // Microsecond timer
+        unsigned long timer;
+        //
+        float pos, vel, acc, jer, sna, cra;
+        //
+        bool flag_rot1, flag_res1, flag_rot2, flag_res2;
+};
+
+#endif

software/cube-controller/src/cube-controller.h

@@ -0,0 +1,14 @@
+// Include definitions
+#include "definitions/parameters.h"
+#include "definitions/pins.h"
+
+// Include drivers
+#include "classes/hall.h"
+#include "classes/icm20948.h"
+#include "classes/motor.h"
+
+// Include modules
+#include "classes/estimator_wheel.h"
+#include "classes/estimator_attitude.h"
+#include "classes/controller_attitude_wheel.h"
+#include "classes/trajectory_attitude.h"

software/cube-controller/src/definitions/parameters.h

@@ -0,0 +1,115 @@
+#ifndef parameters_h
+#define parameters_h
+
+// System frequencies and periods
+const float f = 250;
+const float dt = 1 / f;
+const unsigned int dt_us = dt * 1e6;
+
+// IMU gyroscope offsets
+const float b_gyr_x = 0.74;
+const float b_gyr_y = 0.66;
+const float b_gyr_z = -0.45;
+
+// IMU accelerator offsets and gains
+const float b_acc_x = -6;
+const float b_acc_y = -215;
+const float b_acc_z = 161;
+const float f_acc_x = 1672.7;
+const float f_acc_y = 1673.3;
+const float f_acc_z = 1691.5;
+
+// Physical parameters
+const float pi = 3.14159265359;
+const float g = 9.80665;
+
+// Surface parameters
+const float b = 0.0;
+
+// Electrical motor properties
+const float Ra = 0.942; // Armature (winding) resistance (Ohm)
+const float La = 0.363e-3; // Armature (winding) inductance (H)
+const float Km = 36e-3; // Torque constant (Nm/A)
+const float ia_max = 6; // Maximum current (A)
+const float omega_nl = 6250 * pi / 30; // No load speed (rad/s)
+
+// Mechanical motor properties
+const float tau_c = 3.4e-3; // Coulomb friction torque (Nm)
+const float bw = 1.513e-5; // Rotational viscous friction coefficient (Nms/rad)
+
+// Structure properties
+const float l = 0.15; // Structure side length (m)
+const float m_s = 1.0; // Structure mass (kg)
+const float I_s_xx = 3e-3; // Structure moment of inertia around xyz at center of mass (kgm^2)
+
+// Reaction wheel properties
+const float m_w = 0.225; // Reaction wheel mass (kg)
+const float I_w_xx = 4.855e-4; // Reaction wheel moment of inertia around x axis at center of mass (kgm^2)
+const float I_w_yy = 2.439e-4; // Reaction wheel moment of inertia around yz axis at center of mass (kgm^2)
+
+// Combined properties of structure and reaction wheels
+const float m_c = m_s + 3 * m_w; // Cubli total mass (kg)
+const float I_c_xx_bar = I_s_xx + 2 * I_w_yy + (m_s + 2.0 * m_w) * l * l / 2.0; // In-plane moment of inertia (kgm^2)
+const float I_c_xy_bar = -(m_s + m_w) * l * l / 4.0; // Out-of-plane moment of inertia (kgm^2)
+
+// Auxiliary parameters
+const float m_c_bar = m_c - m_w;
+const float m_c_bar_g_l = m_c_bar * g * l;
+
+// Estimator gains
+const float lds = 1; // How much do you trust the accelerometer compared to the gyroscope? This gain determines that.
+const float ldw = 150; // How much do you trust the hall sensor compared to the wheel model? This gain determines that.
+
+// Controller gains. These must be re-tuned if your cube has different dynamics (weights, inertias, dimensions, etc.)
+const float kp = 300;
+const float kd = 40;
+const float kpw = 0.009;
+const float kdw = 0.02;
+
+// One out of three of the following initial reference quaternions should be uncommented
+// Quaternion reference (Cubli sitting on on corner, corrected for center of mass misalignment)
+const float phi_e = 0 * pi / 180.0;
+const float qu0 =                    cos(phi_e / 2.0 + acos(sqrt(3.0) / 3.0) / 2.0);
+const float qu1 =  sqrt(2.0) / 2.0 * sin(phi_e / 2.0 + acos(sqrt(3.0) / 3.0) / 2.0);
+const float qu2 = -sqrt(2.0) / 2.0 * sin(phi_e / 2.0 + acos(sqrt(3.0) / 3.0) / 2.0);
+const float qu3 =  0.0;
+
+// Quaternion reference (Cubli sitting on x axis edge, corrected for center of mass misalignment)
+// const float phi_e = -3.0 * pi / 180.0;
+// const float qu0 = cos(phi_e / 2.0 - pi / 8.0);
+// const float qu1 = cos(phi_e / 2.0 + 3.0 * pi / 8.0);
+// const float qu2 = 0.0;
+// const float qu3 = 0.0;
+
+// Quaternion reference (Cubli sitting on y axis edge, corrected for center of mass misalignment)
+// const float phi_e = -3.0 * pi / 180.0;
+// const float qu0 = cos(phi_e / 2.0 - pi / 8.0);
+// const float qu1 = 0.0;
+// const float qu2 = -cos(phi_e / 2.0 + 3.0 * pi / 8.0);
+// const float qu3 = 0.0;
+
+// Quaternion stuff
+const float qu0_qu0 = qu0 * qu0;
+const float qu0_qu1 = qu0 * qu1;
+const float qu0_qu2 = qu0 * qu2;
+const float qu0_qu3 = qu0 * qu3;
+const float qu1_qu1 = qu1 * qu1;
+const float qu1_qu2 = qu1 * qu2;
+const float qu1_qu3 = qu1 * qu3;
+const float qu2_qu2 = qu2 * qu2;
+const float qu2_qu3 = qu2 * qu3;
+const float qu3_qu3 = qu3 * qu3;
+
+// Minimum and maximum error limits (for control safety)
+const float phi_min = 5.0 * pi / 180.0;
+const float phi_max = 40.0 * pi / 180.0;
+
+// Minimum jerk trajectory parameters
+const float pos_traj = 2.0 * pi; // Trajectory path (rad)
+const float t_rest = 10.0; // Rest time (s)
+const float t_traj = 20.0; // Trajectory time (s)
+const float cra_0 = 720.0 * pos_traj / pow(t_traj, 5);
+const float sna_0 = 360.0 * pos_traj / pow(t_traj, 4);
+const float jer_0 =  60.0 * pos_traj / pow(t_traj, 3);
+
+#endif

software/cube-controller/src/definitions/pins.h

@@ -0,0 +1,21 @@
+#ifndef pins_h
+#define pins_h
+
+// Motor controllers
+const int M1_ENABLE = 6;
+const int M1_CURRENT = 4;
+const int M2_ENABLE = 12;
+const int M2_CURRENT = 14;
+const int M3_ENABLE = 18;
+const int M3_CURRENT = 16;
+
+// Motor speeds
+const int M1_SPEED = 7;
+const int M2_SPEED = 10;
+const int M3_SPEED = 9;
+
+// IMU
+const int IMU_SDA = 47;
+const int IMU_SCL = 21;
+
+#endif