diff --git a/wired_module/wired_module_arduino/include/BarometerSensor.h b/wired_module/wired_module_arduino/include/BarometerSensor.h
index d19e311e..abbe43d4 100644
--- a/wired_module/wired_module_arduino/include/BarometerSensor.h
+++ b/wired_module/wired_module_arduino/include/BarometerSensor.h
@@ -1,21 +1,14 @@
-#include "I2cSensorBase.h"
+#pragma once
 
-#ifndef BAROMETER_SENSOR
-#define BAROMETER_SENSOR
+#include "I2cSensorBase.h"
 
 class BarometerSensor : public I2cSensorBase {
    public:
-    // Attributes
-    const int scaleFactor = 524288;
-    int16_t c0;
-    int16_t c1;
+    BarometerSensor(i2c_port_t masterPortNum, uint8_t sensorAddress, uint8_t sensorID);
 
-    // Use parent constructor
-    using I2cSensorBase::I2cSensorBase;
-
-    // Methods
-    void configure() override;
     void read() override;
-};
 
-#endif
\ No newline at end of file
+   private:
+    int16_t c0;
+    int16_t c1;
+};
\ No newline at end of file
diff --git a/wired_module/wired_module_arduino/include/I2cMaster.h b/wired_module/wired_module_arduino/include/I2cMaster.h
index 40d51694..fb42dbcb 100644
--- a/wired_module/wired_module_arduino/include/I2cMaster.h
+++ b/wired_module/wired_module_arduino/include/I2cMaster.h
@@ -1,18 +1,11 @@
-#include "driver/i2c.h"
+#pragma once
 
-#ifndef I2C_MASTER
-#define I2C_MASTER
+#include <driver/i2c.h>
 
 class I2cMaster {
    public:
-    // Attributes
     i2c_port_t portNum;
 
-    // Constructor
     I2cMaster(i2c_port_t portNum, int sda, int scl, int clockFrequency);
-
-    // Destructor
     ~I2cMaster();
-};
-
-#endif
\ No newline at end of file
+};
\ No newline at end of file
diff --git a/wired_module/wired_module_arduino/include/I2cSensorBase.h b/wired_module/wired_module_arduino/include/I2cSensorBase.h
index e1ce480f..444d8d5f 100644
--- a/wired_module/wired_module_arduino/include/I2cSensorBase.h
+++ b/wired_module/wired_module_arduino/include/I2cSensorBase.h
@@ -1,22 +1,21 @@
+#pragma once
+
 #include "SensorBase.h"
-#include "driver/i2c.h"
+#include "constants.h"
 
-#ifndef I2C_BASE
-#define I2C_BASE
+#include <driver/i2c.h>
 
 class I2cSensorBase : public SensorBase {
    public:
-    // Attributes
-    uint8_t sensorAddress;
-    i2c_port_t masterPortNum;
-    uint8_t readBuffer[8];
-
-    // Constructor
     explicit I2cSensorBase(i2c_port_t masterPortNum, uint8_t sensorAddress, uint8_t sensorID);
 
-    // Methods
     void read_sensor_register(uint8_t registerAddress, size_t readLength, int timeout);
     void write_sensor_register(uint8_t registerAddress, uint8_t data, int timeout);
-};
 
-#endif
\ No newline at end of file
+   protected:
+    uint8_t readBuffer[I2CConfig::READ_BUFFER_SIZE];
+
+   private:
+    uint8_t sensorAddress;
+    i2c_port_t masterPortNum;
+};
\ No newline at end of file
diff --git a/wired_module/wired_module_arduino/include/MpuSensor.h b/wired_module/wired_module_arduino/include/MpuSensor.h
index 6ba98b54..0bac09f4 100644
--- a/wired_module/wired_module_arduino/include/MpuSensor.h
+++ b/wired_module/wired_module_arduino/include/MpuSensor.h
@@ -1,16 +1,10 @@
-#include "I2cSensorBase.h"
+#pragma once
 
