From baa4c90145aba88dce353d575c39aef58f7d317d Mon Sep 17 00:00:00 2001
From: Samuel Ubuntu Laptop <lausingsamuel@gmail.com>
Date: Thu, 21 May 2026 20:01:19 -0700
Subject: [PATCH 01/14] intiilaize imu.h

---
 src/software/embedded/services/imu.cpp | 51 +++++++++++++++++++++++++
 src/software/embedded/services/imu.h   | 52 ++++++++++++++++++++++++++
 2 files changed, 103 insertions(+)
 create mode 100644 src/software/embedded/services/imu.cpp
 create mode 100644 src/software/embedded/services/imu.h

diff --git a/src/software/embedded/services/imu.cpp b/src/software/embedded/services/imu.cpp
new file mode 100644
index 0000000000..a24a8c6f16
--- /dev/null
+++ b/src/software/embedded/services/imu.cpp
@@ -0,0 +1,51 @@
+#include <fcntl.h>
+#include <linux/i2c-dev.h>
+#include <linux/i2c.h>
+#include <sys/ioctl.h>
+
+#include "software/logger/logger.h"
+
+// these functions taken from
+// https://git.kernel.org/pub/scm/utils/i2c-tools/i2c-tools.git/tree/lib/smbus.c
+__s32 i2c_smbus_access(int file, char read_write, __u8 command, int size,
+                       union i2c_smbus_data* data)
+{
+    struct i2c_smbus_ioctl_data args;
+    __s32 err;
+
+    args.read_write = read_write;
+    args.command    = command;
+    args.size       = size;
+    args.data       = data;
+
+    err = ioctl(file, I2C_SMBUS, &args);
+    if (err == -1)
+        err = -errno;
+    return err;
+}
+
+__s32 i2c_smbus_read_byte_data(int file, __u8 command)
+{
+    union i2c_smbus_data data;
+    int err;
+
+    err = i2c_smbus_access(file, I2C_SMBUS_READ, command, I2C_SMBUS_BYTE_DATA, &data);
+    if (err < 0)
+        return err;
+
+    return 0x0FF & data.byte;
+}
+
+__s32 i2c_smbus_write_byte_data(int file, __u8 command, __u8 value)
+{
+    union i2c_smbus_data data;
+    data.byte = value;
+    return i2c_smbus_access(file, I2C_SMBUS_WRITE, command, I2C_SMBUS_BYTE_DATA, &data);
+}
+
+
+ImuService:ImuService(): initialized_(false)
+{
+
+
+}
diff --git a/src/software/embedded/services/imu.h b/src/software/embedded/services/imu.h
new file mode 100644
index 0000000000..8f2a5576b7
--- /dev/null
+++ b/src/software/embedded/services/imu.h
@@ -0,0 +1,52 @@
+#pragma once
+
+#include <Eigen/Dense>
+#include <utility>
+#include <numbers>
+
+#include "proto/tbots_software_msgs.pb.h"
+#include "software/geom/angular_velocity.h"
+
+/**
+ * Handles low level IMU I2C communication, and some minor offset filtering.
+ */
+class ImuService
+{
+    public:
+		ImuService();
+		/*
+		 * Polls the latest IMU reading of the angular velocity of the robot on the z axis
+		 * @return the current angular velocty of the robot on the z axis
+		 */
+		std::optional<AngularVelocity> pollHeadingVelocity();
+		/*
+		 * Polls the latest IMU reading of the linear acceleration of the robot on the z plane 
+		 * @return the current linear acceleration of the robot on the z plane		 
+		 */
+		std::optional<Eigen::Vector2d> pollLinearAcceleration();
+
+
+		// Variance from datasheet (in rad^2/s^2)
+		static constexpr double IMU_VARIANCE = 
+			(4.0 * 14.4222 / 1000.0 * std::numbers::pi / 180.0) *
+			(4.0 * 14.4222 / 1000.0 * std::numbers::pi / 180.0);
+
+	private:
+		bool initialized_=true;
+		int file_descriptor_=0;
+		doubel degrees_error_;
+		// Maps the maximum raw reading from 16-bit integer to be 2 times gravity	
+		static constexpr double ACCELEROMETER_FULL_SCALE_G = 2.0;
+		// Same for gyroscope, 1000 degrees per second 
+		static constexpr double IMU_FULL_SCALE_DPS=1000.0;
+		
+		/// Various i2c registers.
+		static const uint8_t WHOAMI_REG        = 0xf;
+    	static const uint8_t ACCEL_CONTROL_REG = 0x10;
+    	static const uint8_t GYRO_CONTROL_REG  = 0x11;
+    	static const uint8_t CTRL4_C           = 0x13;
+    	static const uint8_t CTRL6_C           = 0x15;
+    	static const uint8_t CTRL8_XL          = 0x17;
+    	static const uint8_t YAW_LEAST_SIG_REG = 0x26;
+    	static const uint8_t YAW_MOST_SIG_REG  = 0x27;
+}

From 7aaec7e17ec9d2d7c82b63f3eafd43d2fe4566e1 Mon Sep 17 00:00:00 2001
From: Samuel Ubuntu Laptop <lausingsamuel@gmail.com>
Date: Thu, 21 May 2026 20:04:32 -0700
Subject: [PATCH 02/14] finish imu.h

---
 src/software/embedded/services/imu.h | 15 +++++++++++++--
 1 file changed, 13 insertions(+), 2 deletions(-)

diff --git a/src/software/embedded/services/imu.h b/src/software/embedded/services/imu.h
index 8f2a5576b7..5948452ea7 100644
--- a/src/software/embedded/services/imu.h
+++ b/src/software/embedded/services/imu.h
@@ -47,6 +47,17 @@ class ImuService
     	static const uint8_t CTRL4_C           = 0x13;
     	static const uint8_t CTRL6_C           = 0x15;
     	static const uint8_t CTRL8_XL          = 0x17;
-    	static const uint8_t YAW_LEAST_SIG_REG = 0x26;
-    	static const uint8_t YAW_MOST_SIG_REG  = 0x27;
+	 // Device path for the IMU
+	    inline static const std::string IMU_DEVICE = "/dev/i2c-1";
+		// Gyroscope Z-axis (Yaw) Output Data Registers
+	    static constexpr uint8_t YAW_LEAST_SIG_REG = 0x26; // OUTZ_L_G
+	    static constexpr uint8_t YAW_MOST_SIG_REG  = 0x27; // OUTZ_H_G
+	
+	    // Accelerometer X-axis Output Data Registers
+	    static constexpr uint8_t ACCEL_X_LEAST_SIG_REG = 0x28; // OUTX_L_XL
+	    static constexpr uint8_t ACCEL_X_MOST_SIG_REG  = 0x29; // OUTX_H_XL
+	
+	    // Accelerometer Y-axis Output Data Registers
+	    static constexpr uint8_t ACCEL_Y_LEAST_SIG_REG = 0x2A; // OUTY_L_XL
+	    static constexpr uint8_t ACCEL_Y_MOST_SIG_REG  = 0x2B; // OUTY_H_XL
 }

From 6d68e744f3753a07c2d5458dcd0534eb56f50742 Mon Sep 17 00:00:00 2001
From: Samuel Ubuntu Laptop <lausingsamuel@gmail.com>
Date: Thu, 21 May 2026 20:17:59 -0700
Subject: [PATCH 03/14] init imu.cpp

---
 src/software/embedded/services/imu.cpp | 105 ++++++++++++++++++++++++-
 src/software/embedded/services/imu.h   |   5 +-
 2 files changed, 108 insertions(+), 2 deletions(-)