-#ifndef MPU_SENSOR
-#define MPU_SENSOR
+#include "I2cSensorBase.h"
 
 class MpuSensor : public I2cSensorBase {
    public:
-    // Use parent constructor
-    using I2cSensorBase::I2cSensorBase;
+    explicit MpuSensor(i2c_port_t masterPortNum, uint8_t sensorAddress, uint8_t sensorID);
 
-    // Methods
-    void configure() override;
     void read() override;
-};
-
-#endif
\ No newline at end of file
+};
\ No newline at end of file
diff --git a/wired_module/wired_module_arduino/include/SensorBase.h b/wired_module/wired_module_arduino/include/SensorBase.h
index c3550e5d..1d899390 100644
--- a/wired_module/wired_module_arduino/include/SensorBase.h
+++ b/wired_module/wired_module_arduino/include/SensorBase.h
@@ -1,26 +1,19 @@
-#include <CAN.h>
+#pragma once
+
+#include "constants.h"
 
-#ifndef SENSOR_BASE
-#define SENSOR_BASE
+#include <CAN.h>
 
-// Abstract Class
 class SensorBase {
    public:
-    // Attributes
-    uint8_t canBuffer[4];
-    uint8_t sensorID;
+    explicit SensorBase(uint8_t sensorID);
 
-    // Abstract methods
-    virtual void configure() = 0;
     virtual void read() = 0;
+    void send();
 
-    // Methods
-    void send() {
-        CAN.beginPacket(this->sensorID);
-        this->read();                                         // Read data from sensor and store in buffer
-        CAN.write(this->canBuffer, sizeof(this->canBuffer));  // Send packet
-        CAN.endPacket();
-    }
-};
+   protected:
+    uint8_t canBuffer[CANConfig::PACKET_SIZE];
 
-#endif
\ No newline at end of file
+   private:
+    uint8_t sensorID;
+};
\ No newline at end of file
diff --git a/wired_module/wired_module_arduino/include/constants.h b/wired_module/wired_module_arduino/include/constants.h
new file mode 100644
index 00000000..ae4f6a6d
--- /dev/null
+++ b/wired_module/wired_module_arduino/include/constants.h
@@ -0,0 +1,29 @@
+#pragma once
+
+#include <driver/i2c.h>
+
+namespace CANConfig {
+    const int RX_PIN = 16;
+    const int TX_PIN = 17;
+    const double BAUD_RATE = 500E3;
+    const int PACKET_SIZE = 4;
+}
+
+namespace I2CConfig {
+    const i2c_port_t NUM = I2C_NUM_0;
+    const int SCL = GPIO_NUM_22;
+    const int SDA = GPIO_NUM_21;
+    const int FREQ_HZ = 400000;
+    const int READ_BUFFER_SIZE = 8;
+}
+
+namespace MPUConfig {
+    const int ID = 0x13;
+    const int ADDR = 0x68;
+}
+
+namespace BarometerConfig {
+    const int ID = 0x11;
+    const int ADDR = 0x76;
+    const int SCALE_FACTOR = 524288;
+}
diff --git a/wired_module/wired_module_arduino/include/utils.h b/wired_module/wired_module_arduino/include/utils.h
new file mode 100644
index 00000000..7410c924
--- /dev/null
+++ b/wired_module/wired_module_arduino/include/utils.h
@@ -0,0 +1,3 @@
+#pragma once
+
+void onReceive(int packetSize);
\ No newline at end of file
diff --git a/wired_module/wired_module_arduino/src/MpuSensor.cpp b/wired_module/wired_module_arduino/src/MpuSensor.cpp
deleted file mode 100644
index 8b99a15f..00000000
--- a/wired_module/wired_module_arduino/src/MpuSensor.cpp
+++ /dev/null
@@ -1,24 +0,0 @@
-#include "MpuSensor.h"
-
-#include <cstring>
-
-void MpuSensor::configure() {
-    write_sensor_register(0x6B, 0, 2000);
-    write_sensor_register(0x19, 7, 2000);
-}
-
-void MpuSensor ::read() {
-    // Get raw data from sensor
-    read_sensor_register(0x3B, 6, 2000);
-
-    int16_t RAWX = (this->readBuffer[0] << 8) | this->readBuffer[1];
-    int16_t RAWY = (this->readBuffer[2] << 8) | this->readBuffer[3];
-    int16_t RAWZ = (this->readBuffer[4] << 8) | this->readBuffer[5];
-
-    float xg = (float)RAWX / 16384;
-    float yg = (float)RAWY / 16384;
-    float zg = (float)RAWZ / 16384;
-
-    // Write to CAN buffer
-    memcpy(this->canBuffer, &zg, sizeof(zg));
-}
diff --git a/wired_module/wired_module_arduino/src/main.cpp b/wired_module/wired_module_arduino/src/main.cpp
index b9293fa7..f3a7eaaa 100644
--- a/wired_module/wired_module_arduino/src/main.cpp
+++ b/wired_module/wired_module_arduino/src/main.cpp
@@ -1,73 +1,42 @@
-#include <BarometerSensor.h>
+#include "BarometerSensor.h"
+#include "I2cMaster.h"
+#include "MpuSensor.h"
+#include "utils.h"
+
 #include <CAN.h>
-#include <I2cMaster.h>
-#include <MpuSensor.h>
+#include <driver/i2c.h>
 
 #include <vector>
 
-#include "driver/i2c.h"
-
-#define I2C_NUM I2C_NUM_0
-#define I2C_SCL GPIO_NUM_22
-#define I2C_SDA GPIO_NUM_21
-
-// Function prototypes
-static esp_err_t i2c_master_init(void);
-void onReceive(int packetSize);
-void setup();
-void loop();
-
 // ==================================================================
 // SETUP SENSORS HERE
 // ==================================================================
-I2cMaster i2cMaster(I2C_NUM, I2C_SDA, I2C_SCL, 400000);
-MpuSensor mpuSensor(i2cMaster.portNum, 0x68, 0x13);
-BarometerSensor barometerSensor(i2cMaster.portNum, 0x76, 0x11);
-std::vector<SensorBase*> sensors = {&mpuSensor, &barometerSensor};
-// ==================================================================
-
-void onReceive(int packetSize) {
-    // Get all bytes from packet
-    uint8_t buffer[4];
-    int i = 0;
-    while (CAN.available()) {
-        buffer[i] = CAN.read();
-        i++;
-    };
+I2cMaster i2cMaster(I2CConfig::NUM, I2CConfig::SDA, I2CConfig::SCL, I2CConfig::FREQ_HZ);
 
-    // Convert bytes to float
-    float data;
-    memcpy(&data, buffer, sizeof(float));
+MpuSensor mpuSensor(i2cMaster.portNum, MPUConfig::ADDR, MPUConfig::ID);
+BarometerSensor barometerSensor(i2cMaster.portNum, BarometerConfig::ADDR, BarometerConfig::ID);
 
-    Serial.printf("Msg received | ID = 0x%lx | Data = %f\n", CAN.packetId(), data);
-}
+std::vector<SensorBase *> sensors = {&mpuSensor, &barometerSensor};
+// ==================================================================
 
 void setup() {
     // Setup CAN bus
-    CAN.setPins(16, 17);
+    CAN.setPins(CANConfig::RX_PIN, CANConfig::TX_PIN);
     Serial.begin(115200);
     while (!Serial);
 
-    // Start the CAN bus at 500 kbps
-    if (!CAN.begin(500E3)) {
+    // Start CAN bus
+    if (!CAN.begin(CANConfig::BAUD_RATE)) {
         Serial.println("\n\nStarting CAN failed!\n\n");
         while (1);
     }
 
-    // Configure can bus to loopback mode for self test
+    // DELETE WHEN CONNECTED TO RASP PI (used for self testing)
     CAN.loopback();
-
-    // Set up receive callback, for self testing
     CAN.onReceive(onReceive);
-
-    // Configure all sensors
-    for (auto sensor : sensors) {
-        sensor->configure();
-    }
 }
 
 void loop() {
-    // Send CAN bus from all sensors
     for (auto sensor : sensors) {
         sensor->send();
     }
diff --git a/wired_module/wired_module_arduino/src/BarometerSensor.cpp b/wired_module/wired_module_arduino/src/sensors/BarometerSensor.cpp
similarity index 61%
rename from wired_module/wired_module_arduino/src/BarometerSensor.cpp
rename to wired_module/wired_module_arduino/src/sensors/BarometerSensor.cpp
index a5beaad0..dcfed8bf 100644
--- a/wired_module/wired_module_arduino/src/BarometerSensor.cpp
+++ b/wired_module/wired_module_arduino/src/sensors/BarometerSensor.cpp
@@ -1,9 +1,14 @@
 #include "BarometerSensor.h"
+#include "constants.h"
 
 #include <cstring>
 
-void BarometerSensor::configure() {
+// TODO: Add comments and remove magic numbers
+
+BarometerSensor::BarometerSensor(i2c_port_t masterPortNum, uint8_t sensorAddress, uint8_t sensorID)
+    : I2cSensorBase(masterPortNum, sensorAddress, sensorID) {
     read_sensor_register(0x10, 3, 1000);
+    
     this->c0 = this->readBuffer[0] << 4 | this->readBuffer[1] >> 4;
     if (this->c0 & (1 << 11))
         this->c0 = this->c0 | 0XF000;
@@ -14,20 +19,16 @@ void BarometerSensor::configure() {
         this->c1 = this->c1 | 0XF000;
 }
 
-void BarometerSensor ::read() {
-    // Get raw data from sensor
+void BarometerSensor::read() {
     read_sensor_register(0x03, 3, 1000);
 
     int32_t raw_temp = (this->readBuffer[0] << 8) | this->readBuffer[1];
     raw_temp = (raw_temp << 8) | this->readBuffer[2];
 
-    if (raw_temp & (1 << 23)) {
-        raw_temp = raw_temp | 0XFF000000;
-    }
+    if (raw_temp & (1 << 23)) {raw_temp = raw_temp | 0XFF000000;}
 
-    float temp = (float)raw_temp / (float)this->scaleFactor;
+    float temp = (float)raw_temp / (float)BarometerConfig::SCALE_FACTOR;
     temp = this->c0 * 0.5 + this->c1 * temp;
 
-    // Write to CAN buffer
     memcpy(this->canBuffer, &temp, sizeof(temp));
 }
diff --git a/wired_module/wired_module_arduino/src/I2cMaster.cpp b/wired_module/wired_module_arduino/src/sensors/I2cMaster.cpp
similarity index 92%
rename from wired_module/wired_module_arduino/src/I2cMaster.cpp
rename to wired_module/wired_module_arduino/src/sensors/I2cMaster.cpp
index 678cf531..f121694d 100644
--- a/wired_module/wired_module_arduino/src/I2cMaster.cpp
+++ b/wired_module/wired_module_arduino/src/sensors/I2cMaster.cpp
@@ -1,7 +1,7 @@
 #include "I2cMaster.h"
 
-I2cMaster::I2cMaster(i2c_port_t portNum, int sda, int scl, int clockFrequency) {
-    this->portNum = portNum;
+I2cMaster::I2cMaster(i2c_port_t portNum, int sda, int scl, int clockFrequency)
+    : portNum(portNum) {
     i2c_config_t masterConfig = {
         .mode = I2C_MODE_MASTER,
         .sda_io_num = sda,
diff --git a/wired_module/wired_module_arduino/src/I2cSensorBase.cpp b/wired_module/wired_module_arduino/src/sensors/I2cSensorBase.cpp
similarity index 75%
rename from wired_module/wired_module_arduino/src/I2cSensorBase.cpp
rename to wired_module/wired_module_arduino/src/sensors/I2cSensorBase.cpp
index fb4a9207..0550642d 100644
--- a/wired_module/wired_module_arduino/src/I2cSensorBase.cpp
+++ b/wired_module/wired_module_arduino/src/sensors/I2cSensorBase.cpp
@@ -1,10 +1,7 @@
 #include "I2cSensorBase.h"
 
-I2cSensorBase::I2cSensorBase(i2c_port_t masterPortNum, uint8_t sensorAddress, uint8_t sensorID) {
-    this->masterPortNum = masterPortNum;
-    this->sensorAddress = sensorAddress;
-    this->sensorID = sensorID;
-}
+I2cSensorBase::I2cSensorBase(i2c_port_t masterPortNum, uint8_t sensorAddress, uint8_t sensorID)
+    : SensorBase(sensorID), sensorAddress(sensorAddress), masterPortNum(masterPortNum) {}
 
 void I2cSensorBase::read_sensor_register(uint8_t registerAddress, size_t readLength, int timeout) {
     i2c_master_write_read_device(this->masterPortNum, this->sensorAddress, &registerAddress,
@@ -12,7 +9,7 @@ void I2cSensorBase::read_sensor_register(uint8_t registerAddress, size_t readLen
 }
 
 void I2cSensorBase::write_sensor_register(uint8_t registerAddress, uint8_t data, int timeout) {
-    uint8_t writeBuf[2];  // writeBuf[len+1];
+    uint8_t writeBuf[2];
     writeBuf[0] = registerAddress;
     writeBuf[1] = data;
     i2c_master_write_to_device(this->masterPortNum, this->sensorAddress, writeBuf, 2, timeout);
diff --git a/wired_module/wired_module_arduino/src/sensors/MpuSensor.cpp b/wired_module/wired_module_arduino/src/sensors/MpuSensor.cpp
new file mode 100644
index 00000000..5cc9b592
--- /dev/null
+++ b/wired_module/wired_module_arduino/src/sensors/MpuSensor.cpp
@@ -0,0 +1,26 @@
+#include "MpuSensor.h"
+
+#include <cstring>
+
+// TODO: Add comments and remove magic numbers
+
+MpuSensor::MpuSensor(i2c_port_t masterPortNum, uint8_t sensorAddress, uint8_t sensorID)
+	: I2cSensorBase(masterPortNum, sensorAddress, sensorID) {
+	write_sensor_register(0x6B, 0, 2000);
+	write_sensor_register(0x19, 7, 2000);
+}
+
+void MpuSensor::read() {
+	read_sensor_register(0x3B, 6, 2000);
+
+	int16_t RAWX = (this->readBuffer[0] << 8) | this->readBuffer[1];
+	int16_t RAWY = (this->readBuffer[2] << 8) | this->readBuffer[3];
+	int16_t RAWZ = (this->readBuffer[4] << 8) | this->readBuffer[5];
+
+	float xg = (float)RAWX / 16384;
+	float yg = (float)RAWY / 16384;
+	float zg = (float)RAWZ / 16384;
+
+	// Only send the z-axis data for now
+	memcpy(this->canBuffer, &zg, sizeof(zg));
+}
diff --git a/wired_module/wired_module_arduino/src/sensors/SensorBase.cpp b/wired_module/wired_module_arduino/src/sensors/SensorBase.cpp
new file mode 100644
index 00000000..611063c1
--- /dev/null
+++ b/wired_module/wired_module_arduino/src/sensors/SensorBase.cpp
@@ -0,0 +1,11 @@
+#include "SensorBase.h"
+
+SensorBase::SensorBase(uint8_t sensorID) : sensorID(sensorID) {}
+
+void SensorBase::send() {
+    CAN.beginPacket(this->sensorID);
+    this->read();
+    CAN.write(this->canBuffer, sizeof(this->canBuffer));
+    CAN.endPacket();
+}
+
diff --git a/wired_module/wired_module_arduino/src/utils.cpp b/wired_module/wired_module_arduino/src/utils.cpp
new file mode 100644
index 00000000..bed97efa
--- /dev/null
+++ b/wired_module/wired_module_arduino/src/utils.cpp
@@ -0,0 +1,23 @@
+#include "utils.h"
+
+#include <CAN.h>
+
+#include <cstring>
+
+// Callback function for CAN bus. Should be configured and called by rasp pi after removing loopback mode
+void onReceive(int packetSize) {
+    uint8_t buffer[4];
+    float data;
+
+    // Get all bytes from packet
+    int i = 0;
+    while (CAN.available()) {
+        buffer[i] = CAN.read();
+        i++;
+    };
+
+    // Convert bytes to float
+    memcpy(&data, buffer, sizeof(float));
+
+    Serial.printf("Msg received | ID = 0x%lx | Data = %f\n", CAN.packetId(), data);
+}
\ No newline at end of file