diff --git a/src/software/embedded/services/imu.cpp b/src/software/embedded/services/imu.cpp
index a24a8c6f16..c68e09b045 100644
--- a/src/software/embedded/services/imu.cpp
+++ b/src/software/embedded/services/imu.cpp
@@ -2,7 +2,9 @@
 #include <linux/i2c-dev.h>
 #include <linux/i2c.h>
 #include <sys/ioctl.h>
+#include <cliits>
 
+#include "imu.h"
 #include "software/logger/logger.h"
 
 // these functions taken from
@@ -44,8 +46,109 @@ __s32 i2c_smbus_write_byte_data(int file, __u8 command, __u8 value)
 }
 
 
-ImuService:ImuService(): initialized_(false)
+ImuService::ImuService() : initialized_(false)
 {
+    // Establish connection to the IMU and verify that the who am I pin is correct.
+    file_descriptor_ = open(IMU_DEVICE.c_str(), O_RDWR);
+    int ret          = ioctl(file_descriptor_, I2C_SLAVE_FORCE, 0x6b);
+    if (ret < 0)
+    {
+        LOG(WARNING)
+            << "Failed to initialize the IMU: failed to establish i2c connection.";
+        return;
+    }
+    if (i2c_smbus_read_byte_data(file_descriptor_, WHOAMI_REG) != 106)
+    {
+        LOG(WARNING) << "Failed to initialize the IMU: WHOAMI register "
+                     << static_cast<int>(WHOAMI_REG) << " read incorrectly.";
+        return;
+    }
+    // Attempt to enable gyro and accelerometer, checking that writes are successful
+    // See lsm6dsl datasheet for how to set these registers.
+    i2c_smbus_write_byte_data(file_descriptor_, ACCEL_CONTROL_REG, 0b01000000);
+    if (i2c_smbus_read_byte_data(file_descriptor_, ACCEL_CONTROL_REG) != 0b01000000)
+    {  // write unsuccessful
+        LOG(WARNING)
+            << "Failed to initialize the IMU: writing to accelerometer config register "
+            << static_cast<int>(ACCEL_CONTROL_REG) << " unsuccessful";
+        return;
+    }
+    // Set Gyroscope output data rate to 208 Hz, setting FS to 1000 dps (pg 61 of
+    // datasheet for lsm6dsl, pg 21)
+    i2c_smbus_write_byte_data(file_descriptor_, GYRO_CONTROL_REG, 0b01011000);
+    if (i2c_smbus_read_byte_data(file_descriptor_, GYRO_CONTROL_REG) != 0b01011000)
+    {  // write unsuccessful
+        LOG(WARNING)
+            << "Failed to initialize the IMU: writing to gyroscope config register "
+            << "unsuccessful";
+        return;
+    }
 
+    // Enable Gyro digital LPF1 and set bandwidth to ~65Hz (FTYPE=000)
+    // CTRL4_C: bit 1 (LPF1_SEL_G) = 1
+    i2c_smbus_write_byte_data(file_descriptor_, CTRL4_C, 0b00000010);
+    // CTRL6_C: bits 2:0 (FTYPE) = 000
+    i2c_smbus_write_byte_data(file_descriptor_, CTRL6_C, 0b00000000);
 
+    // Enable Accelerometer digital LPF2
+    // CTRL8_XL: bit 7 (LPF2_XL_EN) = 1
+    i2c_smbus_write_byte_data(file_descriptor_, CTRL8_XL, 0b10000000);
+
+    initialized_ = true;
+    LOG(INFO) << "Initialized IMU! Calibrating...";
+    degrees_error_ = 0;
+    // get 100 sample average of stationary reading, so all future readings can be
+    // corrected
+    double sum        = 0;
+    int valid_samples = 0;
+    for (int i = 0; i < 100; i++)
+    {
+        auto poll = pollHeadingRate();
+        if (poll.has_value())
+        {
+            sum += poll->toDegrees();
+            valid_samples++;
+        }
+        usleep(50000);
+    }
+
+    if (valid_samples > 0)
+    {
+        degrees_error_ = sum / valid_samples;
+        LOG(INFO) << "IMU Calibration complete. Offset: " << degrees_error_ << " dps";
+    }
+    else
+    {
+        LOG(WARNING) << "IMU Calibration failed: no valid samples received. Heading "
+                        "stability will be poor.";
+        initialized_ = false;
+    }
+}
+std::optional<int16_t> ImuService::combineBits(uint8_t ls_reg, uint8_t ms_reg)
+{
+    int least_significant = i2c_smbus_read_byte_data(file_descriptor_, ls_reg);
+    int most_significant  = i2c_smbus_read_byte_data(file_descriptor_, ms_reg);
+
+    if (least_significant < 0 || most_significant < 0)
+    {
+        return std::nullopt;
+    }
+
+    return (static_cast<uint8_t>(most_significant) << 8) |
+                        static_cast<uint8_t>(least_significant);
+	
+}
+
+std::optional<AngularVelocity> ImuService::pollHeadingVelocity()
+{
+    if (!initialized_)
+    {
+        return std::nullopt;
+    }
+
+    // Two separate registers for the Gyro output data.
+	int16_t full_word = combineBits(YAW_LEAST_SIG_REG, YAW_MOST_SIG_REG);
+    double degrees_per_sec = static_cast<double>(full_word) /
+                             static_cast<double>(SHRT_MAX) * IMU_FULL_SCALE_DPS;
+    return AngularVelocity::fromDegrees(degrees_per_sec - degrees_error_);
 }
diff --git a/src/software/embedded/services/imu.h b/src/software/embedded/services/imu.h
index 5948452ea7..0652bca6ed 100644
--- a/src/software/embedded/services/imu.h
+++ b/src/software/embedded/services/imu.h
@@ -32,6 +32,7 @@ class ImuService
 			(4.0 * 14.4222 / 1000.0 * std::numbers::pi / 180.0);
 
 	private:
+		std::optional<int16_t> combineBits();
 		bool initialized_=true;
 		int file_descriptor_=0;
 		doubel degrees_error_;
@@ -47,8 +48,10 @@ class ImuService
     	static const uint8_t CTRL4_C           = 0x13;
     	static const uint8_t CTRL6_C           = 0x15;
     	static const uint8_t CTRL8_XL          = 0x17;
-	 // Device path for the IMU
+
+		// Device path for the IMU
 	    inline static const std::string IMU_DEVICE = "/dev/i2c-1";
+
 		// Gyroscope Z-axis (Yaw) Output Data Registers
 	    static constexpr uint8_t YAW_LEAST_SIG_REG = 0x26; // OUTZ_L_G
 	    static constexpr uint8_t YAW_MOST_SIG_REG  = 0x27; // OUTZ_H_G

From f466385fc678db615d6c1a9afe0ea50861f5fc54 Mon Sep 17 00:00:00 2001
From: Samuel Ubuntu Laptop <lausingsamuel@gmail.com>
Date: Thu, 21 May 2026 20:35:57 -0700
Subject: [PATCH 04/14] finish imu.cpp

---
 src/software/embedded/services/imu.cpp | 52 +++++++++++++++++++++++---
 src/software/embedded/services/imu.h   |  1 +
 2 files changed, 47 insertions(+), 6 deletions(-)

diff --git a/src/software/embedded/services/imu.cpp b/src/software/embedded/services/imu.cpp
index c68e09b045..a48ec1e0b6 100644
--- a/src/software/embedded/services/imu.cpp
+++ b/src/software/embedded/services/imu.cpp
@@ -2,9 +2,10 @@
 #include <linux/i2c-dev.h>
 #include <linux/i2c.h>
 #include <sys/ioctl.h>
-#include <cliits>
+#include <climits> // For SHRT_MAX
 
 #include "imu.h"
+#include "shared/constants.h"
 #include "software/logger/logger.h"
 
 // these functions taken from
@@ -103,7 +104,8 @@ ImuService::ImuService() : initialized_(false)
     int valid_samples = 0;
     for (int i = 0; i < 100; i++)
     {
-        auto poll = pollHeadingRate();
+        // Fixed: Call the actual implemented function name
+        auto poll = pollHeadingVelocity();
         if (poll.has_value())
         {
             sum += poll->toDegrees();
@@ -124,6 +126,7 @@ ImuService::ImuService() : initialized_(false)
         initialized_ = false;
     }
 }
+
 std::optional<int16_t> ImuService::combineBits(uint8_t ls_reg, uint8_t ms_reg)
 {
     int least_significant = i2c_smbus_read_byte_data(file_descriptor_, ls_reg);
@@ -134,9 +137,10 @@ std::optional<int16_t> ImuService::combineBits(uint8_t ls_reg, uint8_t ms_reg)
         return std::nullopt;
     }
 
-    return (static_cast<uint8_t>(most_significant) << 8) |
+    uint16_t combined = (static_cast<uint8_t>(most_significant) << 8) |
                         static_cast<uint8_t>(least_significant);
-	
+                        
+    return static_cast<int16_t>(combined);
 }
 
 std::optional<AngularVelocity> ImuService::pollHeadingVelocity()
@@ -146,9 +150,45 @@ std::optional<AngularVelocity> ImuService::pollHeadingVelocity()
         return std::nullopt;
     }
 
-    // Two separate registers for the Gyro output data.
-	int16_t full_word = combineBits(YAW_LEAST_SIG_REG, YAW_MOST_SIG_REG);
+    std::optional<int16_t> opt_full_word = combineBits(YAW_LEAST_SIG_REG, YAW_MOST_SIG_REG);
+    
+    if (!opt_full_word.has_value())
+    {
+        return std::nullopt;
+    }
+
+    int16_t full_word = opt_full_word.value();
+    
     double degrees_per_sec = static_cast<double>(full_word) /
                              static_cast<double>(SHRT_MAX) * IMU_FULL_SCALE_DPS;
     return AngularVelocity::fromDegrees(degrees_per_sec - degrees_error_);
 }
+
+std::optional<Eigen::Vector2d> ImuService::pollLinearAcceleration()
+{
+    if (!initialized_)
+    {
+        return std::nullopt;
+    }
+
+    std::optional<int16_t> opt_x = combineBits(ACCEL_X_LEAST_SIG_REG, ACCEL_X_MOST_SIG_REG);
+    std::optional<int16_t> opt_y = combineBits(ACCEL_Y_LEAST_SIG_REG, ACCEL_Y_MOST_SIG_REG);
+
+    if (!opt_x.has_value() || !opt_y.has_value())
+    {
+        return std::nullopt;
+    }
+
+    int16_t raw_x = opt_x.value();
+    int16_t raw_y = opt_y.value();
+
+    double a_x = (static_cast<double>(raw_x) / SHRT_MAX) 
+                 * ACCELEROMETER_FULL_SCALE_G 
+                 * ACCELERATION_DUE_TO_GRAVITY_METERS_PER_SECOND_SQUARED;
+                 
+    double a_y = (static_cast<double>(raw_y) / SHRT_MAX) 
+                 * ACCELEROMETER_FULL_SCALE_G 
+                 * ACCELERATION_DUE_TO_GRAVITY_METERS_PER_SECOND_SQUARED;
+                 
+    return Eigen::Vector2d(a_x, a_y);
+}
diff --git a/src/software/embedded/services/imu.h b/src/software/embedded/services/imu.h
index 0652bca6ed..e74b3a4da6 100644
--- a/src/software/embedded/services/imu.h
+++ b/src/software/embedded/services/imu.h
@@ -3,6 +3,7 @@
 #include <Eigen/Dense>
 #include <utility>
 #include <numbers>
+#include <climits>
 
 #include "proto/tbots_software_msgs.pb.h"
 #include "software/geom/angular_velocity.h"

From 4199e76903e45ce8f7d726452fb817c6557cd70c Mon Sep 17 00:00:00 2001
From: Samuel Ubuntu Laptop <lausingsamuel@gmail.com>
Date: Thu, 21 May 2026 20:56:54 -0700
Subject: [PATCH 05/14] update imu cpp and add thunderloop test

---
 src/software/embedded/services/imu.cpp |  2 +-
 src/software/embedded/services/imu.h   |  4 ++--
 src/software/embedded/thunderloop.cpp  | 13 +++++++++++--
 src/software/embedded/thunderloop.h    |  1 +
 4 files changed, 15 insertions(+), 5 deletions(-)

diff --git a/src/software/embedded/services/imu.cpp b/src/software/embedded/services/imu.cpp
index a48ec1e0b6..b4dc6c7af6 100644
--- a/src/software/embedded/services/imu.cpp
+++ b/src/software/embedded/services/imu.cpp
@@ -150,7 +150,7 @@ std::optional<AngularVelocity> ImuService::pollHeadingVelocity()
         return std::nullopt;
     }
 
-    std::optional<int16_t> opt_full_word = combineBits(YAW_LEAST_SIG_REG, YAW_MOST_SIG_REG);
+    std::optional<int16_t> opt_full_word = combineBits(HEADING_LEAST_SIG_REG, HEADING_MOST_SIG_REG);
     
     if (!opt_full_word.has_value())
     {
diff --git a/src/software/embedded/services/imu.h b/src/software/embedded/services/imu.h
index e74b3a4da6..d234a05335 100644
--- a/src/software/embedded/services/imu.h
+++ b/src/software/embedded/services/imu.h
@@ -54,8 +54,8 @@ class ImuService
 	    inline static const std::string IMU_DEVICE = "/dev/i2c-1";
 
 		// Gyroscope Z-axis (Yaw) Output Data Registers
-	    static constexpr uint8_t YAW_LEAST_SIG_REG = 0x26; // OUTZ_L_G
-	    static constexpr uint8_t YAW_MOST_SIG_REG  = 0x27; // OUTZ_H_G
+	    static constexpr uint8_t HEADING_LEAST_SIG_REG = 0x26; // OUTZ_L_G
+	    static constexpr uint8_t HEADING_MOST_SIG_REG  = 0x27; // OUTZ_H_G
 	
 	    // Accelerometer X-axis Output Data Registers
 	    static constexpr uint8_t ACCEL_X_LEAST_SIG_REG = 0x28; // OUTX_L_XL
diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp
index b8a440a683..02a08c090d 100644
--- a/src/software/embedded/thunderloop.cpp
+++ b/src/software/embedded/thunderloop.cpp
@@ -10,6 +10,7 @@
 #include "shared/constants.h"
 #include "software/embedded/primitive_executor.h"
 #include "software/embedded/services/motor.h"
+#include "software/embedded/services/imu.h"
 #include "software/logger/logger.h"
 #include "software/logger/network_logger.h"
 #include "software/networking/tbots_network_exception.h"
@@ -120,8 +121,10 @@ Thunderloop::Thunderloop(const robot_constants::RobotConstants& robot_constants,
 
     motor_service_  = std::make_unique<MotorService>(robot_constants, loop_hz);
     g_motor_service = motor_service_.get();
-    motor_service_->setup();
-    LOG(INFO) << "THUNDERLOOP: Motor Service initialized!";
+    mLOG(INFO) << "THUNDERLOOP: Motor Service initialized! Next initializing IMU Service";
+
+    imu_service_ = std::make_unique<ImuService>();
+    LOG(INFO) << "THUNDERLOOP: IMU Service initialized!";otor_service_->setup();
 
     LOG(INFO) << "THUNDERLOOP: finished initialization with ROBOT ID: " << robot_id_
               << ", CHANNEL ID: " << channel_id_
@@ -177,6 +180,12 @@ void Thunderloop::runLoop()
     robot_status_.set_thunderloop_version(thunderloop_hash);
     robot_status_.set_thunderloop_date_flashed(thunderloop_date_flashed);
 
+	
+	std::optional<Angle> imu_poll = imu_service_->pollHeadingVelocity();
+	if (imu_poll.has_value()){
+
+		LOG(INFO) << "IMU Heading Velocity" << imu_poll.value() ;
+	}
     for (;;)
     {
         struct timespec time_since_prev_iter;
diff --git a/src/software/embedded/thunderloop.h b/src/software/embedded/thunderloop.h
index cf70a12b68..7a524a2cbc 100644
--- a/src/software/embedded/thunderloop.h
+++ b/src/software/embedded/thunderloop.h
@@ -13,6 +13,7 @@
 #include "software/embedded/services/motor.h"
 #include "software/embedded/services/network/network.h"
 #include "software/embedded/services/power.h"
+#include "software/embedded/services/imu.h"
 #include "software/embedded/toml_config/toml_config_client.h"
 #include "software/logger/logger.h"
 

From a874142166f2f998084e874e787dff094f8b5d6d Mon Sep 17 00:00:00 2001
From: Samuel Ubuntu Laptop <lausingsamuel@gmail.com>
Date: Sat, 23 May 2026 11:38:35 -0700
Subject: [PATCH 06/14] initialize velocity related stuff

---
 src/software/embedded/services/imu.cpp |  9 +++++++++
 src/software/embedded/services/imu.h   | 11 +++++++++++
 src/software/embedded/thunderloop.h    |  6 ++++++
 3 files changed, 26 insertions(+)

diff --git a/src/software/embedded/services/imu.cpp b/src/software/embedded/services/imu.cpp
index b4dc6c7af6..dba8002fae 100644
--- a/src/software/embedded/services/imu.cpp
+++ b/src/software/embedded/services/imu.cpp
@@ -7,6 +7,7 @@
 #include "imu.h"
 #include "shared/constants.h"
 #include "software/logger/logger.h"
+#include "software/geom/angular_acceleration.h"
 
 // these functions taken from
 // https://git.kernel.org/pub/scm/utils/i2c-tools/i2c-tools.git/tree/lib/smbus.c
@@ -164,6 +165,14 @@ std::optional<AngularVelocity> ImuService::pollHeadingVelocity()
     return AngularVelocity::fromDegrees(degrees_per_sec - degrees_error_);
 }
 
+std::optional<Eigen::Vector2d> ImuService::pollHeadingAcceleration(std::optional<AngularVelocity> prev_angular_velocity, double prev_time){
+	if (!prev_angular_velocity.has_value()){
+		return std::nullopt;
+	}
+	
+	 
+	
+}
 std::optional<Eigen::Vector2d> ImuService::pollLinearAcceleration()
 {
     if (!initialized_)
diff --git a/src/software/embedded/services/imu.h b/src/software/embedded/services/imu.h
index d234a05335..41eef0aba9 100644
--- a/src/software/embedded/services/imu.h
+++ b/src/software/embedded/services/imu.h
@@ -20,6 +20,11 @@ class ImuService
 		 * @return the current angular velocty of the robot on the z axis
 		 */
 		std::optional<AngularVelocity> pollHeadingVelocity();
+		/*
+		 * Polls the latest IMU reading of the linear acceleration of the robot on the z plane 
+		 * @return the current linear acceleration of the robot on the z plane		 
+		 */
+		std::optional<Eigen::Vector2d> pollHeadingAcceleration();
 		/*
 		 * Polls the latest IMU reading of the linear acceleration of the robot on the z plane 
 		 * @return the current linear acceleration of the robot on the z plane		 
@@ -37,6 +42,7 @@ class ImuService
 		bool initialized_=true;
 		int file_descriptor_=0;
 		doubel degrees_error_;
+
 		// Maps the maximum raw reading from 16-bit integer to be 2 times gravity	
 		static constexpr double ACCELEROMETER_FULL_SCALE_G = 2.0;
 		// Same for gyroscope, 1000 degrees per second 
@@ -50,6 +56,11 @@ class ImuService
     	static const uint8_t CTRL6_C           = 0x15;
     	static const uint8_t CTRL8_XL          = 0x17;
 
+		// Deviation from center of mass
+		double x_deviation;
+		double y_deviation;
+		
+
 		// Device path for the IMU
 	    inline static const std::string IMU_DEVICE = "/dev/i2c-1";
 
diff --git a/src/software/embedded/thunderloop.h b/src/software/embedded/thunderloop.h
index 7a524a2cbc..e86dd2157a 100644
--- a/src/software/embedded/thunderloop.h
+++ b/src/software/embedded/thunderloop.h
@@ -16,6 +16,7 @@
 #include "software/embedded/services/imu.h"
 #include "software/embedded/toml_config/toml_config_client.h"
 #include "software/logger/logger.h"
+#include "software/geom/angular_velocity.h"
 
 class Thunderloop
 {
@@ -119,6 +120,7 @@ class Thunderloop
      */
     TbotsProto::PowerStatus pollPowerService(struct timespec& poll_time);
 
+
     /**
      * Wait for networking communication to be established. This function is blocking.
      */
@@ -153,6 +155,10 @@ class Thunderloop
     int kick_constant_;
     int chip_pulse_width_;
 
+	// Store previous angular velocity for angular accleration calculations
+	std::optional<AngularVelocity> prev_angular_velocity;
+	double prev_angular_velocity_time;
+	
     // Primitive Executor
     PrimitiveExecutor primitive_executor_;
 

From fad037620d94e430e6fdedc6ffe1d956c597397a Mon Sep 17 00:00:00 2001
From: Samuel Ubuntu Laptop <lausingsamuel@gmail.com>
Date: Sat, 23 May 2026 13:00:10 -0700
Subject: [PATCH 07/14] remove leftover

---
 src/software/embedded/services/imu.cpp      | 16 ++++-------
 src/software/embedded/services/imu.h        | 32 ++++++++++++---------
 src/software/embedded/services/services.cpp |  0
 src/software/embedded/services/services.h   |  0
 4 files changed, 24 insertions(+), 24 deletions(-)
 create mode 100644 src/software/embedded/services/services.cpp
 create mode 100644 src/software/embedded/services/services.h

diff --git a/src/software/embedded/services/imu.cpp b/src/software/embedded/services/imu.cpp
index dba8002fae..36646935c8 100644
--- a/src/software/embedded/services/imu.cpp
+++ b/src/software/embedded/services/imu.cpp
@@ -7,7 +7,6 @@
 #include "imu.h"
 #include "shared/constants.h"
 #include "software/logger/logger.h"
-#include "software/geom/angular_acceleration.h"
 
 // these functions taken from
 // https://git.kernel.org/pub/scm/utils/i2c-tools/i2c-tools.git/tree/lib/smbus.c
@@ -128,7 +127,7 @@ ImuService::ImuService() : initialized_(false)
     }
 }
 
-std::optional<int16_t> ImuService::combineBits(uint8_t ls_reg, uint8_t ms_reg)
+std::optional<int16_t> ImuService::readAndCombineByteData(uint8_t ls_reg, uint8_t ms_reg)
 {
     int least_significant = i2c_smbus_read_byte_data(file_descriptor_, ls_reg);
     int most_significant  = i2c_smbus_read_byte_data(file_descriptor_, ms_reg);
@@ -151,7 +150,7 @@ std::optional<AngularVelocity> ImuService::pollHeadingVelocity()
         return std::nullopt;
     }
 
-    std::optional<int16_t> opt_full_word = combineBits(HEADING_LEAST_SIG_REG, HEADING_MOST_SIG_REG);
+    std::optional<int16_t> opt_full_word = readAndCombineByteData(HEADING_LEAST_SIG_REG, HEADING_MOST_SIG_REG);
     
     if (!opt_full_word.has_value())
     {
@@ -165,14 +164,9 @@ std::optional<AngularVelocity> ImuService::pollHeadingVelocity()
     return AngularVelocity::fromDegrees(degrees_per_sec - degrees_error_);
 }
 
-std::optional<Eigen::Vector2d> ImuService::pollHeadingAcceleration(std::optional<AngularVelocity> prev_angular_velocity, double prev_time){
-	if (!prev_angular_velocity.has_value()){
-		return std::nullopt;
-	}
-	
 	 
 	
-}
+
 std::optional<Eigen::Vector2d> ImuService::pollLinearAcceleration()
 {
     if (!initialized_)
@@ -180,8 +174,8 @@ std::optional<Eigen::Vector2d> ImuService::pollLinearAcceleration()
         return std::nullopt;
     }
 
-    std::optional<int16_t> opt_x = combineBits(ACCEL_X_LEAST_SIG_REG, ACCEL_X_MOST_SIG_REG);
-    std::optional<int16_t> opt_y = combineBits(ACCEL_Y_LEAST_SIG_REG, ACCEL_Y_MOST_SIG_REG);
+    std::optional<int16_t> opt_x = readAndCombineByteData(ACCEL_X_LEAST_SIG_REG, ACCEL_X_MOST_SIG_REG);
+    std::optional<int16_t> opt_y = readAndCombineByteData(ACCEL_Y_LEAST_SIG_REG, ACCEL_Y_MOST_SIG_REG);
 
     if (!opt_x.has_value() || !opt_y.has_value())
     {
diff --git a/src/software/embedded/services/imu.h b/src/software/embedded/services/imu.h
index 41eef0aba9..76afb603db 100644
--- a/src/software/embedded/services/imu.h
+++ b/src/software/embedded/services/imu.h
@@ -14,17 +14,17 @@
 class ImuService
 {
     public:
+		/**
+     	* Constructs and initializes a new IMU service object.
+     	*
+     	* If successfully initialized, will try to do a simple calibration of the IMU.
+     	*/
 		ImuService();
 		/*
 		 * Polls the latest IMU reading of the angular velocity of the robot on the z axis
 		 * @return the current angular velocty of the robot on the z axis
 		 */
 		std::optional<AngularVelocity> pollHeadingVelocity();
-		/*
-		 * Polls the latest IMU reading of the linear acceleration of the robot on the z plane 
-		 * @return the current linear acceleration of the robot on the z plane		 
-		 */
-		std::optional<Eigen::Vector2d> pollHeadingAcceleration();
 		/*
 		 * Polls the latest IMU reading of the linear acceleration of the robot on the z plane 
 		 * @return the current linear acceleration of the robot on the z plane		 
@@ -38,10 +38,21 @@ class ImuService
 			(4.0 * 14.4222 / 1000.0 * std::numbers::pi / 180.0);
 
 	private:
-		std::optional<int16_t> combineBits();
-		bool initialized_=true;
+
+		/*
+		 * reads byte data from two registers, and combine them into a single value 
+		 * @parama ls_reg register of the least significant register
+		 * @parama ms_reg register of the most significant register	
+		 * @return the combined integer value of the two registers		 
+		 */
+
+		std::optional<int16_t> readAndCombineByteData(uint8_t ls_reg, uint8_t ms_reg);
+
+		bool initialized_=false;
+
 		int file_descriptor_=0;
-		doubel degrees_error_;
+
+		double degrees_error_;
 
 		// Maps the maximum raw reading from 16-bit integer to be 2 times gravity	
 		static constexpr double ACCELEROMETER_FULL_SCALE_G = 2.0;
@@ -56,11 +67,6 @@ class ImuService
     	static const uint8_t CTRL6_C           = 0x15;
     	static const uint8_t CTRL8_XL          = 0x17;
 
-		// Deviation from center of mass
-		double x_deviation;
-		double y_deviation;
-		
-
 		// Device path for the IMU
 	    inline static const std::string IMU_DEVICE = "/dev/i2c-1";
 
diff --git a/src/software/embedded/services/services.cpp b/src/software/embedded/services/services.cpp
new file mode 100644
index 0000000000..e69de29bb2
diff --git a/src/software/embedded/services/services.h b/src/software/embedded/services/services.h
new file mode 100644
index 0000000000..e69de29bb2

From 4d9e8cac40131411234f96c3852aef107e2a5a01 Mon Sep 17 00:00:00 2001
From: Samuel Ubuntu Laptop <lausingsamuel@gmail.com>
Date: Sat, 23 May 2026 13:01:48 -0700
Subject: [PATCH 08/14] fix thunderloop

---
 src/software/embedded/thunderloop.cpp | 3 +++
 src/software/embedded/thunderloop.h   | 1 +
 2 files changed, 4 insertions(+)

diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp
index 02a08c090d..96b351206a 100644
--- a/src/software/embedded/thunderloop.cpp
+++ b/src/software/embedded/thunderloop.cpp
@@ -182,10 +182,13 @@ void Thunderloop::runLoop()
 
 	
 	std::optional<Angle> imu_poll = imu_service_->pollHeadingVelocity();
+	
+	//TODO: Replace with actual IMU data usage
 	if (imu_poll.has_value()){
 
 		LOG(INFO) << "IMU Heading Velocity" << imu_poll.value() ;
 	}
+
     for (;;)
     {
         struct timespec time_since_prev_iter;
diff --git a/src/software/embedded/thunderloop.h b/src/software/embedded/thunderloop.h
index e86dd2157a..0f7df7d902 100644
--- a/src/software/embedded/thunderloop.h
+++ b/src/software/embedded/thunderloop.h
@@ -62,6 +62,7 @@ class Thunderloop
     std::unique_ptr<MotorService> motor_service_;
     std::unique_ptr<NetworkService> network_service_;
     std::unique_ptr<PowerService> power_service_;
+    std::unique_ptr<ImuService> imu_service_;
 
     // TOML config client
     std::unique_ptr<TomlConfigClient> toml_config_client_;

From cfa79b920ed8ab683ea6a33f45b6473c6079e984 Mon Sep 17 00:00:00 2001
From: Samuel Ubuntu Laptop <lausingsamuel@gmail.com>
Date: Sat, 23 May 2026 13:03:58 -0700
Subject: [PATCH 09/14] cleanup

---
 src/software/embedded/services/imu.h        | 2 +-
 src/software/embedded/services/services.cpp | 0
 src/software/embedded/services/services.h   | 0
 src/software/embedded/thunderloop.h         | 5 -----
 4 files changed, 1 insertion(+), 6 deletions(-)
 delete mode 100644 src/software/embedded/services/services.cpp
 delete mode 100644 src/software/embedded/services/services.h

diff --git a/src/software/embedded/services/imu.h b/src/software/embedded/services/imu.h
index 76afb603db..da062ec76f 100644
--- a/src/software/embedded/services/imu.h
+++ b/src/software/embedded/services/imu.h
@@ -40,7 +40,7 @@ class ImuService
 	private:
 
 		/*
-		 * reads byte data from two registers, and combine them into a single value 
+		 * Reads byte data from two registers, and combine them into a single value 
 		 * @parama ls_reg register of the least significant register
 		 * @parama ms_reg register of the most significant register	
 		 * @return the combined integer value of the two registers		 
diff --git a/src/software/embedded/services/services.cpp b/src/software/embedded/services/services.cpp
deleted file mode 100644
index e69de29bb2..0000000000
diff --git a/src/software/embedded/services/services.h b/src/software/embedded/services/services.h
deleted file mode 100644
index e69de29bb2..0000000000
diff --git a/src/software/embedded/thunderloop.h b/src/software/embedded/thunderloop.h
index 0f7df7d902..b3da73b76c 100644
--- a/src/software/embedded/thunderloop.h
+++ b/src/software/embedded/thunderloop.h
@@ -16,7 +16,6 @@
 #include "software/embedded/services/imu.h"
 #include "software/embedded/toml_config/toml_config_client.h"
 #include "software/logger/logger.h"
-#include "software/geom/angular_velocity.h"
 
 class Thunderloop
 {
@@ -156,10 +155,6 @@ class Thunderloop
     int kick_constant_;
     int chip_pulse_width_;
 
-	// Store previous angular velocity for angular accleration calculations
-	std::optional<AngularVelocity> prev_angular_velocity;
-	double prev_angular_velocity_time;
-	
     // Primitive Executor
     PrimitiveExecutor primitive_executor_;
 

From 3442db3693337015c7a0a34935815c977f3279c3 Mon Sep 17 00:00:00 2001
From: Samuel Ubuntu Laptop <lausingsamuel@gmail.com>
Date: Sat, 23 May 2026 13:15:53 -0700
Subject: [PATCH 10/14] fix bug

---
 src/software/embedded/thunderloop.cpp | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp
index 96b351206a..faa7e5e415 100644
--- a/src/software/embedded/thunderloop.cpp
+++ b/src/software/embedded/thunderloop.cpp
@@ -120,7 +120,9 @@ Thunderloop::Thunderloop(const robot_constants::RobotConstants& robot_constants,
         << "THUNDERLOOP: Power Service initialized! Next initializing Motor Service";
 
     motor_service_  = std::make_unique<MotorService>(robot_constants, loop_hz);
-    g_motor_service = motor_service_.get();
+	g_motor_service = motor_service_.get();
+	motor_service_->setup();
+
     mLOG(INFO) << "THUNDERLOOP: Motor Service initialized! Next initializing IMU Service";
 
     imu_service_ = std::make_unique<ImuService>();

From 82aa4af49767f78e14565cf444aa524b6718b324 Mon Sep 17 00:00:00 2001
From: Samuel Ubuntu Laptop <lausingsamuel@gmail.com>
Date: Sat, 23 May 2026 13:49:01 -0700
Subject: [PATCH 11/14] fix build

---
 src/software/embedded/BUILD | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/software/embedded/BUILD b/src/software/embedded/BUILD
index d28b7ff4eb..785335c27e 100644
--- a/src/software/embedded/BUILD
+++ b/src/software/embedded/BUILD
@@ -29,6 +29,7 @@ cc_library(
         "//software/embedded/services:motor",
         "//software/embedded/services:power",
         "//software/embedded/services/network",
+        "//software/embedded/services/imu",
         "//software/embedded/toml_config",
         "//software/logger:network_logger",
         "//software/tracy:tracy_constants",

From b638f35333a6c8caa5161557def1f2b76a61aa22 Mon Sep 17 00:00:00 2001
From: Samuel Ubuntu Laptop <lausingsamuel@gmail.com>
Date: Sat, 23 May 2026 14:10:21 -0700
Subject: [PATCH 12/14] fix BUILD

---
 src/software/embedded/services/BUILD | 13 +++++++++++++
 1 file changed, 13 insertions(+)

diff --git a/src/software/embedded/services/BUILD b/src/software/embedded/services/BUILD
index f763e4f8dc..69c2301f99 100644
--- a/src/software/embedded/services/BUILD
+++ b/src/software/embedded/services/BUILD
@@ -31,6 +31,19 @@ cc_library(
     ],
 )
 
+cc_library(
+    name = "imu",
+    srcs = ["imu.cpp"],
+    hdrs = ["imu.h"],
+    deps = [
+        "//proto:tbots_cc_proto",
+        "//shared:constants",
+        "//software/geom:angular_velocity",
+        "//software/logger",
+        "@eigen",
+    ],
+)
+
 cc_binary(
     name = "robot_auto_test",
     srcs = ["robot_auto_test.cpp"],

From e7d12d3ed0c581691a426e47c6cfd84fc4742a11 Mon Sep 17 00:00:00 2001
From: Samuel Ubuntu Laptop <lausingsamuel@gmail.com>
Date: Sat, 23 May 2026 14:12:02 -0700
Subject: [PATCH 13/14] fix bugs

---
 src/software/embedded/BUILD           | 2 +-
 src/software/embedded/services/imu.h  | 2 +-
 src/software/embedded/thunderloop.cpp | 6 +++---
 3 files changed, 5 insertions(+), 5 deletions(-)

diff --git a/src/software/embedded/BUILD b/src/software/embedded/BUILD
index 785335c27e..b0321b44a9 100644
--- a/src/software/embedded/BUILD
+++ b/src/software/embedded/BUILD
@@ -29,7 +29,7 @@ cc_library(
         "//software/embedded/services:motor",
         "//software/embedded/services:power",
         "//software/embedded/services/network",
-        "//software/embedded/services/imu",
+        "//software/embedded/services:imu",
         "//software/embedded/toml_config",
         "//software/logger:network_logger",
         "//software/tracy:tracy_constants",
diff --git a/src/software/embedded/services/imu.h b/src/software/embedded/services/imu.h
index da062ec76f..f3c49515e7 100644
--- a/src/software/embedded/services/imu.h
+++ b/src/software/embedded/services/imu.h
@@ -81,4 +81,4 @@ class ImuService
 	    // Accelerometer Y-axis Output Data Registers
 	    static constexpr uint8_t ACCEL_Y_LEAST_SIG_REG = 0x2A; // OUTY_L_XL
 	    static constexpr uint8_t ACCEL_Y_MOST_SIG_REG  = 0x2B; // OUTY_H_XL
-}
+};
diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp
index faa7e5e415..652833a3f0 100644
--- a/src/software/embedded/thunderloop.cpp
+++ b/src/software/embedded/thunderloop.cpp
@@ -123,10 +123,10 @@ Thunderloop::Thunderloop(const robot_constants::RobotConstants& robot_constants,
 	g_motor_service = motor_service_.get();
 	motor_service_->setup();
 
-    mLOG(INFO) << "THUNDERLOOP: Motor Service initialized! Next initializing IMU Service";
+    LOG(INFO) << "THUNDERLOOP: Motor Service initialized! Next initializing IMU Service";
 
     imu_service_ = std::make_unique<ImuService>();
-    LOG(INFO) << "THUNDERLOOP: IMU Service initialized!";otor_service_->setup();
+    LOG(INFO) << "THUNDERLOOP: IMU Service initialized!";
 
     LOG(INFO) << "THUNDERLOOP: finished initialization with ROBOT ID: " << robot_id_
               << ", CHANNEL ID: " << channel_id_
@@ -183,7 +183,7 @@ void Thunderloop::runLoop()
     robot_status_.set_thunderloop_date_flashed(thunderloop_date_flashed);
 
 	
-	std::optional<Angle> imu_poll = imu_service_->pollHeadingVelocity();
+	std::optional<AngularVelocity> imu_poll = imu_service_->pollHeadingVelocity();
 	
 	//TODO: Replace with actual IMU data usage
 	if (imu_poll.has_value()){

From 8e789ba02a70beb786a1e1f0685c6867ef78bfd1 Mon Sep 17 00:00:00 2001
From: "pre-commit-ci-lite[bot]"
 <117423508+pre-commit-ci-lite[bot]@users.noreply.github.com>
Date: Sat, 23 May 2026 21:18:43 +0000
Subject: [PATCH 14/14] [pre-commit.ci lite] apply automatic fixes

---
 src/software/embedded/BUILD            |   2 +-
 src/software/embedded/services/imu.cpp |  38 +++----
 src/software/embedded/services/imu.h   | 139 ++++++++++++-------------
 src/software/embedded/thunderloop.cpp  |  20 ++--
 src/software/embedded/thunderloop.h    |   2 +-
 5 files changed, 101 insertions(+), 100 deletions(-)

diff --git a/src/software/embedded/BUILD b/src/software/embedded/BUILD
index b0321b44a9..67262ae526 100644
--- a/src/software/embedded/BUILD
+++ b/src/software/embedded/BUILD
@@ -26,10 +26,10 @@ cc_library(
     deps = [
         ":primitive_executor",
         "//proto:tbots_cc_proto",
+        "//software/embedded/services:imu",
         "//software/embedded/services:motor",
         "//software/embedded/services:power",
         "//software/embedded/services/network",
-        "//software/embedded/services:imu",
         "//software/embedded/toml_config",
         "//software/logger:network_logger",
         "//software/tracy:tracy_constants",
diff --git a/src/software/embedded/services/imu.cpp b/src/software/embedded/services/imu.cpp
index 36646935c8..03da1758e0 100644
--- a/src/software/embedded/services/imu.cpp
+++ b/src/software/embedded/services/imu.cpp
@@ -1,10 +1,12 @@
+#include "imu.h"
+
 #include <fcntl.h>
 #include <linux/i2c-dev.h>
 #include <linux/i2c.h>
 #include <sys/ioctl.h>
-#include <climits> // For SHRT_MAX
 
-#include "imu.h"
+#include <climits>  // For SHRT_MAX
+
 #include "shared/constants.h"
 #include "software/logger/logger.h"
 
@@ -139,7 +141,7 @@ std::optional<int16_t> ImuService::readAndCombineByteData(uint8_t ls_reg, uint8_
 
     uint16_t combined = (static_cast<uint8_t>(most_significant) << 8) |
                         static_cast<uint8_t>(least_significant);
-                        
+
     return static_cast<int16_t>(combined);
 }
 
@@ -150,22 +152,22 @@ std::optional<AngularVelocity> ImuService::pollHeadingVelocity()
         return std::nullopt;
     }
 
-    std::optional<int16_t> opt_full_word = readAndCombineByteData(HEADING_LEAST_SIG_REG, HEADING_MOST_SIG_REG);
-    
+    std::optional<int16_t> opt_full_word =
+        readAndCombineByteData(HEADING_LEAST_SIG_REG, HEADING_MOST_SIG_REG);
+
     if (!opt_full_word.has_value())
     {
         return std::nullopt;
     }
 
     int16_t full_word = opt_full_word.value();
-    
+
     double degrees_per_sec = static_cast<double>(full_word) /
                              static_cast<double>(SHRT_MAX) * IMU_FULL_SCALE_DPS;
     return AngularVelocity::fromDegrees(degrees_per_sec - degrees_error_);
 }
 
-	 
-	
+
 
 std::optional<Eigen::Vector2d> ImuService::pollLinearAcceleration()
 {
@@ -174,8 +176,10 @@ std::optional<Eigen::Vector2d> ImuService::pollLinearAcceleration()
         return std::nullopt;
     }
 
-    std::optional<int16_t> opt_x = readAndCombineByteData(ACCEL_X_LEAST_SIG_REG, ACCEL_X_MOST_SIG_REG);
-    std::optional<int16_t> opt_y = readAndCombineByteData(ACCEL_Y_LEAST_SIG_REG, ACCEL_Y_MOST_SIG_REG);
+    std::optional<int16_t> opt_x =
+        readAndCombineByteData(ACCEL_X_LEAST_SIG_REG, ACCEL_X_MOST_SIG_REG);
+    std::optional<int16_t> opt_y =
+        readAndCombineByteData(ACCEL_Y_LEAST_SIG_REG, ACCEL_Y_MOST_SIG_REG);
 
     if (!opt_x.has_value() || !opt_y.has_value())
     {
@@ -185,13 +189,11 @@ std::optional<Eigen::Vector2d> ImuService::pollLinearAcceleration()
     int16_t raw_x = opt_x.value();
     int16_t raw_y = opt_y.value();
 
-    double a_x = (static_cast<double>(raw_x) / SHRT_MAX) 
-                 * ACCELEROMETER_FULL_SCALE_G 
-                 * ACCELERATION_DUE_TO_GRAVITY_METERS_PER_SECOND_SQUARED;
-                 
-    double a_y = (static_cast<double>(raw_y) / SHRT_MAX) 
-                 * ACCELEROMETER_FULL_SCALE_G 
-                 * ACCELERATION_DUE_TO_GRAVITY_METERS_PER_SECOND_SQUARED;
-                 
+    double a_x = (static_cast<double>(raw_x) / SHRT_MAX) * ACCELEROMETER_FULL_SCALE_G *
+                 ACCELERATION_DUE_TO_GRAVITY_METERS_PER_SECOND_SQUARED;
+
+    double a_y = (static_cast<double>(raw_y) / SHRT_MAX) * ACCELEROMETER_FULL_SCALE_G *
+                 ACCELERATION_DUE_TO_GRAVITY_METERS_PER_SECOND_SQUARED;
+
     return Eigen::Vector2d(a_x, a_y);
 }
diff --git a/src/software/embedded/services/imu.h b/src/software/embedded/services/imu.h
index f3c49515e7..c1373d8053 100644
--- a/src/software/embedded/services/imu.h
+++ b/src/software/embedded/services/imu.h
@@ -1,9 +1,9 @@
 #pragma once
 
 #include <Eigen/Dense>
-#include <utility>
-#include <numbers>
 #include <climits>
+#include <numbers>
+#include <utility>
 
 #include "proto/tbots_software_msgs.pb.h"
 #include "software/geom/angular_velocity.h"
@@ -13,72 +13,71 @@
  */
 class ImuService
 {
-    public:
-		/**
-     	* Constructs and initializes a new IMU service object.
-     	*
-     	* If successfully initialized, will try to do a simple calibration of the IMU.
-     	*/
-		ImuService();
-		/*
-		 * Polls the latest IMU reading of the angular velocity of the robot on the z axis
-		 * @return the current angular velocty of the robot on the z axis
-		 */
-		std::optional<AngularVelocity> pollHeadingVelocity();
-		/*
-		 * Polls the latest IMU reading of the linear acceleration of the robot on the z plane 
-		 * @return the current linear acceleration of the robot on the z plane		 
-		 */
-		std::optional<Eigen::Vector2d> pollLinearAcceleration();
-
-
-		// Variance from datasheet (in rad^2/s^2)
-		static constexpr double IMU_VARIANCE = 
-			(4.0 * 14.4222 / 1000.0 * std::numbers::pi / 180.0) *
-			(4.0 * 14.4222 / 1000.0 * std::numbers::pi / 180.0);
-
-	private:
-
-		/*
-		 * Reads byte data from two registers, and combine them into a single value 
-		 * @parama ls_reg register of the least significant register
-		 * @parama ms_reg register of the most significant register	
-		 * @return the combined integer value of the two registers		 
-		 */
-
-		std::optional<int16_t> readAndCombineByteData(uint8_t ls_reg, uint8_t ms_reg);
-
-		bool initialized_=false;
-
-		int file_descriptor_=0;
-
-		double degrees_error_;
-
-		// Maps the maximum raw reading from 16-bit integer to be 2 times gravity	
-		static constexpr double ACCELEROMETER_FULL_SCALE_G = 2.0;
-		// Same for gyroscope, 1000 degrees per second 
-		static constexpr double IMU_FULL_SCALE_DPS=1000.0;
-		
-		/// Various i2c registers.
-		static const uint8_t WHOAMI_REG        = 0xf;
-    	static const uint8_t ACCEL_CONTROL_REG = 0x10;
-    	static const uint8_t GYRO_CONTROL_REG  = 0x11;
-    	static const uint8_t CTRL4_C           = 0x13;
-    	static const uint8_t CTRL6_C           = 0x15;
-    	static const uint8_t CTRL8_XL          = 0x17;
-
-		// Device path for the IMU
-	    inline static const std::string IMU_DEVICE = "/dev/i2c-1";
-
-		// Gyroscope Z-axis (Yaw) Output Data Registers
-	    static constexpr uint8_t HEADING_LEAST_SIG_REG = 0x26; // OUTZ_L_G
-	    static constexpr uint8_t HEADING_MOST_SIG_REG  = 0x27; // OUTZ_H_G
-	
-	    // Accelerometer X-axis Output Data Registers
-	    static constexpr uint8_t ACCEL_X_LEAST_SIG_REG = 0x28; // OUTX_L_XL
-	    static constexpr uint8_t ACCEL_X_MOST_SIG_REG  = 0x29; // OUTX_H_XL
-	
-	    // Accelerometer Y-axis Output Data Registers
-	    static constexpr uint8_t ACCEL_Y_LEAST_SIG_REG = 0x2A; // OUTY_L_XL
-	    static constexpr uint8_t ACCEL_Y_MOST_SIG_REG  = 0x2B; // OUTY_H_XL
+   public:
+    /**
+     * Constructs and initializes a new IMU service object.
+     *
+     * If successfully initialized, will try to do a simple calibration of the IMU.
+     */
+    ImuService();
+    /*
+     * Polls the latest IMU reading of the angular velocity of the robot on the z axis
+     * @return the current angular velocty of the robot on the z axis
+     */
+    std::optional<AngularVelocity> pollHeadingVelocity();
+    /*
+     * Polls the latest IMU reading of the linear acceleration of the robot on the z plane
+     * @return the current linear acceleration of the robot on the z plane
+     */
+    std::optional<Eigen::Vector2d> pollLinearAcceleration();
+
+
+    // Variance from datasheet (in rad^2/s^2)
+    static constexpr double IMU_VARIANCE =
+        (4.0 * 14.4222 / 1000.0 * std::numbers::pi / 180.0) *
+        (4.0 * 14.4222 / 1000.0 * std::numbers::pi / 180.0);
+
+   private:
+    /*
+     * Reads byte data from two registers, and combine them into a single value
+     * @parama ls_reg register of the least significant register
+     * @parama ms_reg register of the most significant register
+     * @return the combined integer value of the two registers
+     */
+
+    std::optional<int16_t> readAndCombineByteData(uint8_t ls_reg, uint8_t ms_reg);
+
+    bool initialized_ = false;
+
+    int file_descriptor_ = 0;
+
+    double degrees_error_;
+
+    // Maps the maximum raw reading from 16-bit integer to be 2 times gravity
+    static constexpr double ACCELEROMETER_FULL_SCALE_G = 2.0;
+    // Same for gyroscope, 1000 degrees per second
+    static constexpr double IMU_FULL_SCALE_DPS = 1000.0;
+
+    /// Various i2c registers.
+    static const uint8_t WHOAMI_REG        = 0xf;
+    static const uint8_t ACCEL_CONTROL_REG = 0x10;
+    static const uint8_t GYRO_CONTROL_REG  = 0x11;
+    static const uint8_t CTRL4_C           = 0x13;
+    static const uint8_t CTRL6_C           = 0x15;
+    static const uint8_t CTRL8_XL          = 0x17;
+
+    // Device path for the IMU
+    inline static const std::string IMU_DEVICE = "/dev/i2c-1";
+
+    // Gyroscope Z-axis (Yaw) Output Data Registers
+    static constexpr uint8_t HEADING_LEAST_SIG_REG = 0x26;  // OUTZ_L_G
+    static constexpr uint8_t HEADING_MOST_SIG_REG  = 0x27;  // OUTZ_H_G
+
+    // Accelerometer X-axis Output Data Registers
+    static constexpr uint8_t ACCEL_X_LEAST_SIG_REG = 0x28;  // OUTX_L_XL
+    static constexpr uint8_t ACCEL_X_MOST_SIG_REG  = 0x29;  // OUTX_H_XL
+
+    // Accelerometer Y-axis Output Data Registers
+    static constexpr uint8_t ACCEL_Y_LEAST_SIG_REG = 0x2A;  // OUTY_L_XL
+    static constexpr uint8_t ACCEL_Y_MOST_SIG_REG  = 0x2B;  // OUTY_H_XL
 };
diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp
index 652833a3f0..fafaf5432f 100644
--- a/src/software/embedded/thunderloop.cpp
+++ b/src/software/embedded/thunderloop.cpp
@@ -9,8 +9,8 @@
 #include "proto/tbots_software_msgs.pb.h"
 #include "shared/constants.h"
 #include "software/embedded/primitive_executor.h"
-#include "software/embedded/services/motor.h"
 #include "software/embedded/services/imu.h"
+#include "software/embedded/services/motor.h"
 #include "software/logger/logger.h"
 #include "software/logger/network_logger.h"
 #include "software/networking/tbots_network_exception.h"
@@ -120,8 +120,8 @@ Thunderloop::Thunderloop(const robot_constants::RobotConstants& robot_constants,
         << "THUNDERLOOP: Power Service initialized! Next initializing Motor Service";
 
     motor_service_  = std::make_unique<MotorService>(robot_constants, loop_hz);
-	g_motor_service = motor_service_.get();
-	motor_service_->setup();
+    g_motor_service = motor_service_.get();
+    motor_service_->setup();
 
     LOG(INFO) << "THUNDERLOOP: Motor Service initialized! Next initializing IMU Service";
 
@@ -182,14 +182,14 @@ void Thunderloop::runLoop()
     robot_status_.set_thunderloop_version(thunderloop_hash);
     robot_status_.set_thunderloop_date_flashed(thunderloop_date_flashed);
 
-	
-	std::optional<AngularVelocity> imu_poll = imu_service_->pollHeadingVelocity();
-	
-	//TODO: Replace with actual IMU data usage
-	if (imu_poll.has_value()){
 
-		LOG(INFO) << "IMU Heading Velocity" << imu_poll.value() ;
-	}
+    std::optional<AngularVelocity> imu_poll = imu_service_->pollHeadingVelocity();
+
+    // TODO: Replace with actual IMU data usage
+    if (imu_poll.has_value())
+    {
+        LOG(INFO) << "IMU Heading Velocity" << imu_poll.value();
+    }
 
     for (;;)
     {
diff --git a/src/software/embedded/thunderloop.h b/src/software/embedded/thunderloop.h
index b3da73b76c..e553a78a54 100644
--- a/src/software/embedded/thunderloop.h
+++ b/src/software/embedded/thunderloop.h
@@ -10,10 +10,10 @@
 #include "shared/constants.h"
 #include "shared/robot_constants.h"
 #include "software/embedded/primitive_executor.h"
+#include "software/embedded/services/imu.h"
 #include "software/embedded/services/motor.h"
 #include "software/embedded/services/network/network.h"
 #include "software/embedded/services/power.h"
-#include "software/embedded/services/imu.h"
 #include "software/embedded/toml_config/toml_config_client.h"
 #include "software/logger/logger.h"