diff --git a/.github/workflows/frontend_CI.yml b/.github/workflows/frontend_CI.yml
new file mode 100644
index 0000000..027a2b2
--- /dev/null
+++ b/.github/workflows/frontend_CI.yml
@@ -0,0 +1,40 @@
+name: Frontend CI
+
+on:
+  push:
+    branches:
+      - '**'
+    paths:
+      - 'frontend/**'
+      - '.github/workflows/frontend_CI.yml'
+  pull_request:
+    branches:
+      - '**'
+    paths:
+      - 'frontend/**'
+      - '.github/workflows/frontend_CI.yml'
+
+jobs:
+  typecheck-and-build:
+    runs-on: ubuntu-latest
+    defaults:
+      run:
+        working-directory: frontend
+
+    steps:
+      - uses: actions/checkout@v4
+
+      - uses: actions/setup-node@v4
+        with:
+          node-version: '20'
+          cache: 'npm'
+          cache-dependency-path: frontend/package-lock.json
+
+      - name: Install dependencies
+        run: npm ci
+
+      - name: Type check
+        run: npm run typecheck
+
+      - name: Build
+        run: npm run build
diff --git a/.github/workflows/motor_microcontroller_CI.yml b/.github/workflows/motor_microcontroller_CI.yml
new file mode 100644
index 0000000..de37c49
--- /dev/null
+++ b/.github/workflows/motor_microcontroller_CI.yml
@@ -0,0 +1,52 @@
+name: Motor Microcontroller CI
+
+on:
+  workflow_dispatch:
+  push:
+    branches:
+      - "**"
+    paths:
+      - "firmware/motor_microcontroller/**"
+  pull_request:
+    branches:
+      - "**"
+    paths:
+      - "firmware/motor_microcontroller/**"
+
+jobs:
+  build:
+    runs-on: ubuntu-latest
+    if: github.event_name != 'pull_request' || github.event.pull_request.head.repo.full_name != github.event.pull_request.base.repo.full_name
+
+    steps:
+      - uses: actions/checkout@v4
+      - uses: actions/cache@v4
+        with:
+          path: |
+            ~/.cache/pip
+            ~/.platformio/.cache
+          key: ${{ runner.os }}-pio
+      - uses: actions/setup-python@v5
+        with:
+          python-version: "3.11"
+      - name: Install PlatformIO Core
+        run: |
+          curl -fsSL -o get-platformio.py https://raw.githubusercontent.com/platformio/platformio-core-installer/master/get-platformio.py
+          python3 get-platformio.py
+          mkdir -p /usr/local/bin
+          ln -s ~/.platformio/penv/bin/platformio /usr/local/bin/platformio
+          ln -s ~/.platformio/penv/bin/pio /usr/local/bin/pio
+          ln -s ~/.platformio/penv/bin/piodebuggdb /usr/local/bin/piodebuggdb
+      - name: Copy custom ROS interfaces
+        run: |
+          mkdir -p firmware/motor_microcontroller/extra_packages
+          cp -r src/autogiro_interfaces firmware/motor_microcontroller/extra_packages/
+      - name: Build PlatformIO Project
+        run: pio run -e pico -e ROS
+        working-directory: firmware/motor_microcontroller
+      - name: Upload firmware
+        uses: actions/upload-artifact@v4
+        with:
+          name: motor_firmware.uf2
+          path: firmware/motor_microcontroller/.pio/build/ROS/firmware.uf2
+          if-no-files-found: warn
diff --git a/.github/workflows/sensor_microcontroller_CI.yml b/.github/workflows/sensor_microcontroller_CI.yml
new file mode 100644
index 0000000..7e5cb86
--- /dev/null
+++ b/.github/workflows/sensor_microcontroller_CI.yml
@@ -0,0 +1,52 @@
+name: Sensor Microcontroller CI
+
+on:
+  workflow_dispatch:
+  push:
+    branches:
+      - "**"
+    paths:
+      - "firmware/sensor_microcontroller/**"
+  pull_request:
+    branches:
+      - "**"
+    paths:
+      - "firmware/sensor_microcontroller/**"
+
+jobs:
+  build:
+    runs-on: ubuntu-latest
+    if: github.event_name != 'pull_request' || github.event.pull_request.head.repo.full_name != github.event.pull_request.base.repo.full_name
+
+    steps:
+      - uses: actions/checkout@v4
+      - uses: actions/cache@v4
+        with:
+          path: |
+            ~/.cache/pip
+            ~/.platformio/.cache
+          key: ${{ runner.os }}-pio
+      - uses: actions/setup-python@v5
+        with:
+          python-version: "3.11"
+      - name: Install PlatformIO Core
+        run: |
+          curl -fsSL -o get-platformio.py https://raw.githubusercontent.com/platformio/platformio-core-installer/master/get-platformio.py
+          python3 get-platformio.py
+          mkdir -p /usr/local/bin
+          ln -s ~/.platformio/penv/bin/platformio /usr/local/bin/platformio
+          ln -s ~/.platformio/penv/bin/pio /usr/local/bin/pio
+          ln -s ~/.platformio/penv/bin/piodebuggdb /usr/local/bin/piodebuggdb
+      - name: Copy custom ROS interfaces
+        run: |
+          mkdir -p firmware/sensor_microcontroller/extra_packages
+          cp -r src/autogiro_interfaces firmware/sensor_microcontroller/extra_packages/
+      - name: Build PlatformIO Project
+        run: pio run -e regular -e ROS
+        working-directory: firmware/sensor_microcontroller
+      - name: Upload firmware
+        uses: actions/upload-artifact@v4
+        with:
+          name: sensor_firmware.uf2
+          path: firmware/sensor_microcontroller/.pio/build/ROS/firmware.uf2
+          if-no-files-found: warn
diff --git a/firmware/motor_microcontroller/.gitignore b/firmware/motor_microcontroller/.gitignore
new file mode 100644
index 0000000..be306bf
--- /dev/null
+++ b/firmware/motor_microcontroller/.gitignore
@@ -0,0 +1,8 @@
+.pio
+.vscode/.browse.c_cpp.db*
+.vscode/c_cpp_properties.json
+.vscode/launch.json
+.vscode/ipch
+.idea
+.vscode
+.DS_Store
\ No newline at end of file
diff --git a/firmware/motor_microcontroller/README.md b/firmware/motor_microcontroller/README.md
new file mode 100644
index 0000000..90036c7
--- /dev/null
+++ b/firmware/motor_microcontroller/README.md
@@ -0,0 +1,5 @@
+[![PlatformIO CI](https://github.com/WheelchairSeniorDesign/MotorMicrocontroller/actions/workflows/Platformio_CI.yml/badge.svg)](https://github.com/WheelchairSeniorDesign/MotorMicrocontroller/actions/workflows/Platformio_CI.yml)
+
+# MotorMicrocontroller
+
+Brake, Enable, Direction (per each motor), DAC clock, DAC data
diff --git a/firmware/motor_microcontroller/include/BatteryFunctions.h b/firmware/motor_microcontroller/include/BatteryFunctions.h
new file mode 100644
index 0000000..a7a4ab5
--- /dev/null
+++ b/firmware/motor_microcontroller/include/BatteryFunctions.h
@@ -0,0 +1,27 @@
+//
+// Created by Laxman on 4/9/25.
+//
+
+#ifndef BATTERYFUNCTIONS_H
+#define BATTERYFUNCTIONS_H
+
+#include <Arduino.h>
+#include "hardware/adc.h"
+
+// ADC pin and configuration
+#define ADC_PIN 28
+#define VREF 3.3
+#define ADC_MAX 4095.0
+#define OPAMP_GAIN 10.1
+#define VOLTAGE_DIVIDER_RATIO 0.01205
+
+// Battery voltage range
+#define BATTERY_VOLTAGE_MAX 27.0
+#define BATTERY_VOLTAGE_MIN 21.0
+
+void initBatterySensor();
+float readBatteryVoltage();
+int8_t calculateBatteryPercentage(float battery_voltage);
+
+#endif // BATTERYFUNCTIONS_H
+
diff --git a/firmware/motor_microcontroller/include/README b/firmware/motor_microcontroller/include/README
new file mode 100644
index 0000000..194dcd4
--- /dev/null
+++ b/firmware/motor_microcontroller/include/README
@@ -0,0 +1,39 @@
+
+This directory is intended for project header files.
+
+A header file is a file containing C declarations and macro definitions
+to be shared between several project source files. You request the use of a
+header file in your project source file (C, C++, etc) located in `src` folder
+by including it, with the C preprocessing directive `#include'.
+
+```src/main.c
+
+#include "header.h"
+
+int main (void)
+{
+ ...
+}
+```
+
+Including a header file produces the same results as copying the header file
+into each source file that needs it. Such copying would be time-consuming
+and error-prone. With a header file, the related declarations appear
+in only one place. If they need to be changed, they can be changed in one
+place, and programs that include the header file will automatically use the
+new version when next recompiled. The header file eliminates the labor of
+finding and changing all the copies as well as the risk that a failure to
+find one copy will result in inconsistencies within a program.
+
+In C, the usual convention is to give header files names that end with `.h'.
+It is most portable to use only letters, digits, dashes, and underscores in
+header file names, and at most one dot.
+
+Read more about using header files in official GCC documentation:
+
+* Include Syntax
+* Include Operation
+* Once-Only Headers
+* Computed Includes
+
+https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html
diff --git a/firmware/motor_microcontroller/include/RefSpeed.h b/firmware/motor_microcontroller/include/RefSpeed.h
new file mode 100644
index 0000000..3f5be43
--- /dev/null
+++ b/firmware/motor_microcontroller/include/RefSpeed.h
@@ -0,0 +1,19 @@
+//
+// Created by Robbie on 12/8/24.
+//
+
+#ifndef MOTORMICROCONTROLLER_REFSPEED_H
+#define MOTORMICROCONTROLLER_REFSPEED_H
+
+#include <Arduino.h>
+
+/**
+ * Struct representing the reference speed and direction.
+ */
+struct refSpeed {
+    int8_t leftSpeed{};      ///< Speed of the left wheel.
+    int8_t rightSpeed{};     ///< Speed of the right wheel.
+};
+
+
+#endif //MOTORMICROCONTROLLER_REFSPEED_H
diff --git a/firmware/motor_microcontroller/include/globals.h b/firmware/motor_microcontroller/include/globals.h
new file mode 100644
index 0000000..a86ab91
--- /dev/null
+++ b/firmware/motor_microcontroller/include/globals.h
@@ -0,0 +1,11 @@
+//
+// Created by Robbie on 5/14/25.
+//
+
+#ifndef GLOBALS_H
+#define GLOBALS_H
+
+extern float speedR;
+extern float speedL;
+
+#endif //GLOBALS_H
diff --git a/firmware/motor_microcontroller/include/microRosFunctions.h b/firmware/motor_microcontroller/include/microRosFunctions.h
new file mode 100644
index 0000000..37aa17e
--- /dev/null
+++ b/firmware/motor_microcontroller/include/microRosFunctions.h
@@ -0,0 +1,36 @@
+//
+// Created by leslier on 12/8/2024.
+//
+
+#ifndef MOTORMICROCONTROLLER_MICROROSFUNCTIONS_H
+#define MOTORMICROCONTROLLER_MICROROSFUNCTIONS_H
+
+#include <Arduino.h>
+#include "RefSpeed.h"
+
+extern bool eBrake;
+
+
+bool create_entities();
+
+void destroy_entities();
+
+void microRosTick();
+
+/**
+ * Transmits the message over ROS. Not in use right now
+ * @param /
+ */
+//void transmitMsg();
+
+
+/**
+ * Function to get the reference speed from the ROS topic.
+ * @return The reference speed using the refSpeed struct.
+ */
+refSpeed getRefSpeed();
+
+
+void transmitDac(int16_t leftDacValue, int16_t rightDacValue);
+
+#endif //MOTORMICROCONTROLLER_MICROROSFUNCTIONS_H
diff --git a/firmware/motor_microcontroller/lib/README b/firmware/motor_microcontroller/lib/README
new file mode 100644
index 0000000..2593a33
--- /dev/null
+++ b/firmware/motor_microcontroller/lib/README
@@ -0,0 +1,46 @@
+
+This directory is intended for project specific (private) libraries.
+PlatformIO will compile them to static libraries and link into executable file.
+
+The source code of each library should be placed in an own separate directory
+("lib/your_library_name/[here are source files]").
+
+For example, see a structure of the following two libraries `Foo` and `Bar`:
+
+|--lib
+|  |
+|  |--Bar
+|  |  |--docs
+|  |  |--examples
+|  |  |--src
+|  |     |- Bar.c
+|  |     |- Bar.h
+|  |  |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html
+|  |
+|  |--Foo
+|  |  |- Foo.c
+|  |  |- Foo.h
+|  |
+|  |- README --> THIS FILE
+|
+|- platformio.ini
+|--src
+   |- main.c
+
+and a contents of `src/main.c`:
+```
+#include <Foo.h>
+#include <Bar.h>
+
+int main (void)
+{
+  ...
+}
+
+```
+
+PlatformIO Library Dependency Finder will find automatically dependent
+libraries scanning project source files.
+
+More information about PlatformIO Library Dependency Finder
+- https://docs.platformio.org/page/librarymanager/ldf.html
diff --git a/firmware/motor_microcontroller/platformio.ini b/firmware/motor_microcontroller/platformio.ini
new file mode 100644
index 0000000..6cf87a8
--- /dev/null
+++ b/firmware/motor_microcontroller/platformio.ini
@@ -0,0 +1,42 @@
+; PlatformIO Project Configuration File
+;
+;   Build options: build flags, source filter
+;   Upload options: custom upload port, speed and extra flags
+;   Library options: dependencies, extra library storages
+;   Advanced options: extra scripting
+;
+; Please visit documentation for the other options and examples
+; https://docs.platformio.org/page/projectconf.html
+
+[env:pico]
+platform = raspberrypi
+board = pico
+framework = arduino
+lib_deps = 
+	adafruit/Adafruit MCP4725@^2.0.2
+
+[env:ROS]
+platform = raspberrypi
+board = pico
+framework = arduino
+lib_deps = 
+	adafruit/Adafruit MCP4725@^2.0.2
+	https://github.com/micro-ROS/micro_ros_platformio
+build_flags =
+	-D ROS
+
+board_microros_distro = humble
+board_microros_transport = serial
+
+[env:ROS-debug]
+platform = raspberrypi
+board = pico
+framework = arduino
+lib_deps = 
+	adafruit/Adafruit MCP4725@^2.0.2
+	https://github.com/micro-ROS/micro_ros_platformio
+
+build_flags = 
+	-D ROS_DEBUG
+board_microros_distro = humble
+board_microros_transport = serial
diff --git a/firmware/motor_microcontroller/src/BatteryFuctions.cpp b/firmware/motor_microcontroller/src/BatteryFuctions.cpp
new file mode 100644
index 0000000..f7ec7d8
--- /dev/null
+++ b/firmware/motor_microcontroller/src/BatteryFuctions.cpp
@@ -0,0 +1,31 @@
+#include "BatteryFunctions.h"
+
+void initBatterySensor()
+{
+    adc_init();
+    adc_gpio_init(ADC_PIN);
+    adc_select_input(2);
+}
+
+float readBatteryVoltage()
+{
+    uint16_t raw_adc = adc_read();
+    float adc_voltage = (raw_adc * VREF) / ADC_MAX;
+    float scaled_voltage = adc_voltage / VOLTAGE_DIVIDER_RATIO;
+    float battery_voltage = scaled_voltage / OPAMP_GAIN;
+    return battery_voltage;
+}
+
+int8_t calculateBatteryPercentage(float battery_voltage)
+{
+    float percent = ((battery_voltage - BATTERY_VOLTAGE_MIN) /
+                     (BATTERY_VOLTAGE_MAX - BATTERY_VOLTAGE_MIN)) *
+                    100.0;
+
+    if (percent > 100.0)
+        percent = 100.0;
+    if (percent < 0.0)
+        percent = 0.0;
+
+    return static_cast<int8_t>(percent);
+}
diff --git a/firmware/motor_microcontroller/src/main.cpp b/firmware/motor_microcontroller/src/main.cpp
new file mode 100644
index 0000000..08180b9
--- /dev/null
+++ b/firmware/motor_microcontroller/src/main.cpp
@@ -0,0 +1,242 @@
+/* Eren Tekbas for ECE Senior Design 2025
+MicroController Handling Reference Speed
+This code is for the microcontroller to retrieve the target speed from the onboard
+computer and send it to the motor controller after adjusting it.
+It will also read the speed of the motor and send it to the onboard computer.
+*/
+//
+
+#include <Arduino.h>
+#include <Adafruit_MCP4725.h>
+#include "RefSpeed.h"
+#include "BatteryFunctions.h"
+#include "globals.h"
+
+#if defined(ROS) || defined(ROS_DEBUG)
+#include <micro_ros_platformio.h>
+#include "microRosFunctions.h"
+
+#endif
+
+Adafruit_MCP4725 dacA;
+Adafruit_MCP4725 dacB;
+// For Adafruit MCP4725A1 the address is 0x62 (default) or 0x63 (ADDR pin tied to VCC)
+
+int dacClockPin = 5; // GPIO number for these variables
+int brakePin = 10;
+int directionLPin = 12;
+int directionRPin = 13;
+int enablePin = 11;
+int speedPin = 4;
+int motorSpeedPin = 22;
+int batteryPin = 28;
+int speedFreqRPin = 15;
+int speedFreqLPin = 14;
+
+// 4095 is max
+float motorMaxSpeed = 1000;
+
+// variables to be used in the code
+bool brake;        // brake for motor controller
+bool directionL;   // direction for motor controller LEFT
+bool directionR;   // direction for motor controller RIGHT
+bool enable;       // enable for motor controller
+int motorSpeed;    // value read from the motor speed sensor
+int16_t refSpeedR; // value sent to the motor controller for speed of right motor
+int16_t refSpeedL; // value sent to the motor controller for speed of left motor
+int16_t batteryValue;
+refSpeed refSpeedSensors;
+
+// //variables to handle frequecy reading and tranfer to speed
+volatile uint32_t pulse_count_1 = 0;
+volatile uint32_t pulse_count_2 = 0;
+
+void pulse_handler_1() { pulse_count_1++; }
+void pulse_handler_2() { pulse_count_2++; }
+
+float freqR;
+float freqL;
+float speedR;
+float speedL;
+unsigned long freqSampleStart = 0;
+const unsigned long freqSampleDuration = 1000; // in ms
+
+bool measuringFreq = false;
+
+void setup()
+{
+    Serial.begin(115200); // start I2C communication protocol
+
+    while (!Serial)
+    {
+        delay(10); // wait for serial
+    }
+
+    delay(2000);
+
+#if defined(ROS) || defined(ROS_DEBUG)
+    set_microros_serial_transports(Serial);
+    delay(2000);
+#endif
+
+    // initiate ADC for battery level reading testing
+    initBatterySensor(); // BATTERY: initialize ADC hardware
+
+    // initiate the DACs
+    while (!dacA.begin(0x62))
+    {
+        Serial.println("DAC A not found");
+        delay(500);
+    }
+    while (!dacB.begin(0x63))
+    {
+        Serial.println("DAC B not found");
+        delay(500);
+    }
+    // pinMode(dacClockPin,OUTPUT); // set the pins to be used as output
+    // pinMode(speedPin,OUTPUT);
+    pinMode(batteryPin, INPUT);
+    pinMode(directionLPin, OUTPUT);
+    pinMode(directionRPin, OUTPUT);
+    pinMode(brakePin, OUTPUT);
+    // pinMode(refSpeedPin,INPUT); // set the pins to be used as input
+    pinMode(motorSpeedPin, INPUT);
+
+    brake = false;
+    enable = true;
+
+    pinMode(speedFreqRPin, INPUT_PULLDOWN);
+    pinMode(speedFreqLPin, INPUT_PULLDOWN);
+    attachInterrupt(digitalPinToInterrupt(speedFreqRPin), pulse_handler_1, RISING);
+    attachInterrupt(digitalPinToInterrupt(speedFreqLPin), pulse_handler_2, RISING);
+}
+
+void getFreq()
+{
+    if (!measuringFreq)
+    {
+        // Start measuring
+        pulse_count_1 = 0;
+        pulse_count_2 = 0;
+        freqSampleStart = millis();
+        measuringFreq = true;
+    }
+
+    if (millis() - freqSampleStart >= freqSampleDuration)
+    {
+        // Finish measuring
+        freqR = (pulse_count_1 * 1000.0) / freqSampleDuration;
+        freqL = (pulse_count_2 * 1000.0) / freqSampleDuration;
+        measuringFreq = false;
+    }
+}
+
+// conversion of frequency to MPH
+void freqToSpeed()
+{
+    speedR = (freqR * 10 / 21.33) * 3.14 * 12.5 * 60 / 63360;
+    speedL = (freqL * 10 / 21.33) * 3.14 * 12.5 * 60 / 63360;
+}
+
+void loop()
+{
+
+#if defined(ROS) || defined(ROS_DEBUG)
+    microRosTick();
+    refSpeedSensors = getRefSpeed();
+
+#endif
+
+    // enable = true; // enable the motor controller
+    /*joystickSpeed{} = digitalRead(refSpeedPin); // read the reference speed from the onboard computer
+    speedR = joystickSpeed.speedR;
+    speedL = joystickSpeed.speedL;
+    */
+    // this part will be discussed with the sensors team
+    /*
+    directionR = true; // initially forward direction
+    directionL = true; // initially forward direction
+    brake = false; // initially no brake
+    if(speedR == 0 && speedL == 0){ // if the reference speed is 0, stop the motor
+      brake = true; // activate the brake if joystick outputs 0 in both directions
+    }
+
+    else{
+      brake = false; // deactivate the brake if joystick outputs a value other than 0
+      if(speedR<0){
+        directionR = false; // set the direction to right if the joystick outputs a negative value
+        refSpeedR = speedR*-1; // set the reference speed to the absolute value of the joystick output
+      }
+      else if(speedL<0){
+        directionL = false; // set the direction to left if the joystick outputs a positive value
+        refSpeedL = speedL*-1; // set the reference speed to the absolute value of the joystick output
+      }
+      else{
+        refSpeedR = speedR; // set the referrence speed to the joystick output
+        refSpeedL = speedL;
+      }
+
+      //refSpeedR = refSpeedR*4095/100; // adjust the reference speed Right to the motor controller
+      //refSpeedL = refSpeedL*4095/100; // adjust the reference speed Left to the motor controller
+
+    }
+    */
+
+    enable = false; // enable the motor controller
+    if (refSpeedSensors.rightSpeed == 0 && refSpeedSensors.leftSpeed == 0)
+    {
+        // brake = false; //brake if speeds are 0
+        enable = true;
+    }
+    else
+    {
+        brake = true; // disable brake if speeds are not 0
+    }
+    // add break if emergency button is pushed
+
+    if (refSpeedSensors.rightSpeed > 0)
+    {
+        directionR = false;
+    }
+    else
+    {
+        directionR = true;
+    }
+
+    if (refSpeedSensors.leftSpeed > 0)
+    {
+        directionL = false;
+    }
+    else
+    {
+        directionL = true;
+    }
+
+    float tempRefSpeedR = abs(refSpeedSensors.rightSpeed) * motorMaxSpeed / 100;
+    float tempRefSpeedL = abs(refSpeedSensors.leftSpeed) * motorMaxSpeed / 100;
+
+    refSpeedR = static_cast<int16_t>(tempRefSpeedR);
+    refSpeedL = static_cast<int16_t>(tempRefSpeedL);
+
+#if defined(ROS) || defined(ROS_DEBUG)
+    // Adding the ebrake. The brake variable is flipped, so false = brake on
+    if (eBrake)
+    {
+        brake = false;
+    }
+    transmitDac(refSpeedL, refSpeedR);
+#endif
+
+    digitalWrite(directionLPin, directionL);
+    digitalWrite(directionRPin, directionR);
+    digitalWrite(enablePin, enable);
+    digitalWrite(brakePin, brake);
+    dacB.setVoltage(refSpeedR, false);
+    dacA.setVoltage(refSpeedL, false);
+    getFreq();
+    freqToSpeed();
+
+#ifdef ROS_DEBUG
+    transmitDac(refSpeedL, refSpeedR);
+#endif
+}
diff --git a/firmware/motor_microcontroller/src/microRosFunctions.cpp b/firmware/motor_microcontroller/src/microRosFunctions.cpp
new file mode 100644
index 0000000..00e58ca
--- /dev/null
+++ b/firmware/motor_microcontroller/src/microRosFunctions.cpp
@@ -0,0 +1,287 @@
+#if defined(ROS) || defined(ROS_DEBUG)
+#include "microRosFunctions.h"
+#include "BatteryFunctions.h"
+#include "globals.h"
+#include <micro_ros_platformio.h>
+#include <autogiro_interfaces/msg/ref_speed.h>
+#include <autogiro_interfaces/msg/brake.h>
+#include <autogiro_interfaces/msg/battery.h> // BATTERY
+#include <autogiro_interfaces/msg/motors.h>  // Added: for sending wheel speed in MPH
+#include <autogiro_interfaces/msg/dac_values.h>
+
+#include <rcl/rcl.h>
+#include <rcl/error_handling.h>
+#include <rclc/rclc.h>
+#include <rclc/executor.h>
+
+
+bool eBrake = false;
+
+
+// ROS messages
+#ifdef ROS_DEBUG
+
+#include <autogiro_interfaces/msg/sensors.h>
+#endif
+
+// Executor
+rclc_executor_t executor;
+
+// Subscriber
+rcl_subscription_t subscriber;
+autogiro_interfaces__msg__RefSpeed refSpeedMsg;
+
+rcl_subscription_t brake_subscriber;
+autogiro_interfaces__msg__Brake brakeMsg;
+
+// BATTERY
+rcl_publisher_t batteryPublisher;
+rcl_timer_t batteryTimer;
+autogiro_interfaces__msg__Battery batteryMsg;
+
+
+// MOTOR MPH
+rcl_publisher_t motorPublisher;
+rcl_timer_t motorTimer;
+autogiro_interfaces__msg__Motors motorMsg;
+
+
+rcl_publisher_t dacPublisher;
+autogiro_interfaces__msg__DacValues dacMsg;
+
+
+rclc_support_t support;
+rcl_allocator_t allocator;
+rcl_node_t node;
+rcl_timer_t timer;
+
+enum states {
+  WAITING_AGENT,
+  AGENT_AVAILABLE,
+  AGENT_CONNECTED,
+  AGENT_DISCONNECTED
+} state;
+
+
+#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){error_loop();}}
+#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){} }
+
+#define EXECUTE_EVERY_N_MS(MS, X)  do { \
+  static volatile int64_t init = -1; \
+  if (init == -1) { init = uxr_millis();} \
+  if (uxr_millis() - init > MS) { X; init = uxr_millis();} \
+} while (0)\
+
+void error_loop() {
+    while(1) {
+        delay(100);
+    }
+}
+
+
+void timer_callback(rcl_timer_t * inputTimer, int64_t last_call_time) {
+    RCLC_UNUSED(last_call_time);
+    if (inputTimer != NULL) {
+        RCSOFTCHECK(rcl_publish(&dacPublisher, &dacMsg, NULL));
+    }
+}
+
+void battery_timer_callback(rcl_timer_t *input_timer, int64_t last_call_time) {
+    RCLC_UNUSED(last_call_time);
+    if (input_timer != NULL) {
+        float voltage = readBatteryVoltage();
+        int8_t percent = calculateBatteryPercentage(voltage);
+        batteryMsg.battery_percent = percent;
+        RCSOFTCHECK(rcl_publish(&batteryPublisher, &batteryMsg, NULL));
+    }
+}
+
+//  Directly publish refSpeed in mph no conversio needed
+void motor_timer_callback(rcl_timer_t *input_timer, int64_t last_call_time) {
+    RCLC_UNUSED(last_call_time);
+    if (input_timer != NULL) {
+        motorMsg.left_mph = static_cast<int8_t>(speedL);
+        motorMsg.right_mph = static_cast<int8_t>(speedR);
+        RCSOFTCHECK(rcl_publish(&motorPublisher, &motorMsg, NULL));
+    }
+}
+
+void transmitDac(int16_t leftDacValue, int16_t rightDacValue) {
+
+    dacMsg.left_dac = leftDacValue;
+    dacMsg.right_dac = rightDacValue;
+    //RCSOFTCHECK(rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100)));
+
+}
+
+
+void subscription_callback(const void *msgin)
+{
+    const autogiro_interfaces__msg__RefSpeed *msg = (const autogiro_interfaces__msg__RefSpeed *)msgin;
+    refSpeedMsg = *msg;
+#ifdef ROS_DEBUG
+
+    //Try using Serial1 to use a Uart adapter to print this out
+//    Serial1.print("Left Speed: ");
+//    Serial1.println(msg->left_speed);
+//    Serial1.print("Right Speed: ");
+//    Serial1.println(msg->right_speed);
+
+
+    // if (msg->left_speed == 100 || msg->right_speed == 100) {
+    //     digitalWrite(LED_BUILTIN, HIGH);
+    // } else {
+    //     digitalWrite(LED_BUILTIN, LOW);
+    // }
+
+#elif ROS
+
+#endif
+}
+
+
+void brake_subscription_callback(const void *msgin)
+{
+    const autogiro_interfaces__msg__Brake *msg = (const autogiro_interfaces__msg__Brake *)msgin;
+    eBrake = msg->brake;
+}
+
+bool create_entities(){
+    allocator = rcl_get_default_allocator();
+
+    // create init_options
+  RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));
+
+  // create node
+  RCCHECK(rclc_node_init_default(&node, "motor_node", "", &support));
+
+  RCCHECK(rclc_subscription_init_best_effort(
+            &subscriber,
+            &node,
+            ROSIDL_GET_MSG_TYPE_SUPPORT(autogiro_interfaces, msg, RefSpeed),
+            "ref_speed"));
+
+  // Create Brake Subscriber
+    RCCHECK(rclc_subscription_init_best_effort(
+            &brake_subscriber,
+            &node,
+            ROSIDL_GET_MSG_TYPE_SUPPORT(autogiro_interfaces, msg, Brake),
+            "ebrake"));
+
+
+    RCCHECK(rclc_publisher_init_default(
+        &batteryPublisher,
+        &node,
+        ROSIDL_GET_MSG_TYPE_SUPPORT(autogiro_interfaces, msg, Battery),
+        "battery_status"));
+
+    RCCHECK(rclc_timer_init_default(
+        &batteryTimer,
+        &support,
+        RCL_MS_TO_NS(3000),
+        battery_timer_callback));
+
+
+    //  Init motor speed publisher and timer
+    RCCHECK(rclc_publisher_init_default(
+        &motorPublisher,
+        &node,
+        ROSIDL_GET_MSG_TYPE_SUPPORT(autogiro_interfaces, msg, Motors),
+        "motor_speed"));
+
+    RCCHECK(rclc_timer_init_default(
+        &motorTimer,
+        &support,
+        RCL_MS_TO_NS(1000),
+        motor_timer_callback));
+
+    RCCHECK(rclc_publisher_init_best_effort(
+        &dacPublisher,
+        &node,
+        ROSIDL_GET_MSG_TYPE_SUPPORT(autogiro_interfaces, msg, DacValues),
+        "dac_value"));
+
+    RCCHECK(rclc_timer_init_default(
+        &timer,
+        &support,
+        RCL_MS_TO_NS(500),
+        timer_callback));
+
+    //Number of handles = # timers + # subscriptions + # clients + # services
+    executor = rclc_executor_get_zero_initialized_executor();
+    RCCHECK(rclc_executor_init(&executor, &support.context, 5, &allocator));
+
+    // add sub to executor
+    RCCHECK(rclc_executor_add_subscription(&executor, &subscriber, &refSpeedMsg, &subscription_callback, ON_NEW_DATA));
+
+    // Add brake sub to executor
+    RCCHECK(rclc_executor_add_subscription(&executor, &brake_subscriber, &brakeMsg, &brake_subscription_callback, ON_NEW_DATA));
+
+    RCCHECK(rclc_executor_add_timer(&executor, &batteryTimer)); // BATTERY: add battery timer to executor
+
+   // Add motor timer to executor
+    RCCHECK(rclc_executor_add_timer(&executor, &motorTimer));
+
+    RCCHECK(rclc_executor_add_timer(&executor, &timer));
+
+    state = WAITING_AGENT;
+
+    return true;
+}
+
+void destroy_entities() {
+    rmw_context_t *rmw_context = rcl_context_get_rmw_context(&support.context);
+    (void) rmw_uros_set_context_entity_destroy_session_timeout(rmw_context, 0);
+
+    RCCHECK(rcl_subscription_fini(&subscriber, &node));
+    RCCHECK(rcl_subscription_fini(&brake_subscriber, &node));
+    RCCHECK(rcl_publisher_fini(&motorPublisher, &node));
+    RCCHECK(rcl_publisher_fini(&batteryPublisher, &node));
+    RCCHECK(rcl_publisher_fini(&dacPublisher, &node));
+    RCCHECK(rcl_timer_fini(&batteryTimer));
+    RCCHECK(rcl_timer_fini(&motorTimer));
+    RCCHECK(rcl_timer_fini(&timer));
+    RCCHECK(rclc_executor_fini(&executor));
+    RCCHECK(rcl_node_fini(&node));
+    RCCHECK(rclc_support_fini(&support));
+
+}
+
+void microRosTick(){
+    switch (state) {
+        case WAITING_AGENT:
+            EXECUTE_EVERY_N_MS(500,
+                               state = (RMW_RET_OK == rmw_uros_ping_agent(100, 1)) ? AGENT_AVAILABLE : WAITING_AGENT;);
+            break;
+        case AGENT_AVAILABLE:
+            state = (true == create_entities()) ? AGENT_CONNECTED : WAITING_AGENT;
+            if (state == WAITING_AGENT) {
+                destroy_entities();
+            };
+            break;
+        case AGENT_CONNECTED:
+            EXECUTE_EVERY_N_MS(200, state = (RMW_RET_OK == rmw_uros_ping_agent(100, 1)) ? AGENT_CONNECTED
+                                                                                        : AGENT_DISCONNECTED;);
+            if (state == AGENT_CONNECTED) {
+                rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100));
+            }
+            break;
+        case AGENT_DISCONNECTED:
+            destroy_entities();
+            state = WAITING_AGENT;
+            break;
+        default:
+            break;
+    }
+}
+
+
+refSpeed getRefSpeed() {
+    refSpeed refSpeed;
+    refSpeed.leftSpeed = refSpeedMsg.left_speed;
+    refSpeed.rightSpeed = refSpeedMsg.right_speed;
+    return refSpeed;
+}
+
+#endif
+
diff --git a/firmware/motor_microcontroller/test/README b/firmware/motor_microcontroller/test/README
new file mode 100644
index 0000000..9b1e87b
--- /dev/null
+++ b/firmware/motor_microcontroller/test/README
@@ -0,0 +1,11 @@
+
+This directory is intended for PlatformIO Test Runner and project tests.
+
+Unit Testing is a software testing method by which individual units of
+source code, sets of one or more MCU program modules together with associated
+control data, usage procedures, and operating procedures, are tested to
+determine whether they are fit for use. Unit testing finds problems early
+in the development cycle.
+
+More information about PlatformIO Unit Testing:
+- https://docs.platformio.org/en/latest/advanced/unit-testing/index.html
diff --git a/firmware/sensor_microcontroller/.gitignore b/firmware/sensor_microcontroller/.gitignore
new file mode 100644
index 0000000..9b111bd
--- /dev/null
+++ b/firmware/sensor_microcontroller/.gitignore
@@ -0,0 +1,41 @@
+.idea
+.vscode
+cmake-*
+.DS_Store
+build
+
+# Format documentation: https://kicad.org/help/file-formats/
+
+# Temporary files
+*.000
+*.bak
+*.bck
+*.kicad_pcb-bak
+*.kicad_sch-bak
+*-backups
+*.kicad_prl
+*.sch-bak
+*~
+_autosave-*
+*.tmp
+*-save.pro
+*-save.kicad_pcb
+fp-info-cache
+
+# Netlist files (exported from Eeschema)
+*.net
+
+# Autorouter files (exported from Pcbnew)
+*.dsn
+*.ses
+
+# Exported BOM files
+*.xml
+*.csv
+
+PlatformIO files
+venv
+build
+.DS_Store
+.pio
+
diff --git a/firmware/sensor_microcontroller/README.md b/firmware/sensor_microcontroller/README.md
new file mode 100644
index 0000000..23d9f29
--- /dev/null
+++ b/firmware/sensor_microcontroller/README.md
@@ -0,0 +1,24 @@
+[![PlatformIO CI](https://github.com/WheelchairSeniorDesign/SensorMicrocontroller/actions/workflows/Platformio_CI.yml/badge.svg)](https://github.com/WheelchairSeniorDesign/SensorMicrocontroller/actions/workflows/Platformio_CI.yml)
+
+# About
+this repo is for all the code related to the microcontroller system for our wheelchair.
+
+# Contributing Guidelines
+
+### Development Environent Setup
+1. Install a C/C++ IDE  which supports PlatformIO (e.g. [Visual Studio Code](https://code.visualstudio.com/), [CLion](https://www.jetbrains.com/clion/)).
+2. Install [PlatformIO](https://platformio.org/install/) as an extension in your IDE.
+3. Open any project folder in your IDE.
+4. Open the PlatformIO Home from the PlatformIO extension in your IDE.
+5. Build the project using the PlatformIO build button.
+6. Upload the project to the Rapspberry Pi Pico using the PlatformIO upload button.
+7. Open the serial monitor to view the output of the Pico. (If using Clion, don't use the upload and monitor button, instead use the Clion Serial Monitor Plugin)
+8. Make changes to the code and test them on the Pico.
+9. If you are going to add new libraries to the project, add them to the `platformio.ini` file.
+10. Make sure that you develop your changes in a separate branch in Git
+
+### Code Style
+Please put all `.c/cpp` files in the src directory and the corrisponding `.h` files in the include directory. Please break out subsystems into new files to keep the main file clean. Also please leave function descriptions above your function declarations.
+
+### Pull Request Guidelines
+Please write a brief decription of what your code changed, and why you made those changes. Set yourself as the assignee, and assign someone on the team as the reviewer. Do not merge your code until at least 1 person on the team has reviewed it and given approval.
diff --git a/firmware/sensor_microcontroller/include/ADCFunctions.h b/firmware/sensor_microcontroller/include/ADCFunctions.h
new file mode 100644
index 0000000..e895a9c
--- /dev/null
+++ b/firmware/sensor_microcontroller/include/ADCFunctions.h
@@ -0,0 +1,24 @@
+//
+// Created by Robbie on 11/14/24.
+//
+
+#ifndef JOYSTICKIMUDEMO_ADCFUNCTIONS_H
+#define JOYSTICKIMUDEMO_ADCFUNCTIONS_H
+
+#include <Arduino.h>
+#include <Adafruit_ADS1X15.h>
+
+/**
+ * Initializes the ADC with the given parameters
+ * @param adc - An instance of the Adafruit_ADS1115 class for the ADC
+ * @param i2c_addr - The I2C address of the ADC in hex
+ */
+bool adcInit(Adafruit_ADS1115 &adc, uint8_t i2c_addr);
+
+/***
+ * Prints the ADC data to the serial monitor
+ * @param adc - An instance of the Adafruit_ADS1115 class for the ADC
+ */
+void printADC(Adafruit_ADS1115 &adc);
+
+#endif //JOYSTICKIMUDEMO_ADCFUNCTIONS_H
diff --git a/firmware/sensor_microcontroller/include/FanFunctions.h b/firmware/sensor_microcontroller/include/FanFunctions.h
new file mode 100644
index 0000000..2373f1c
--- /dev/null
+++ b/firmware/sensor_microcontroller/include/FanFunctions.h
@@ -0,0 +1,57 @@
+//
+// Created by Robbie on 2/9/25.
+//
+
+#ifndef MICROCONTROLLERCODE_FANFUNCTIONS_H
+#define MICROCONTROLLERCODE_FANFUNCTIONS_H
+
+#include "Arduino.h"
+
+
+struct FanSpeeds{
+    int16_t fan_speed_0;
+    int16_t fan_speed_1;
+    int16_t fan_speed_2;
+    int16_t fan_speed_3;
+};
+
+struct FanDutyCycles{
+    int8_t fan_0_duty_cycle;
+    int8_t fan_1_duty_cycle;
+    int8_t fan_2_duty_cycle;
+    int8_t fan_3_duty_cycle;
+};
+
+
+#define FAN_0 16
+#define TACH_0 17
+#define FAN_1 18
+#define TACH_1 19
+#define FAN_2 20
+#define TACH_2 21
+#define FAN_3 22
+#define TACH_3 26
+
+
+
+//TODO add fuctions to read individual and all fan speeds
+
+void setFanIndividual(uint8_t fan, uint8_t dutyCycle);
+
+void setAllFans(FanDutyCycles dutyCycles);
+
+FanSpeeds getAllFanSpeeds();
+
+void setupRPMCounter();
+
+static void handleTach0Interrupt();
+
+static void handleTach1Interrupt();
+
+static void handleTach2Interrupt();
+
+static void handleTach3Interrupt();
+
+uint32_t getRPM();
+
+#endif //MICROCONTROLLERCODE_FANFUNCTIONS_H
diff --git a/firmware/sensor_microcontroller/include/FingerprintFunctions.h b/firmware/sensor_microcontroller/include/FingerprintFunctions.h
new file mode 100644
index 0000000..8b9a67f
--- /dev/null
+++ b/firmware/sensor_microcontroller/include/FingerprintFunctions.h
@@ -0,0 +1,16 @@
+//
+// Functions created by Nam
+//
+
+#ifndef FINGERPRINTFUNCTIONS_H
+#define FINGERPRINTFUNCTIONS_H
+
+#include <Arduino.h>
+
+uint8_t getFingerprintID();
+
+bool setupFingerprint();
+
+void loopFingerprint();
+
+#endif //FINGERPRINTFUNCTIONS_H
diff --git a/firmware/sensor_microcontroller/include/IMUFunctions.h b/firmware/sensor_microcontroller/include/IMUFunctions.h
new file mode 100644
index 0000000..ce3f22b
--- /dev/null
+++ b/firmware/sensor_microcontroller/include/IMUFunctions.h
@@ -0,0 +1,45 @@
+//
+// Created by Robbie on 11/13/24.
+//
+
+#ifndef JOYSTICKIMUDEMO_IMUFUNCTIONS_H
+#define JOYSTICKIMUDEMO_IMUFUNCTIONS_H
+
+#include <Arduino.h>
+#include <Adafruit_ICM20948.h>
+
+struct IMUData {
+    float accel_x;
+    float accel_y;
+    float accel_z;
+    float gyro_x;
+    float gyro_y;
+    float gyro_z;
+    float mag_x;
+    float mag_y;
+    float mag_z;
+};
+
+/**
+ * Initializes the IMU with the given parameters
+ * @param icm - An instance of the Adafruit_ICM20948 class for the IMU
+ * @param accelRang - The range of the accelerometer using the icm20948_accel_range_t enum
+ * @param gyroRang  - The range of the gyroscope using the icm20948_gyro_range_t enum
+ * @param magDataRate - The data rate of the magnetometer using the ak09916_data_rate_t enum
+ */
+bool imuInit(Adafruit_ICM20948 &icm, icm20948_accel_range_t accelRang, icm20948_gyro_range_t gyroRang, ak09916_data_rate_t magDataRate);
+
+/**
+ * Prints the IMU data to the serial monitor
+ * @param icm - An instance of the Adafruit_ICM20948 class for the IMU
+ */
+void printImuData(Adafruit_ICM20948 &icm);
+
+/**
+ * Returns a IMUData struct with updated values from the IMU.
+ * @param icm - An instance of the Adafruit_ICM20948 class for the IMU
+ * @return Returns an IMUData struct with the values from the IMU at call time.
+ */
+IMUData getIMUData(Adafruit_ICM20948 &icm);
+
+#endif //JOYSTICKIMUDEMO_IMUFUNCTIONS_H
diff --git a/firmware/sensor_microcontroller/include/JoystickFunctions.h b/firmware/sensor_microcontroller/include/JoystickFunctions.h
new file mode 100644
index 0000000..96e895e
--- /dev/null
+++ b/firmware/sensor_microcontroller/include/JoystickFunctions.h
@@ -0,0 +1,51 @@
+//
+// Created by Robbie on 11/18/24.
+//
+
+#ifndef JOYSTICK_JOYSTICKFUNCTIONS_H
+#define JOYSTICK_JOYSTICKFUNCTIONS_H
+
+#include <Adafruit_ADS1X15.h>
+
+/**
+ * Struct representing the reference speed and direction.
+ */
+struct RefSpeed {
+    int8_t leftSpeed;      ///< Speed of the left wheel.
+    int8_t rightSpeed;     ///< Speed of the right wheel.
+};
+
+/**
+ * Struct representing the reference displace
+ */
+struct RefDisplacement {
+    int16_t longDisp;      ///< Forward/Backward displacement with + indicating forward
+    int16_t latDisp;     ///< Side to side displacement with + indicating right
+};
+
+/**
+ * Reads a value from the joystick connected to the ADC and returns the reference speeds
+ * @param adc An instance of the adc the joystick is connected to
+ * @return A RefSpeed for the wheelchair containing the wheel speeds and the direction
+ */
+RefSpeed joystickToSpeed(Adafruit_ADS1115 &adc);
+
+/**
+ * Reads a value from the joystick connected to the ADC and returns the reference displacement
+ * @param adc An instance of the adc the joystick is connected to
+ * @return A RefDisplacement for the joystick
+ */
+RefDisplacement joystickToDisplacement(Adafruit_ADS1115 &adc);
+
+template <typename T>
+/**
+ * Custom clamp function to keep a value between to values
+ * @tparam T The type of the value to clamp.
+ * @param value The value to clamp.
+ * @param min The minimum value.
+ * @param max The maximum value.
+ * @return Returns the clamped value.
+ */
+T clamp(T value, T min, T max);
+
+#endif //JOYSTICK_JOYSTICKFUNCTIONS_H
diff --git a/firmware/sensor_microcontroller/include/LidarFunctions.h b/firmware/sensor_microcontroller/include/LidarFunctions.h
new file mode 100644
index 0000000..9360611
--- /dev/null
+++ b/firmware/sensor_microcontroller/include/LidarFunctions.h
@@ -0,0 +1,16 @@
+//
+// Created by Robbie on 4/9/25.
+//
+
+#ifndef MICROCONTROLLERCODE_LIDARFUNCTIONS_H
+#define MICROCONTROLLERCODE_LIDARFUNCTIONS_H
+
+#include <Arduino.h>
+
+#define LIDAR_PIN 7
+
+void setupLidar();
+
+void lidarState(boolean status);
+
+#endif //MICROCONTROLLERCODE_LIDARFUNCTIONS_H
diff --git a/firmware/sensor_microcontroller/include/LightFunctions.h b/firmware/sensor_microcontroller/include/LightFunctions.h
new file mode 100644
index 0000000..d2b2685
--- /dev/null
+++ b/firmware/sensor_microcontroller/include/LightFunctions.h
@@ -0,0 +1,15 @@
+//
+// Created by aec117-fkmj9r3 on 3/11/25.
+//
+
+#ifndef LIGHTFUNCTIONS_H
+#define LIGHTFUNCTIONS_H
+
+#define LIGHT_PIN_1 14
+#define LIGHT_PIN_2 15
+
+void setupLight();
+
+void setLight(int lightState);
+
+#endif //LIGHTFUNCTIONS_H
diff --git a/firmware/sensor_microcontroller/include/PIRFunctions.h b/firmware/sensor_microcontroller/include/PIRFunctions.h
new file mode 100644
index 0000000..f958d94
--- /dev/null
+++ b/firmware/sensor_microcontroller/include/PIRFunctions.h
@@ -0,0 +1,30 @@
+//
+// Functions created by Nam
+// Modified by Robbie
+//
+
+#ifndef PIRFUNCTIONS_H
+#define PIRFUNCTIONS_H
+
+
+#define PIR_0 10
+#define PIR_1 11
+#define PIR_2 12
+#define PIR_3 13
+
+struct PIRSensors {
+    bool pir0;
+    bool pir1;
+    bool pir2;
+    bool pir3;
+};
+
+//TODO add comments for all the functions
+
+void setupPIR();
+
+bool readPIRSingle(uint8_t pirPin);
+
+PIRSensors readAllPIR();
+
+#endif //PIRFUNCTIONS_H
diff --git a/firmware/sensor_microcontroller/include/PWMFunctions.h b/firmware/sensor_microcontroller/include/PWMFunctions.h
new file mode 100644
index 0000000..5deb04e
--- /dev/null
+++ b/firmware/sensor_microcontroller/include/PWMFunctions.h
@@ -0,0 +1,20 @@
+//
+// Created by Robbie on 2/5/25.
+//
+
+#ifndef PWMFUNCTIONS_H
+#define PWMFUNCTIONS_H
+
+#include <Arduino.h>
+
+
+
+/**
+ * Sets the PWM level at the specified pin to the frequency and duty cycle selected
+ * @param pin - the GPIO pin to use for PWM (must be a compatible pin)
+ * @param frequency - The PWM frequency
+ * @param duty_cycle - The duty cycle in percent
+ */
+void setPWM(uint pin, float frequency, float duty_cycle);
+
+#endif //PWMFUNCTIONS_H
diff --git a/firmware/sensor_microcontroller/include/README b/firmware/sensor_microcontroller/include/README
new file mode 100644
index 0000000..194dcd4
--- /dev/null
+++ b/firmware/sensor_microcontroller/include/README
@@ -0,0 +1,39 @@
+
+This directory is intended for project header files.
+
+A header file is a file containing C declarations and macro definitions
+to be shared between several project source files. You request the use of a
+header file in your project source file (C, C++, etc) located in `src` folder
+by including it, with the C preprocessing directive `#include'.
+
+```src/main.c
+
+#include "header.h"
+
+int main (void)
+{
+ ...
+}
+```
+
+Including a header file produces the same results as copying the header file
+into each source file that needs it. Such copying would be time-consuming
+and error-prone. With a header file, the related declarations appear
+in only one place. If they need to be changed, they can be changed in one
+place, and programs that include the header file will automatically use the
+new version when next recompiled. The header file eliminates the labor of
+finding and changing all the copies as well as the risk that a failure to
+find one copy will result in inconsistencies within a program.
+
+In C, the usual convention is to give header files names that end with `.h'.
+It is most portable to use only letters, digits, dashes, and underscores in
+header file names, and at most one dot.
+
+Read more about using header files in official GCC documentation:
+
+* Include Syntax
+* Include Operation
+* Once-Only Headers
+* Computed Includes
+
+https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html
diff --git a/firmware/sensor_microcontroller/include/UltrasonicFunctions.h b/firmware/sensor_microcontroller/include/UltrasonicFunctions.h
new file mode 100644
index 0000000..ea4b78e
--- /dev/null
+++ b/firmware/sensor_microcontroller/include/UltrasonicFunctions.h
@@ -0,0 +1,33 @@
+//
+// Created by Robbie on 12/3/24.
+//
+
+#ifndef JOYSTICK_ULTRASONICFUNCTIONS_H
+#define JOYSTICK_ULTRASONICFUNCTIONS_H
+#include <Adafruit_ADS1X15.h>
+
+struct USData {
+    int us_front_0;
+    int us_front_1;
+    int us_back;
+    int us_left;
+    int us_right;
+};
+
+/**
+ * Returns the distance read by an ultrasonic sensor.
+ * @param adc An instance of the ADC that the sensor is attached to.
+ * @param pinNumber The pin number of the ADC that the sensor is attached to.
+ * @return Returns the distance on the ultrasonic sensor, measured in cm. Returns -1 if the pin is misconfigured.
+ */
+uint16_t ultrasonicDistance(Adafruit_ADS1115 &adc, uint8_t pinNumber);
+
+/**
+ * Returns vales from all the ultrasonic sensors. Requires 2 ADCs to work
+ * @param adc0 The ADC for the joystick and single ultrasonic sensor
+ * @param adc1 The dedicate ADC for other ultrasonic sensors
+ * @return Returns an USData struct containing the values from all the ultrasonic sensors
+ */
+USData allUltrasonicDistance(Adafruit_ADS1115 &adc0, Adafruit_ADS1115 &adc1);
+
+#endif //JOYSTICK_ULTRASONICFUNCTIONS_H
diff --git a/firmware/sensor_microcontroller/include/microRosFunctions.h b/firmware/sensor_microcontroller/include/microRosFunctions.h
new file mode 100644
index 0000000..ff76d19
--- /dev/null
+++ b/firmware/sensor_microcontroller/include/microRosFunctions.h
@@ -0,0 +1,63 @@
+//
+// Created by Robbie on 11/20/24.
+//
+
+#ifndef JOYSTICK_MICROROSFUNCTIONS_H
+#define JOYSTICK_MICROROSFUNCTIONS_H
+
+#include <Arduino.h>
+#include "JoystickFunctions.h"
+#include "UltrasonicFunctions.h"
+#include "PIRFunctions.h"
+#include "FanFunctions.h"
+#include "IMUFunctions.h"
+
+#ifdef ROS
+/**
+ * Sets up microROS communication
+ * @param timerValue The transmission period
+ * @param nodeName The name of the node
+ * @param sensorTopicName The name of the topic
+ * @param fingerprintTopicName
+ */
+boolean microRosSetup(unsigned int timerValue, const char *nodeName, const char *sensorTopicName, const char *fingerprintTopicName);
+
+bool create_entities();
+
+void destroy_entities();
+
+void microRosTick();
+
+void transmitMsg(RefDisplacement thetaRef, RefSpeed omegaRef, USData ultrasonicData, PIRSensors pirSensors, FanSpeeds fanSpeeds, IMUData imuData);
+
+void publishFingerprint(uint8_t fingerprintID);
+
+void publishError(bool joystick_adc_error, bool ultrasonic_adc_error, bool fingerprint_error, bool imu_error);
+
+void fan_subscription_callback(const void *msgin);
+
+void light_subscription_callback(const void *msgin);
+
+void lidar_subscription_callback(const void *msgin);
+
+#elif ROS_DEBUG
+/**
+ * Sets up microROS communication
+ * @param timerValue The transmission period
+ * @param nodeName The name of the node
+ * @param topicName The name of the topic
+ */
+boolean microRosSetup(unsigned int timerValue, const char *nodeName, const char *topicName);
+
+/**
+ * Transmits the message over ROS
+ * @param omegaRef The referenceSpeed struct to transmit
+ */
+void transmitMsg(RefSpeed omegaRef);
+#endif
+
+void checkConnection();
+
+void reconnectAgent();
+
+#endif // JOYSTICK_MICROROSFUNCTIONS_H
diff --git a/firmware/sensor_microcontroller/lib/README b/firmware/sensor_microcontroller/lib/README
new file mode 100644
index 0000000..2593a33
--- /dev/null
+++ b/firmware/sensor_microcontroller/lib/README
@@ -0,0 +1,46 @@
+
+This directory is intended for project specific (private) libraries.
+PlatformIO will compile them to static libraries and link into executable file.
+
+The source code of each library should be placed in an own separate directory
+("lib/your_library_name/[here are source files]").
+
+For example, see a structure of the following two libraries `Foo` and `Bar`:
+
+|--lib
+|  |
+|  |--Bar
+|  |  |--docs
+|  |  |--examples
+|  |  |--src
+|  |     |- Bar.c
+|  |     |- Bar.h
+|  |  |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html
+|  |
+|  |--Foo
+|  |  |- Foo.c
+|  |  |- Foo.h
+|  |
+|  |- README --> THIS FILE
+|
+|- platformio.ini
+|--src
+   |- main.c
+
+and a contents of `src/main.c`:
+```
+#include <Foo.h>
+#include <Bar.h>
+
+int main (void)
+{
+  ...
+}
+
+```
+
+PlatformIO Library Dependency Finder will find automatically dependent
+libraries scanning project source files.
+
+More information about PlatformIO Library Dependency Finder
+- https://docs.platformio.org/page/librarymanager/ldf.html
diff --git a/firmware/sensor_microcontroller/platformio.ini b/firmware/sensor_microcontroller/platformio.ini
new file mode 100644
index 0000000..115c553
--- /dev/null
+++ b/firmware/sensor_microcontroller/platformio.ini
@@ -0,0 +1,66 @@
+; PlatformIO Project Configuration File
+;
+;   Build options: build flags, source filter
+;   Upload options: custom upload port, speed and extra flags
+;   Library options: dependencies, extra library storages
+;   Advanced options: extra scripting
+;
+; Please visit documentation for the other options and examples
+; https://docs.platformio.org/page/projectconf.html
+
+[env:regular]
+platform = raspberrypi
+board = pico
+framework = arduino
+lib_deps =
+    adafruit/Adafruit ADS1X15@^2.5.0
+    adafruit/Adafruit ICM20X@^2.0.7
+
+    adafruit/Adafruit Fingerprint Sensor Library@^2.1.3
+    SPI
+build_flags =
+
+[env:debug]
+platform = raspberrypi
+board = pico
+framework = arduino
+lib_deps =
+	adafruit/Adafruit ADS1X15@^2.5.0
+  adafruit/Adafruit ICM20X@^2.0.7
+
+    adafruit/Adafruit Fingerprint Sensor Library@^2.1.3
+    SPI
+build_flags =
+    -D DEBUG
+
+[env:ROS]
+platform = raspberrypi
+board = pico
+framework = arduino
+lib_deps =
+    adafruit/Adafruit ADS1X15@^2.5.0
+    adafruit/Adafruit ICM20X@^2.0.7
+    adafruit/Adafruit Fingerprint Sensor Library@^2.1.3
+    SPI
+    https://github.com/micro-ROS/micro_ros_platformio
+build_flags =
+    -D ROS
+
+board_microros_distro = humble
+board_microros_transport = serial
+
+[env:ROS-Debug]
+platform = raspberrypi
+board = pico
+framework = arduino
+lib_deps =
+    adafruit/Adafruit ADS1X15@^2.5.0
+    adafruit/Adafruit ICM20X@^2.0.7
+    adafruit/Adafruit Fingerprint Sensor Library@^2.1.3
+    SPI
+    https://github.com/micro-ROS/micro_ros_platformio
+build_flags =
+    -D ROS_DEBUG
+
+board_microros_distro = humble
+board_microros_transport = serial
\ No newline at end of file
diff --git a/firmware/sensor_microcontroller/src/ADCFunctions.cpp b/firmware/sensor_microcontroller/src/ADCFunctions.cpp
new file mode 100644
index 0000000..ed21185
--- /dev/null
+++ b/firmware/sensor_microcontroller/src/ADCFunctions.cpp
@@ -0,0 +1,49 @@
+//
+// Created by Robbie on 11/14/24.
+//
+
+#include "ADCFunctions.h"
+#include <Wire.h>
+#include <Adafruit_ADS1X15.h>
+
+
+/**
+ * Initializes the ADC with the given parameters
+ * @param adc - An instance of the Adafruit_ADS1115 class for the ADC
+ * @param i2c_addr - The I2C address of the ADC in hex
+ */
+ bool adcInit(Adafruit_ADS1115 &adc, uint8_t i2c_addr){
+    int adc_count = 0;
+
+    //Initialize ADC
+    while(!adc.begin(i2c_addr)){ // Initialize ads1115 at address 0x49
+        Serial.println("Failed to find ADS1115 chip at address " + String(i2c_addr, HEX));
+        if (adc_count > 10) {
+            return true;
+        }
+        adc_count++;
+    }
+    Serial.println("ADS1115 Found!");
+    adc.setGain(GAIN_ONE); //Setting the gain to +/- 4.096V  1 bit = 2mV for more precise readings
+     return false;
+}
+
+
+/***
+ * Prints the ADC data to the serial monitor
+ * @param adc - An instance of the Adafruit_ADS1115 class for the ADC
+ */
+void printADC(Adafruit_ADS1115 &adc){
+    //Get ADC data
+    int16_t adc0, adc1, adc2, adc3;
+
+    adc0 = adc.readADC_SingleEnded(0);
+    adc1 = adc.readADC_SingleEnded(1);
+    adc2 = adc.readADC_SingleEnded(2);
+    adc3 = adc.readADC_SingleEnded(3);
+    Serial.print("AIN0: "); Serial.println(adc0);
+    Serial.print("AIN1: "); Serial.println(adc1);
+    Serial.print("AIN2: "); Serial.println(adc2);
+    Serial.print("AIN3: "); Serial.println(adc3);
+    Serial.println();
+}
\ No newline at end of file
diff --git a/firmware/sensor_microcontroller/src/FanFunctions.cpp b/firmware/sensor_microcontroller/src/FanFunctions.cpp
new file mode 100644
index 0000000..7590fce
--- /dev/null
+++ b/firmware/sensor_microcontroller/src/FanFunctions.cpp
@@ -0,0 +1,104 @@
+//
+// Created by Robbie on 2/9/25.
+//
+
+#include <Arduino.h>
+#include "FanFunctions.h"
+#include "PWMFunctions.h"
+#include "hardware/pio.h"
+
+static float frequency = 25000;
+
+static volatile uint32_t pulse_count_single = 0;
+static uint32_t rpm_test = 0;
+static volatile int16_t pulse_count[4] = {0, 0, 0, 0};
+static volatile int16_t rpm[4] = {0, 0, 0, 0};
+
+void setFanIndividual(uint8_t fan, uint8_t dutyCycle){
+    setPWM(fan, frequency, dutyCycle);
+}
+
+void setAllFans(FanDutyCycles dutyCycles){
+    setPWM(FAN_0, frequency, dutyCycles.fan_0_duty_cycle);
+    setPWM(FAN_1, frequency, dutyCycles.fan_1_duty_cycle);
+    setPWM(FAN_2, frequency, dutyCycles.fan_2_duty_cycle);
+    setPWM(FAN_3, frequency, dutyCycles.fan_3_duty_cycle);
+}
+
+//TODO figure out how to read the actual fan speeds
+
+static int16_t last_pulse_count_test[4] = {0, 0, 0, 0};
+static uint32_t last_time_test[4] = {0, 0, 0, 0};
+
+FanSpeeds getAllFanSpeeds() {
+    FanSpeeds speeds{};
+
+    uint32_t current_time = millis();
+    for (uint8_t fanIndex = 0; fanIndex < 4; fanIndex++) {
+        if (current_time - last_time_test[fanIndex] >= 1000) {  // Calculate RPM every second
+            uint32_t pulses = pulse_count[fanIndex] - last_pulse_count_test[fanIndex];
+            last_pulse_count_test[fanIndex] = pulse_count[fanIndex];
+            last_time_test[fanIndex] = current_time;
+
+            // Assuming the fan gives 2 pulses per revolution
+            rpm[fanIndex] = (pulses * 60) / 2;
+        }
+    }
+
+    speeds.fan_speed_0 = rpm[0];
+    speeds.fan_speed_1 = rpm[1];
+    speeds.fan_speed_2 = rpm[2];
+    speeds.fan_speed_3 = rpm[3];
+
+    return speeds;
+}
+
+
+void setupRPMCounter(){
+    pinMode(TACH_0, INPUT);
+    pinMode(TACH_1, INPUT);
+    pinMode(TACH_2, INPUT);
+    pinMode(TACH_3, INPUT);
+    attachInterrupt(digitalPinToInterrupt(TACH_0), handleTach0Interrupt, FALLING);
+    attachInterrupt(digitalPinToInterrupt(TACH_1), handleTach1Interrupt, FALLING);
+    attachInterrupt(digitalPinToInterrupt(TACH_2), handleTach2Interrupt, FALLING);
+    attachInterrupt(digitalPinToInterrupt(TACH_3), handleTach3Interrupt, FALLING);
+}
+
+static void handleTach0Interrupt() {
+    pulse_count[0]++;
+    pulse_count_single++;
+}
+
+static void handleTach1Interrupt() {
+    pulse_count[1]++;
+}
+
+static void handleTach2Interrupt() {
+    pulse_count[2]++;
+}
+
+static void handleTach3Interrupt() {
+    pulse_count[3]++;
+}
+
+static uint32_t last_time = 0;
+uint32_t getRPM(){
+    static uint32_t last_pulse_count = 0;
+
+    uint32_t current_time = millis();
+    if (current_time - last_time >= 1000) {  // Calculate RPM every second
+        uint32_t pulses = pulse_count_single - last_pulse_count;
+        last_pulse_count = pulse_count_single;
+        last_time = current_time;
+
+        // Assuming the fan gives 2 pulses per revolution
+        rpm_test = (pulses * 60) / 2;
+
+    }
+    return rpm_test;
+}
+
+
+
+
diff --git a/firmware/sensor_microcontroller/src/FingerprintFunctions.cpp b/firmware/sensor_microcontroller/src/FingerprintFunctions.cpp
new file mode 100644
index 0000000..65ec796
--- /dev/null
+++ b/firmware/sensor_microcontroller/src/FingerprintFunctions.cpp
@@ -0,0 +1,137 @@
+//
+// Functions created by Nam
+// Modified by Robbie
+//
+
+#include "FingerprintFunctions.h"
+#include <Arduino.h>
+#include <Adafruit_Fingerprint.h>
+
+#if (defined(__AVR__) || defined(ESP8266)) && !defined(__AVR_ATmega2560__)
+// pin #2 is IN from sensor (GREEN wire)
+// pin #3 is OUT from arduino  (WHITE wire)
+// Set up the serial port to use softwareserial..
+SoftwareSerial fingerprintSerial(0, 1);
+
+#else
+// On Leonardo/M0/etc, others with hardware serial, use hardware serial!
+// #0 is green wire, #1 is white
+#define fingerprintSerial Serial1
+
+#endif
+
+Adafruit_Fingerprint finger = Adafruit_Fingerprint(&fingerprintSerial);
+
+uint8_t getFingerprintID() {
+  uint8_t p = finger.getImage();
+  switch (p) {
+    case FINGERPRINT_OK:
+      Serial.println("Image taken");
+      break;
+    case FINGERPRINT_NOFINGER:
+      Serial.println("No finger detected");
+      return p;
+    case FINGERPRINT_PACKETRECIEVEERR:
+      Serial.println("Communication error");
+      return p;
+    case FINGERPRINT_IMAGEFAIL:
+      Serial.println("Imaging error");
+      return p;
+    default:
+      Serial.println("Unknown error");
+      return p;
+  }
+  // OK success!
+  p = finger.image2Tz();
+  switch (p) {
+    case FINGERPRINT_OK:
+      Serial.println("Image converted");
+      break;
+    case FINGERPRINT_IMAGEMESS:
+      Serial.println("Image too messy");
+      return p;
+    case FINGERPRINT_PACKETRECIEVEERR:
+      Serial.println("Communication error");
+      return p;
+    case FINGERPRINT_FEATUREFAIL:
+      Serial.println("Could not find fingerprint features");
+      return p;
+    case FINGERPRINT_INVALIDIMAGE:
+      Serial.println("Could not find fingerprint features");
+      return p;
+    default:
+      Serial.println("Unknown error");
+      return p;
+  }
+  // OK converted!
+  p = finger.fingerSearch();
+  if (p == FINGERPRINT_OK) {
+    Serial.println("Found a print match!");
+  } else if (p == FINGERPRINT_PACKETRECIEVEERR) {
+    Serial.println("Communication error");
+    return p;
+  } else if (p == FINGERPRINT_NOTFOUND) {
+    Serial.println("Did not find a match");
+    return p;
+  } else {
+    Serial.println("Unknown error");
+    return p;
+  }
+
+  // found a match!
+  Serial.print("Found ID #"); Serial.print(finger.fingerID);
+  Serial.print(" with confidence of "); Serial.println(finger.confidence);
+
+  return finger.fingerID;
+}
+
+bool setupFingerprint()
+{
+  while (!Serial);  // For Yun/Leo/Micro/Zero/...
+  delay(100);
+  Serial.println("\n\nAdafruit finger detect test");
+
+  // set the data rate for the sensor serial port
+  finger.begin(57600);
+  delay(5);
+  if (finger.verifyPassword()) {
+    Serial.println("Found fingerprint sensor!");
+  } else {
+    int init_count = 0;
+    Serial.println("Did not find fingerprint sensor :(");
+    while (!finger.verifyPassword()) {
+      Serial.println(init_count);
+      if (init_count > 10) {
+        return true;
+      }
+      init_count++;
+    }
+  }
+
+  Serial.println(F("Reading sensor parameters"));
+  finger.getParameters();
+  Serial.print(F("Status: 0x")); Serial.println(finger.status_reg, HEX);
+  Serial.print(F("Sys ID: 0x")); Serial.println(finger.system_id, HEX);
+  Serial.print(F("Capacity: ")); Serial.println(finger.capacity);
+  Serial.print(F("Security level: ")); Serial.println(finger.security_level);
+  Serial.print(F("Device address: ")); Serial.println(finger.device_addr, HEX);
+  Serial.print(F("Packet len: ")); Serial.println(finger.packet_len);
+  Serial.print(F("Baud rate: ")); Serial.println(finger.baud_rate);
+
+  finger.getTemplateCount();
+
+  if (finger.templateCount == 0) {
+    Serial.print("Sensor doesn't contain any fingerprint data. Please run the 'enroll' example.");
+  }
+  else {
+    Serial.println("Waiting for valid finger...");
+    Serial.print("Sensor contains "); Serial.print(finger.templateCount); Serial.println(" templates");
+  }
+  return false;
+}
+
+void loopFingerprint()                     // run over and over again
+{
+  getFingerprintID();
+  delay(50);            //don't ned to run this at full speed.
+}
diff --git a/firmware/sensor_microcontroller/src/IMUFunctions.cpp b/firmware/sensor_microcontroller/src/IMUFunctions.cpp
new file mode 100644
index 0000000..760c691
--- /dev/null
+++ b/firmware/sensor_microcontroller/src/IMUFunctions.cpp
@@ -0,0 +1,183 @@
+//
+// Created by Robbie on 11/13/24.
+//
+
+#include "IMUFunctions.h"
+
+#include <Arduino.h>
+#include <Adafruit_ICM20X.h>
+#include <Adafruit_ICM20948.h>
+#include <Adafruit_Sensor.h>
+#include <Wire.h>
+
+
+/**
+ * Initializes the IMU with the given parameters
+ * @param icm - An instance of the Adafruit_ICM20948 class for the IMU
+ * @param accelRang - The range of the accelerometer using the icm20948_accel_range_t enum
+ * @param gyroRang  - The range of the gyroscope using the icm20948_gyro_range_t enum
+ * @param magDataRate - The data rate of the magnetometer using the ak09916_data_rate_t enum
+ */
+bool imuInit(Adafruit_ICM20948 &icm, icm20948_accel_range_t accelRang, icm20948_gyro_range_t gyroRang, ak09916_data_rate_t magDataRate){
+    int init_count = 0;
+    // Try to initialize IMU!
+    while (!icm.begin_I2C()) {
+        // if (!icm.begin_SPI(ICM_CS)) {
+        // if (!icm.begin_SPI(ICM_CS, ICM_SCK, ICM_MISO, ICM_MOSI)) {
+
+        Serial.println("Failed to find ICM20948 chip");
+        if (init_count > 10) {
+            return true;
+        }
+        init_count++;
+    }
+
+
+    Serial.println("ICM20948 Found!");
+    icm.setAccelRange(accelRang);
+    Serial.print("Accelerometer range set to: ");
+    switch (icm.getAccelRange()) {
+        case ICM20948_ACCEL_RANGE_2_G:
+            Serial.println("+-2G");
+            break;
+        case ICM20948_ACCEL_RANGE_4_G:
+            Serial.println("+-4G");
+            break;
+        case ICM20948_ACCEL_RANGE_8_G:
+            Serial.println("+-8G");
+            break;
+        case ICM20948_ACCEL_RANGE_16_G:
+            Serial.println("+-16G");
+            break;
+    }
+    Serial.println("OK");
+
+    icm.setGyroRange(gyroRang);
+    Serial.print("Gyro range set to: ");
+    switch (icm.getGyroRange()) {
+        case ICM20948_GYRO_RANGE_250_DPS:
+            Serial.println("250 degrees/s");
+            break;
+        case ICM20948_GYRO_RANGE_500_DPS:
+            Serial.println("500 degrees/s");
+            break;
+        case ICM20948_GYRO_RANGE_1000_DPS:
+            Serial.println("1000 degrees/s");
+            break;
+        case ICM20948_GYRO_RANGE_2000_DPS:
+            Serial.println("2000 degrees/s");
+            break;
+    }
+
+    //  icm.setAccelRateDivisor(4095);
+    uint16_t accel_divisor = icm.getAccelRateDivisor();
+    float accel_rate = 1125 / (1.0 + accel_divisor);
+
+    Serial.print("Accelerometer data rate divisor set to: ");
+    Serial.println(accel_divisor);
+    Serial.print("Accelerometer data rate (Hz) is approximately: ");
+    Serial.println(accel_rate);
+
+    //  icm.setGyroRateDivisor(255);
+    uint8_t gyro_divisor = icm.getGyroRateDivisor();
+    float gyro_rate = 1100 / (1.0 + gyro_divisor);
+
+    Serial.print("Gyro data rate divisor set to: ");
+    Serial.println(gyro_divisor);
+    Serial.print("Gyro data rate (Hz) is approximately: ");
+    Serial.println(gyro_rate);
+
+    icm.setMagDataRate(magDataRate);
+    Serial.print("Magnetometer data rate set to: ");
+    switch (icm.getMagDataRate()) {
+        case AK09916_MAG_DATARATE_SHUTDOWN:
+            Serial.println("Shutdown");
+            break;
+        case AK09916_MAG_DATARATE_SINGLE:
+            Serial.println("Single/One shot");
+            break;
+        case AK09916_MAG_DATARATE_10_HZ:
+            Serial.println("10 Hz");
+            break;
+        case AK09916_MAG_DATARATE_20_HZ:
+            Serial.println("20 Hz");
+            break;
+        case AK09916_MAG_DATARATE_50_HZ:
+            Serial.println("50 Hz");
+            break;
+        case AK09916_MAG_DATARATE_100_HZ:
+            Serial.println("100 Hz");
+            break;
+    }
+    Serial.println();
+    return false;
+}
+
+
+/**
+ * Prints the IMU data to the serial monitor
+ * @param icm - An instance of the Adafruit_ICM20948 class for the IMU
+ */
+void printImuData(Adafruit_ICM20948 &icm){
+
+    sensors_event_t accel;
+    sensors_event_t gyro;
+    sensors_event_t mag;
+    sensors_event_t temp;
+    icm.getEvent(&accel, &gyro, &temp, &mag);
+
+    Serial.print("Temperature ");
+    Serial.print(temp.temperature);
+    Serial.println(" deg C");
+
+    /* Display the results (acceleration is measured in m/s^2) */
+    Serial.print("Accel X: ");
+    Serial.print(accel.acceleration.x);
+    Serial.print(" \tY: ");
+    Serial.print(accel.acceleration.y);
+    Serial.print(" \tZ: ");
+    Serial.print(accel.acceleration.z);
+    Serial.println(" m/s^2 ");
+
+    Serial.print("Mag X: ");
+    Serial.print(mag.magnetic.x);
+    Serial.print(" \tY: ");
+    Serial.print(mag.magnetic.y);
+    Serial.print(" \tZ: ");
+    Serial.print(mag.magnetic.z);
+    Serial.println(" uT");
+
+    /* Display the results (acceleration is measured in m/s^2) */
+    Serial.print("Gyro X: ");
+    Serial.print(gyro.gyro.x);
+    Serial.print(" \tY: ");
+    Serial.print(gyro.gyro.y);
+    Serial.print(" \tZ: ");
+    Serial.print(gyro.gyro.z);
+    Serial.println(" radians/s ");
+    Serial.println();
+
+
+    delay(100);
+}
+
+IMUData getIMUData(Adafruit_ICM20948 &icm){
+    sensors_event_t accel;
+    sensors_event_t gyro;
+    sensors_event_t mag;
+    sensors_event_t temp;
+    icm.getEvent(&accel, &gyro, &temp, &mag);
+    IMUData data{};
+    data.accel_x = accel.acceleration.x;
+    data.accel_y = accel.acceleration.y;
+    data.accel_z = accel.acceleration.z;
+    data.gyro_x = gyro.gyro.x;
+    data.gyro_y = gyro.gyro.y;
+    data.gyro_z = gyro.gyro.z;
+    data.mag_x = mag.magnetic.x;
+    data.mag_y = mag.magnetic.y;
+    data.mag_z = mag.magnetic.z;
+
+    return data;
+}
+
diff --git a/firmware/sensor_microcontroller/src/JoystickFunctions.cpp b/firmware/sensor_microcontroller/src/JoystickFunctions.cpp
new file mode 100644
index 0000000..ec6b1f4
--- /dev/null
+++ b/firmware/sensor_microcontroller/src/JoystickFunctions.cpp
@@ -0,0 +1,157 @@
+//
+// Created by Robbie on 11/18/24.
+//
+
+#include "JoystickFunctions.h"
+#include <Adafruit_ADS1X15.h>
+#include <algorithm>
+
+int diffParam = 30;
+int deadzoneParam = 30;
+
+RefDisplacement joystickToDisplacement(Adafruit_ADS1115 &adc){
+    int forwardJoystick = adc.readADC_SingleEnded(0); //a0 is forward/backward
+    int sidewaysJoystick = adc.readADC_SingleEnded(1); //a1 is left/right
+
+    RefDisplacement displacements;
+    /*
+     * Joystick middle values: ~8500
+     * a0 middle value: ~8500
+     * a1 middle value: ~8300
+     * a0 deadzone 10000 - 6500
+     * a1 deadzone 11000 - 6000
+     * Joystick Min: 0
+     * Joystick Max: 17390
+     * Output is a value -100 to 100 for the speed of the motor
+     */
+
+    //Converting the speeds so they start around 0 and then go positive and negative
+    // displacements.longDisp = forwardJoystick - (8500+4400); //The second value is used to zero it out when the ADC gain is set to 0 instead of the default (2/3)
+    // displacements.latDisp = sidewaysJoystick - (8400+4400);
+
+    displacements.longDisp = forwardJoystick;
+    displacements.latDisp = sidewaysJoystick;
+    return displacements;
+}
+
+RefSpeed joystickToSpeed(Adafruit_ADS1115 &adc){
+    int forwardJoystick = adc.readADC_SingleEnded(0); //a0 is forward/backward
+    int sidewaysJoystick = adc.readADC_SingleEnded(1); //a1 is left/right
+
+    /*
+     * Joystick middle values: ~8500
+     * a0 middle value: ~8500
+     * a1 middle value: ~8300
+     * a0 deadzone 10000 - 6500
+     * a1 deadzone 11000 - 6000
+     * Joystick Min: 0
+     * Joystick Max: 17390
+     * Output is a value -100 to 100 for the speed of the motor
+     */
+
+    //Converting the speeds so they start around 0 and then go positive and negative
+    forwardJoystick = forwardJoystick - (8500+4400); //The second value is used to zero it out when the ADC gain is set to 0 instead of the default (2/3)
+    sidewaysJoystick = sidewaysJoystick - (8400+4400);
+
+//    Serial.print("Forward joystick: ");
+//    Serial.println(forwardJoystick);
+//    Serial.print("Sideways joystick: ");
+//    Serial.println(sidewaysJoystick);
+
+    //setting the speeds
+    RefSpeed speeds{};
+    const float MAX_INPUT = 13000.0f;
+    const float BACKWARD_X_THRESH = 0.65f;  // 0.0…1.0
+
+    // 1) normalize
+    float x = constrain(sidewaysJoystick / MAX_INPUT, -1.0f, 1.0f);
+    float y = constrain(forwardJoystick  / MAX_INPUT, -1.0f, 1.0f);
+
+    // 1b) pure backward shortcut
+    if (fabsf(x) < BACKWARD_X_THRESH && y < 0.0f) {
+        int8_t rev = (int8_t)roundf(y * 100.0f);
+        speeds.leftSpeed  = rev;
+        speeds.rightSpeed = rev;
+
+        //Deadzone
+        if(speeds.leftSpeed < deadzoneParam && speeds.leftSpeed > -deadzoneParam && speeds.rightSpeed < deadzoneParam && speeds.rightSpeed > -deadzoneParam){
+            speeds.leftSpeed = 0;
+            speeds.rightSpeed = 0;
+            return speeds;
+        }
+
+        return speeds;
+    }
+
+    // 2) magnitude clamp
+    float mag = hypotf(x, y);
+    mag = constrain(mag, 0.0f, 1.0f);
+
+    // 3+4) decide pivot vs mix
+    float outer, inner;
+    if (fabsf(x) > 0.0f && y <= 0.0f) {
+        // any sideways + backward → pivot (inner=0, outer=1)
+        outer = 1.0f;
+        inner = 0.0f;
+    } else {
+        // forward or straight back (x==0) → smooth mix
+        float angle     = atan2f(fabsf(x), fabsf(y));
+        float turn_prop = angle / (M_PI_2);
+        outer = 1.0f;
+        inner = 1.0f - turn_prop;
+    }
+
+    // 5) assign inner/outer to left/right
+    float left_f  = (x >= 0.0f) ? inner : outer;
+    float right_f = (x >= 0.0f) ? outer : inner;
+
+    // 6) direction: any sideways → forward pivot; otherwise follow y
+    int dir = (fabsf(x) > 0.0f) ? +1 : (y >= 0.0f ? +1 : -1);
+
+    // 7) apply magnitude & direction
+    left_f  *= mag * dir;
+    right_f *= mag * dir;
+
+    // 8) clamp & scale to –100…+100
+    left_f  = constrain(left_f,  -1.0f, 1.0f);
+    right_f = constrain(right_f, -1.0f, 1.0f);
+    speeds.leftSpeed  = (int8_t)roundf(left_f  * 100.0f);
+    speeds.rightSpeed = (int8_t)roundf(right_f * 100.0f);
+
+    //Deadzone
+    if(speeds.leftSpeed < deadzoneParam && speeds.leftSpeed > -deadzoneParam && speeds.rightSpeed < deadzoneParam && speeds.rightSpeed > -deadzoneParam){
+        speeds.leftSpeed = 0;
+        speeds.rightSpeed = 0;
+        return speeds;
+    }
+
+    //Middle zone to have the same speed
+    int diff = abs(speeds.leftSpeed - speeds.rightSpeed);
+    if(diff > -diffParam && diff < diffParam){
+        // Check if the speeds are positive or negative and set accordingly
+        if (speeds.leftSpeed > 0 && speeds.rightSpeed > 0) {
+            // Both speeds are positive, set the smaller one to the larger one
+            if (speeds.leftSpeed < speeds.rightSpeed) {
+                speeds.leftSpeed = speeds.rightSpeed;
+            } else {
+                speeds.rightSpeed = speeds.leftSpeed;
+            }
+        } else if (speeds.leftSpeed < 0 && speeds.rightSpeed < 0) {
+            // Both speeds are negative, set the larger one to the smaller one
+            if (speeds.leftSpeed > speeds.rightSpeed) {
+                speeds.leftSpeed = speeds.rightSpeed;
+            } else {
+                speeds.rightSpeed = speeds.leftSpeed;
+            }
+        }
+    }
+
+    return speeds;
+}
+
+template <typename T>
+T clamp(T value, T min, T max) {
+    if (value < min) return min;
+    if (value > max) return max;
+    return value;
+}
diff --git a/firmware/sensor_microcontroller/src/LidarFunctions.cpp b/firmware/sensor_microcontroller/src/LidarFunctions.cpp
new file mode 100644
index 0000000..67dc6dd
--- /dev/null
+++ b/firmware/sensor_microcontroller/src/LidarFunctions.cpp
@@ -0,0 +1,19 @@
+//
+// Created by Robbie on 4/9/25.
+//
+
+#include <Arduino.h>
+#include "LidarFunctions.h"
+
+void setupLidar(){
+    pinMode(LIDAR_PIN, OUTPUT);
+    lidarState(false);
+}
+
+void lidarState(boolean status){
+    if(status){
+        digitalWrite(LIDAR_PIN, HIGH);
+    } else {
+        digitalWrite(LIDAR_PIN, LOW);
+    }
+}
\ No newline at end of file
diff --git a/firmware/sensor_microcontroller/src/LightFunctions.cpp b/firmware/sensor_microcontroller/src/LightFunctions.cpp
new file mode 100644
index 0000000..0fe7bdd
--- /dev/null
+++ b/firmware/sensor_microcontroller/src/LightFunctions.cpp
@@ -0,0 +1,32 @@
+//
+// Created by aec117-fkmj9r3 on 3/11/25.
+//
+
+#include <Arduino.h>
+#include "LightFunctions.h"
+
+void setupLight(){
+    pinMode(LIGHT_PIN_1, OUTPUT);
+    pinMode(LIGHT_PIN_2, OUTPUT);
+    setLight(0);
+ }
+
+ void setLight(int lightState){
+    switch (lightState) {
+        case 0:
+            digitalWrite(LIGHT_PIN_1, LOW);
+            digitalWrite(LIGHT_PIN_2, LOW);
+        break;
+        case 1:
+            digitalWrite(LIGHT_PIN_1, LOW);
+            digitalWrite(LIGHT_PIN_2, HIGH);
+        break;
+        case 2:
+            digitalWrite(LIGHT_PIN_1, HIGH);
+            digitalWrite(LIGHT_PIN_2, LOW);
+        default:
+            digitalWrite(LIGHT_PIN_1, HIGH);
+            digitalWrite(LIGHT_PIN_2, LOW);
+        break;
+    }
+ }
\ No newline at end of file
diff --git a/firmware/sensor_microcontroller/src/PIRFunctions.cpp b/firmware/sensor_microcontroller/src/PIRFunctions.cpp
new file mode 100644
index 0000000..9da297c
--- /dev/null
+++ b/firmware/sensor_microcontroller/src/PIRFunctions.cpp
@@ -0,0 +1,30 @@
+//
+// Functions created by Nam
+// Modified by Robbie
+//
+
+#include <Arduino.h>
+#include "PIRFunctions.h"
+
+//TODO figure out if we need to add an extra PIR sensor attached to the joystick DAC
+
+void setupPIR() {
+    pinMode(PIR_0, INPUT); // initialize sensor as an input
+    pinMode(PIR_1, INPUT); // initialize sensor as an input
+    pinMode(PIR_2, INPUT); // initialize sensor as an input
+    pinMode(PIR_3, INPUT); // initialize sensor as an input
+    Serial.println("PIR detected");
+}
+
+bool readPIRSingle(uint8_t pirPin) {
+    return digitalRead(pirPin);
+}
+
+PIRSensors readAllPIR() {
+    PIRSensors pir_sensors{};
+    pir_sensors.pir0 = digitalRead(PIR_0);
+    pir_sensors.pir1 = digitalRead(PIR_1);
+    pir_sensors.pir2 = digitalRead(PIR_2);
+    pir_sensors.pir3 = digitalRead(PIR_3);
+    return pir_sensors;
+}
\ No newline at end of file
diff --git a/firmware/sensor_microcontroller/src/PWMFunctions.cpp b/firmware/sensor_microcontroller/src/PWMFunctions.cpp
new file mode 100644
index 0000000..67f3bbf
--- /dev/null
+++ b/firmware/sensor_microcontroller/src/PWMFunctions.cpp
@@ -0,0 +1,23 @@
+//
+// Created by Robbie on 2/5/25.
+//
+
+#include "PWMFunctions.h"
+#include "hardware/pwm.h"
+#include "hardware/gpio.h"
+
+void setPWM(uint pin, float frequency, float duty_cycle) {
+    uint slice = pwm_gpio_to_slice_num(pin);
+    uint channel = pwm_gpio_to_channel(pin);
+
+    gpio_set_function(pin, GPIO_FUNC_PWM);
+
+    float clock_freq = 125000000.0; // Default RP2040 system clock (125 MHz)
+    uint16_t wrap = clock_freq / frequency - 1;
+    pwm_set_wrap(slice, wrap);
+
+    uint16_t level = (wrap * duty_cycle) / 100.0;
+    pwm_set_chan_level(slice, channel, level);
+
+    pwm_set_enabled(slice, true);
+}
\ No newline at end of file
diff --git a/firmware/sensor_microcontroller/src/UltrasonicFunctions.cpp b/firmware/sensor_microcontroller/src/UltrasonicFunctions.cpp
new file mode 100644
index 0000000..db4551c
--- /dev/null
+++ b/firmware/sensor_microcontroller/src/UltrasonicFunctions.cpp
@@ -0,0 +1,45 @@
+//
+// Created by Robbie on 12/3/24.
+//
+
+#include "UltrasonicFunctions.h"
+#include <Adafruit_ADS1X15.h>
+#include "ADCFunctions.h"
+
+//TODO tune these numbers to get a more accurate reading
+#define  MAX_RANG      (520)//the max measurement value of the module is 520cm(a little bit longer than  effective max range)
+#define ADC_SOLUTION 26230 //Max value for a 16 bit dac at 3.3V
+
+//Distance Formula: Distance=Vout(mV)×520/Vin(mV)
+
+
+
+uint16_t ultrasonicDistance(Adafruit_ADS1115 &adc, uint8_t pinNumber){
+    if(pinNumber > 3){
+        Serial.println("Please select a pin between 0 and 3");
+        return -1;
+    }
+    //uint16_t distance = bitToMv(adc, pinNumber) * MAX_RANG / ADC_SOLUTION;
+    uint16_t distance = adc.readADC_SingleEnded(pinNumber) * MAX_RANG / ADC_SOLUTION;
+
+#ifdef DEBUG
+    // Serial.print("Raw Reading: ");
+    // Serial.println(adc.readADC_SingleEnded(pinNumber));
+    // Serial.print("Ultrasonic Distance: ");
+    // Serial.println(distance);
+#endif
+    return distance;
+}
+
+//adc0 is joystick and adc1 is dedicated ADC
+USData allUltrasonicDistance(Adafruit_ADS1115 &adc0, Adafruit_ADS1115 &adc1){
+    USData usData{};
+
+    usData.us_front_0 = adc0.readADC_SingleEnded(2) * MAX_RANG / ADC_SOLUTION;
+    usData.us_front_1 = adc1.readADC_SingleEnded(0) * MAX_RANG / ADC_SOLUTION;
+    usData.us_back = adc1.readADC_SingleEnded(1) * MAX_RANG / ADC_SOLUTION;
+    usData.us_left = adc1.readADC_SingleEnded(2) * MAX_RANG / ADC_SOLUTION;
+    usData.us_right = adc1.readADC_SingleEnded(3) * MAX_RANG / ADC_SOLUTION;
+
+    return usData;
+}
\ No newline at end of file
diff --git a/firmware/sensor_microcontroller/src/main.cpp b/firmware/sensor_microcontroller/src/main.cpp
new file mode 100644
index 0000000..71ae5bd
--- /dev/null
+++ b/firmware/sensor_microcontroller/src/main.cpp
@@ -0,0 +1,237 @@
+/*
+
+*/
+
+#include <Wire.h>
+#include <Adafruit_ADS1X15.h>
+#include <Adafruit_ICM20948.h>
+#include <hardware/watchdog.h>
+#include "ADCFunctions.h"
+#include "JoystickFunctions.h"
+#include "UltrasonicFunctions.h"
+#include "FingerprintFunctions.h"
+#include "PIRFunctions.h"
+#include "IMUFunctions.h"
+#include "FanFunctions.h"
+#include "LightFunctions.h"
+#include "LidarFunctions.h"
+
+#if defined(ROS) || defined(ROS_DEBUG)
+#include <micro_ros_platformio.h>
+#include <microRosFunctions.h>
+#endif
+
+Adafruit_ADS1115 joystickAdc;
+Adafruit_ADS1115 ultrasonicAdc;// Construct an ads1115
+Adafruit_ICM20948 icm;
+
+
+bool joystick_adc_error = false;
+bool ultrasonic_adc_error = false;
+bool fingerprint_error = false;
+bool imu_error = false;
+int error_timer = 5000;
+
+void setup() {
+
+    setupLidar();
+    lidarState(true); // Enabling the LiDAR at start so it can grab the SDK correctly
+
+    Serial.begin(115200);
+    FanDutyCycles startDutyCycles{};
+    startDutyCycles.fan_0_duty_cycle = 0;
+    startDutyCycles.fan_1_duty_cycle = 0;
+    startDutyCycles.fan_2_duty_cycle = 0;
+    startDutyCycles.fan_3_duty_cycle = 0;
+    setAllFans(startDutyCycles);
+
+    while(!Serial){
+        delay(10); //wait for serial
+    }
+
+    delay(2000);
+    Serial.println("Hello microcontroller");
+    #ifdef ROS
+
+    set_microros_serial_transports(Serial);
+    delay(2000);
+
+//    const char* nodeName = "sensors_node";
+//    const char* sensorTopicName = "sensors";
+//    const char* fingerprintTopicName = "fingerprint";
+
+    //microRosSetup(1, nodeName, sensorTopicName, fingerprintTopicName);
+
+    #elif ROS_DEBUG
+
+    const char* nodeName = "sensors_node";
+    const char* topicName = "refSpeed";
+    while(!microRosSetup(1, nodeName, topicName));
+    #endif
+
+
+
+
+    watchdog_enable(5000, 1);  // updating the watchdog// set the watchdog to run at 5s interval
+
+    ultrasonic_adc_error = adcInit(ultrasonicAdc, 0x49); //default address
+    joystick_adc_error =  adcInit(joystickAdc, 0x48); //default address
+    imu_error = imuInit(icm, ICM20948_ACCEL_RANGE_2_G, ICM20948_GYRO_RANGE_250_DPS, AK09916_MAG_DATARATE_10_HZ);
+    fingerprint_error = setupFingerprint();
+    Serial.print("Fingerprint error: ");
+    setAllFans(startDutyCycles);
+    setupRPMCounter();
+    setupLight();
+
+#ifdef ROS
+    if (!joystick_adc_error || !ultrasonic_adc_error || !fingerprint_error || !imu_error) {
+        publishError(joystick_adc_error, ultrasonic_adc_error, fingerprint_error, imu_error);
+        error_timer = 500;
+    }
+#endif
+
+
+Serial.println("Joystick Error: " + String(joystick_adc_error));
+    Serial.println("Ultrasonic Error: " + String(ultrasonic_adc_error));
+    Serial.println("Fingerprint Error: " + String(fingerprint_error));
+    Serial.println("IMU Error: " + String(imu_error));
+}
+
+unsigned long lastFingerprintTime = 0;
+unsigned long lastErrorTime = 0;
+
+//unsigned long lastMicroRosTime = 0;
+
+void loop() {
+    unsigned long currentMillis = millis();
+
+    //TODO might want to figure out how to put these on core1 so that they can run in parallel
+    //uint32_t start = millis();
+    RefSpeed omegaRef{};
+    RefDisplacement thetaRef{};
+    if (!joystick_adc_error) {
+        omegaRef = joystickToSpeed(joystickAdc);
+        thetaRef = joystickToDisplacement(joystickAdc);
+    }
+
+    
+
+
+    //uint32_t joystickTime = millis() - start;
+    USData usDistances{};
+    if (!ultrasonic_adc_error && !joystick_adc_error) {
+        usDistances = allUltrasonicDistance(joystickAdc, ultrasonicAdc);
+    }
+
+
+    //uint32_t ultrasonicTime = millis() - start - joystickTime;
+    PIRSensors pirSensors = readAllPIR();
+    //uint32_t pirTime = millis() - start - joystickTime - ultrasonicTime;
+    //uint8_t fingerID = getFingerprintID(); //TODO might want to put this on a timer so that it runs less frequently
+    //uint32_t fingerprintTime = millis() - start - joystickTime - ultrasonicTime - pirTime;
+    IMUData imuData{};
+    if (!imu_error) {
+        imuData = getIMUData(icm);
+    }
+
+
+    //uint32_t imuTime = millis() - start - joystickTime - ultrasonicTime - pirTime - fingerprintTime;
+    FanSpeeds fanSpeeds = getAllFanSpeeds(); //TODO might want to put on a timer as well
+    //uint32_t fanTime = millis() - start - joystickTime - ultrasonicTime - pirTime - fingerprintTime - imuTime;
+
+    uint8_t fingerID = 2;
+    if (currentMillis - lastFingerprintTime >= 5000) {
+        lastFingerprintTime = currentMillis;
+        if (!fingerprint_error) {
+            fingerID = getFingerprintID();
+        }
+
+        //Serial.println("Fingerprint ID: " + String(fingerID));
+    }
+
+    watchdog_update(); //updating the watchdog
+
+    #ifdef ROS
+
+    microRosTick();
+
+    transmitMsg(thetaRef,omegaRef, usDistances, pirSensors, fanSpeeds, imuData);
+
+    if (currentMillis - lastErrorTime >= error_timer) {
+        lastErrorTime = currentMillis;
+        publishError(joystick_adc_error, ultrasonic_adc_error, fingerprint_error, imu_error);
+    }
+
+    if(fingerID != 2){
+        publishFingerprint(fingerID);
+    }
+    #elif ROS_DEBUG
+
+
+    transmitMsg(thetaRef,omegaRef);
+
+
+    #elif DEBUG
+
+//     Serial.print("Joystick Time: ");
+//     Serial.println(joystickTime);
+//     Serial.print("Ultrasonic Time: ");
+//     Serial.println(ultrasonicTime);
+//     Serial.print("PIR Time: ");
+//     Serial.println(pirTime);
+//     Serial.print("Fingerprint Time: ");
+//     Serial.println(fingerprintTime);
+//     Serial.print("IMU Time: ");
+//     Serial.println(imuTime);
+//     Serial.print("Fan Time: ");
+//     Serial.println(fanTime);
+
+     Serial.print("Fan0 Speed: ");
+     Serial.println(fanSpeeds.fan_speed_0);
+     Serial.print("Fan1 Speed: ");
+     Serial.println(fanSpeeds.fan_speed_1);
+     Serial.print("Fan2 Speed: ");
+     Serial.println(fanSpeeds.fan_speed_2);
+     Serial.print("Fan3 Speed: ");
+     Serial.println(fanSpeeds.fan_speed_3);
+     Serial.print("Right Speed: ");
+     Serial.println(omegaRef.rightSpeed);
+     Serial.print("Left Speed: ");
+     Serial.println(omegaRef.leftSpeed);
+     //Serial.print("Ultrasonic Distance: ");
+     //Serial.println(usDistance);
+//     Serial.print("PIR 0 struct: ");
+//     Serial.println(pirSensors.pir0);
+//     Serial.print("PIR 1: ");
+//     Serial.println(pirSensors.pir1);
+//     Serial.print("PIR 2: ");
+//     Serial.println(pirSensors.pir2);
+//     Serial.print("PIR 3: ");
+//     Serial.println(pirSensors.pir3);
+//     Serial.print("Fingerprint ID: ");
+//     Serial.println(fingerID);
+
+
+     Serial.print("Accel X: ");
+     Serial.print(imuData.accel_x);
+     Serial.print(" \tY: ");
+     Serial.print(imuData.accel_y);
+     Serial.print(" \tZ: ");
+     Serial.println(imuData.accel_z);
+
+     Serial.print("Gyro X: ");
+     Serial.print(imuData.gyro_x);
+     Serial.print(" \tY: ");
+     Serial.print(imuData.gyro_y);
+     Serial.print(" \tZ: ");
+     Serial.println(imuData.gyro_z);
+
+     Serial.print("Mag X: ");
+     Serial.print(imuData.mag_x);
+     Serial.print(" \tY: ");
+     Serial.print(imuData.mag_y);
+     Serial.print(" \tZ: ");
+     Serial.println(imuData.mag_z);
+    #endif
+
+}
diff --git a/firmware/sensor_microcontroller/src/microRosFunctions.cpp b/firmware/sensor_microcontroller/src/microRosFunctions.cpp
new file mode 100644
index 0000000..4691722
--- /dev/null
+++ b/firmware/sensor_microcontroller/src/microRosFunctions.cpp
@@ -0,0 +1,418 @@
+//
+// Created by Robbie on 11/20/24.
+//
+#if defined(ROS) || defined(ROS_DEBUG)
+#include "microRosFunctions.h"
+#include "FanFunctions.h"
+#include "JoystickFunctions.h"
+#include "LidarFunctions.h"
+#include "LightFunctions.h"
+#include <Arduino.h>
+#include <hardware/watchdog.h>
+#include <micro_ros_platformio.h>
+
+#include <rcl/error_handling.h>
+#include <rcl/rcl.h>
+#include <rclc/executor.h>
+#include <rclc/rclc.h>
+
+#include <autogiro_interfaces/msg/fan_speed.h>
+#include <autogiro_interfaces/msg/fingerprint.h>
+#include <autogiro_interfaces/msg/lidar.h>
+#include <autogiro_interfaces/msg/light.h>
+#include <autogiro_interfaces/msg/sensor_error.h>
+#include <autogiro_interfaces/msg/sensors.h>
+
+#ifdef ROS_DEBUG
+#include <autogiro_interfaces/msg/ref_speed.h>
+#endif
+
+rcl_publisher_t sensorPublisher;
+rcl_publisher_t fingerprintPublisher;
+rcl_publisher_t errorPublisher;
+
+rcl_subscription_t fanSubscriber;
+rcl_subscription_t lightSubscriber;
+rcl_subscription_t lidarSubscriber;
+
+autogiro_interfaces__msg__FanSpeed fanMsg;
+autogiro_interfaces__msg__Light lightMsg;
+autogiro_interfaces__msg__Lidar lidarMsg;
+#ifdef ROS
+autogiro_interfaces__msg__Sensors sensorMsg;
+#elif ROS_DEBUG
+autogiro_interfaces__msg__RefSpeed msg;
+#endif
+
+rclc_executor_t executor;
+rclc_support_t support;
+rcl_allocator_t allocator;
+rcl_node_t node;
+rcl_timer_t timer;
+
+boolean agent_connected = false;
+
+enum states {
+  WAITING_AGENT,
+  AGENT_AVAILABLE,
+  AGENT_CONNECTED,
+  AGENT_DISCONNECTED
+} state;
+
+#define EXECUTE_EVERY_N_MS(MS, X)                                              \
+  do {                                                                         \
+    static volatile int64_t init = -1;                                         \
+    if (init == -1) {                                                          \
+      init = uxr_millis();                                                     \
+    }                                                                          \
+    if (uxr_millis() - init > MS) {                                            \
+      X;                                                                       \
+      init = uxr_millis();                                                     \
+    }                                                                          \
+  } while (0)
+
+#define RCCHECK(fn)                                                            \
+  {                                                                            \
+    rcl_ret_t temp_rc = fn;                                                    \
+    if ((temp_rc != RCL_RET_OK)) {                                             \
+      error_loop();                                                            \
+    }                                                                          \
+  }
+#define RCSOFTCHECK(fn)                                                        \
+  {                                                                            \
+    rcl_ret_t temp_rc = fn;                                                    \
+    if ((temp_rc != RCL_RET_OK)) {                                             \
+    }                                                                          \
+  }
+
+// Error handle loop
+void error_loop() {
+  while (1) {
+    delay(100);
+  }
+}
+
+void timer_callback(rcl_timer_t *inputTimer, int64_t last_call_time) {
+  RCLC_UNUSED(last_call_time);
+  if (inputTimer != NULL) {
+#ifdef ROS
+    rcl_publish(&sensorPublisher, &sensorMsg, NULL);
+#elif ROS_DEBUG
+    RCSOFTCHECK(rcl_publish(&sensorPublisher, &msg, NULL));
+#endif
+  }
+}
+
+bool create_entities() {
+  allocator = rcl_get_default_allocator();
+
+  // create init_options
+  RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));
+
+  // create node
+  RCCHECK(rclc_node_init_default(&node, "sensors_node", "", &support));
+
+#ifdef ROS
+  // create publisher
+  RCCHECK(rclc_publisher_init_best_effort(
+      &sensorPublisher, &node,
+      ROSIDL_GET_MSG_TYPE_SUPPORT(autogiro_interfaces, msg, Sensors),
+      "sensors"));
+  // Create fingerprint publisher
+  RCCHECK(rclc_publisher_init_default(
+      &fingerprintPublisher, &node,
+      ROSIDL_GET_MSG_TYPE_SUPPORT(autogiro_interfaces, msg, Fingerprint),
+      "fingerprint"));
+
+  // Create error publisher
+  RCCHECK(rclc_publisher_init_default(
+      &errorPublisher, &node,
+      ROSIDL_GET_MSG_TYPE_SUPPORT(autogiro_interfaces, msg, SensorError),
+      "sensor_error"));
+
+  // create timer,
+  // unsigned int timer_timeout = 1;
+  RCCHECK(rclc_timer_init_default(&timer, &support, RCL_MS_TO_NS(10),
+                                  timer_callback));
+
+#elif ROS_DEBUG
+  // create publisher
+  RCCHECK(rclc_publisher_init_best_effort(
+      &sensorPublisher, &node,
+      ROSIDL_GET_MSG_TYPE_SUPPORT(autogiro_interfaces, msg, RefSpeed),
+      topicName));
+#endif
+
+  // Create subscriber
+  RCCHECK(rclc_subscription_init_default(
+      &fanSubscriber, &node,
+      ROSIDL_GET_MSG_TYPE_SUPPORT(autogiro_interfaces, msg, FanSpeed),
+      "fan_duty_cycles"));
+
+  // Create subscriber
+  RCCHECK(rclc_subscription_init_default(
+      &lightSubscriber, &node,
+      ROSIDL_GET_MSG_TYPE_SUPPORT(autogiro_interfaces, msg, Light), "light"));
+
+  // Create subscriber
+  RCCHECK(rclc_subscription_init_default(
+      &lidarSubscriber, &node,
+      ROSIDL_GET_MSG_TYPE_SUPPORT(autogiro_interfaces, msg, Lidar), "lidar"));
+
+  // create executor
+  // Number of handles = # timers + # subscriptions + # clients + # services
+  executor = rclc_executor_get_zero_initialized_executor();
+  RCCHECK(rclc_executor_init(&executor, &support.context, 5, &allocator));
+  RCCHECK(rclc_executor_add_subscription(&executor, &fanSubscriber, &fanMsg,
+                                         &fan_subscription_callback,
+                                         ON_NEW_DATA));
+  RCCHECK(rclc_executor_add_subscription(&executor, &lightSubscriber, &lightMsg,
+                                         &light_subscription_callback,
+                                         ON_NEW_DATA));
+  RCCHECK(rclc_executor_add_subscription(&executor, &lidarSubscriber, &lidarMsg,
+                                         &lidar_subscription_callback,
+                                         ON_NEW_DATA));
+  RCCHECK(rclc_executor_add_timer(&executor, &timer));
+
+#ifdef ROS
+  sensorMsg.left_speed = 0;
+  sensorMsg.right_speed = 0;
+#elif ROS_DEBUG
+  msg.left_speed = 0;
+  msg.right_speed = 0;
+#endif
+
+  state = WAITING_AGENT;
+
+  return true;
+}
+
+void destroy_entities() {
+  rmw_context_t *rmw_context = rcl_context_get_rmw_context(&support.context);
+  (void)rmw_uros_set_context_entity_destroy_session_timeout(rmw_context, 0);
+
+  RCCHECK(rcl_subscription_fini(&fanSubscriber, &node));
+  RCCHECK(rcl_subscription_fini(&lidarSubscriber, &node));
+  RCCHECK(rcl_subscription_fini(&lightSubscriber, &node));
+  RCCHECK(rcl_publisher_fini(&sensorPublisher, &node));
+  RCCHECK(rcl_publisher_fini(&fingerprintPublisher, &node));
+  RCCHECK(rcl_timer_fini(&timer));
+  RCCHECK(rclc_executor_fini(&executor));
+  RCCHECK(rcl_node_fini(&node));
+  RCCHECK(rclc_support_fini(&support));
+}
+
+void microRosTick() {
+  switch (state) {
+  case WAITING_AGENT:
+    EXECUTE_EVERY_N_MS(500, state = (RMW_RET_OK == rmw_uros_ping_agent(100, 1))
+                                        ? AGENT_AVAILABLE
+                                        : WAITING_AGENT;);
+    break;
+  case AGENT_AVAILABLE:
+    state = (true == create_entities()) ? AGENT_CONNECTED : WAITING_AGENT;
+    if (state == WAITING_AGENT) {
+      destroy_entities();
+    };
+    break;
+  case AGENT_CONNECTED:
+    EXECUTE_EVERY_N_MS(200, state = (RMW_RET_OK == rmw_uros_ping_agent(100, 1))
+                                        ? AGENT_CONNECTED
+                                        : AGENT_DISCONNECTED;);
+    if (state == AGENT_CONNECTED) {
+      rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100));
+    }
+    break;
+  case AGENT_DISCONNECTED:
+    destroy_entities();
+    state = WAITING_AGENT;
+    break;
+  default:
+    break;
+  }
+}
+
+// TODO add the fan subscriber
+#ifdef ROS
+boolean microRosSetup(unsigned int timer_timeout, const char *nodeName,
+                      const char *sensorTopicName,
+                      const char *fingerprintTopicName) {
+#elif ROS_DEBUG
+boolean microRosSetup(unsigned int timer_timeout, const char *nodeName,
+                      const char *topicName) {
+#endif
+  set_microros_serial_transports(Serial);
+  delay(2000);
+
+  //    // Check for agent BEFORE proceeding
+  //    if (!rmw_uros_ping_agent(100, 3)) { // 100ms timeout, 3 retries
+  //        printf("Micro-ROS agent not found — skipping setup.\n");
+  //        return false;
+  //    }
+
+  allocator = rcl_get_default_allocator();
+
+  // Set the domain ID
+  // const size_t domain_id = 7; // Replace with your desired domain ID
+
+  // create init_options
+  RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));
+
+  // create node
+  RCCHECK(rclc_node_init_default(&node, nodeName, "", &support));
+
+#ifdef ROS
+  // create publisher
+  RCCHECK(rclc_publisher_init_best_effort(
+      &sensorPublisher, &node,
+      ROSIDL_GET_MSG_TYPE_SUPPORT(autogiro_interfaces, msg, Sensors),
+      sensorTopicName));
+  // Create fingerprint publisher
+  RCCHECK(rclc_publisher_init_default(
+      &fingerprintPublisher, &node,
+      ROSIDL_GET_MSG_TYPE_SUPPORT(autogiro_interfaces, msg, Fingerprint),
+      fingerprintTopicName));
+
+  // create timer,
+  // unsigned int timer_timeout = 1;
+  RCCHECK(rclc_timer_init_default(&timer, &support, RCL_MS_TO_NS(timer_timeout),
+                                  timer_callback));
+
+#elif ROS_DEBUG
+  // create publisher
+  RCCHECK(rclc_publisher_init_best_effort(
+      &sensorPublisher, &node,
+      ROSIDL_GET_MSG_TYPE_SUPPORT(autogiro_interfaces, msg, RefSpeed),
+      topicName));
+#endif
+
+  // Create subscriber
+  RCCHECK(rclc_subscription_init_default(
+      &fanSubscriber, &node,
+      ROSIDL_GET_MSG_TYPE_SUPPORT(autogiro_interfaces, msg, FanSpeed),
+      "fan_duty_cycles"));
+
+  // Create subscriber
+  RCCHECK(rclc_subscription_init_default(
+      &lightSubscriber, &node,
+      ROSIDL_GET_MSG_TYPE_SUPPORT(autogiro_interfaces, msg, Light), "light"));
+
+  // Create subscriber
+  RCCHECK(rclc_subscription_init_default(
+      &lidarSubscriber, &node,
+      ROSIDL_GET_MSG_TYPE_SUPPORT(autogiro_interfaces, msg, Lidar), "lidar"));
+
+  // create executor
+  // Number of handles = # timers + # subscriptions + # clients + # services
+  RCCHECK(rclc_executor_init(&executor, &support.context, 5, &allocator));
+  RCCHECK(rclc_executor_add_subscription(&executor, &fanSubscriber, &fanMsg,
+                                         &fan_subscription_callback,
+                                         ON_NEW_DATA));
+  RCCHECK(rclc_executor_add_subscription(&executor, &lightSubscriber, &lightMsg,
+                                         &light_subscription_callback,
+                                         ON_NEW_DATA));
+  RCCHECK(rclc_executor_add_subscription(&executor, &lidarSubscriber, &lidarMsg,
+                                         &lidar_subscription_callback,
+                                         ON_NEW_DATA));
+  RCCHECK(rclc_executor_add_timer(&executor, &timer));
+
+#ifdef ROS
+  sensorMsg.left_speed = 0;
+  sensorMsg.right_speed = 0;
+#elif ROS_DEBUG
+  msg.left_speed = 0;
+  msg.right_speed = 0;
+#endif
+  return true;
+}
+
+void publishFingerprint(uint8_t fingerprintID) {
+  autogiro_interfaces__msg__Fingerprint fingerprintMsg;
+  fingerprintMsg.user_id = fingerprintID;
+  RCSOFTCHECK(rcl_publish(&fingerprintPublisher, &fingerprintMsg, NULL));
+}
+
+void publishError(const bool joystick_adc_error,
+                  const bool ultrasonic_adc_error, const bool fingerprint_error,
+                  const bool imu_error) {
+  autogiro_interfaces__msg__SensorError sensorErrorMsg;
+  sensorErrorMsg.joystick_adc_error = joystick_adc_error;
+  sensorErrorMsg.ultrasonic_adc_error = ultrasonic_adc_error;
+  sensorErrorMsg.fingerprint_error = fingerprint_error;
+  sensorErrorMsg.imu_error = imu_error;
+  RCSOFTCHECK(rcl_publish(&errorPublisher, &sensorErrorMsg, NULL));
+}
+
+void fan_subscription_callback(const void *msgin) {
+  const auto *msg = (const autogiro_interfaces__msg__FanSpeed *)msgin;
+  FanDutyCycles duty_cycles{};
+  duty_cycles.fan_0_duty_cycle = msg->fan_percent_0;
+  duty_cycles.fan_1_duty_cycle = msg->fan_percent_1;
+  duty_cycles.fan_2_duty_cycle = msg->fan_percent_2;
+  duty_cycles.fan_3_duty_cycle = msg->fan_percent_3;
+  setAllFans(duty_cycles);
+}
+
+void light_subscription_callback(const void *msgin) {
+  const auto *msg = (const autogiro_interfaces__msg__Light *)msgin;
+  const int lightState = msg->state;
+  setLight(lightState);
+}
+
+void lidar_subscription_callback(const void *msgin) {
+  const auto *msg = (const autogiro_interfaces__msg__Lidar *)msgin;
+  if (msg->state == 0) {
+    lidarState(false);
+  } else if (msg->state == 1) {
+    lidarState(true);
+  }
+}
+
+#ifdef ROS
+void transmitMsg(RefDisplacement thetaRef, RefSpeed omegaRef,
+                 USData ultrasonicData, PIRSensors pirSensors,
+                 FanSpeeds fanSpeeds, IMUData imuData) {
+  sensorMsg.long_disp = thetaRef.longDisp;
+  sensorMsg.lat_disp = thetaRef.latDisp;
+  sensorMsg.left_speed = omegaRef.leftSpeed;
+  sensorMsg.right_speed = omegaRef.rightSpeed;
+  sensorMsg.ultrasonic_front_0 = ultrasonicData.us_front_0;
+  sensorMsg.ultrasonic_front_1 = ultrasonicData.us_front_1;
+  sensorMsg.ultrasonic_back = ultrasonicData.us_back;
+  sensorMsg.ultrasonic_left = ultrasonicData.us_left;
+  sensorMsg.ultrasonic_right = ultrasonicData.us_right;
+  sensorMsg.pir_front = pirSensors.pir0;
+  sensorMsg.pir_back = pirSensors.pir1;
+  sensorMsg.pir_left = pirSensors.pir2;
+  sensorMsg.pir_right = pirSensors.pir3;
+  sensorMsg.fan_speed_0 = fanSpeeds.fan_speed_0;
+  sensorMsg.fan_speed_1 = fanSpeeds.fan_speed_1;
+  sensorMsg.fan_speed_2 = fanSpeeds.fan_speed_2;
+  sensorMsg.fan_speed_3 = fanSpeeds.fan_speed_3;
+  sensorMsg.linear_acceleration_x = imuData.accel_x;
+  sensorMsg.linear_acceleration_y = imuData.accel_y;
+  sensorMsg.linear_acceleration_z = imuData.accel_z;
+  sensorMsg.angular_velocity_x = imuData.gyro_x;
+  sensorMsg.angular_velocity_y = imuData.gyro_y;
+  sensorMsg.angular_velocity_z = imuData.gyro_z;
+  sensorMsg.magnetic_field_x = imuData.mag_x;
+  sensorMsg.magnetic_field_y = imuData.mag_y;
+  sensorMsg.magnetic_field_z = imuData.mag_z;
+
+  RCSOFTCHECK(rclc_executor_spin_some(&executor, RCL_MS_TO_NS(10)));
+}
+
+#elif ROS_DEBUG
+
+void transmitMsg(RefDisplacement thetaRef, RefSpeed omegaRef) {
+  msg.left_speed = omegaRef.leftSpeed;
+  msg.right_speed = omegaRef.rightSpeed;
+  msg.long_disp = thetaRef.longDisp;
+  msg.lat_disp = thetaRef.latDisp;
+
+  RCSOFTCHECK(rclc_executor_spin_some(&executor, RCL_MS_TO_NS(10)));
+}
+
+#endif
+
+#endif
diff --git a/firmware/sensor_microcontroller/test/README b/firmware/sensor_microcontroller/test/README
new file mode 100644
index 0000000..9b1e87b
--- /dev/null
+++ b/firmware/sensor_microcontroller/test/README
@@ -0,0 +1,11 @@
+
+This directory is intended for PlatformIO Test Runner and project tests.
+
+Unit Testing is a software testing method by which individual units of
+source code, sets of one or more MCU program modules together with associated
+control data, usage procedures, and operating procedures, are tested to
+determine whether they are fit for use. Unit testing finds problems early
+in the development cycle.
+
+More information about PlatformIO Unit Testing:
+- https://docs.platformio.org/en/latest/advanced/unit-testing/index.html
diff --git a/frontend/src/renderer/src/types/ros-msgs.gen.ts b/frontend/src/renderer/src/types/ros-msgs.gen.ts
index 5a198d3..30b8da5 100644
--- a/frontend/src/renderer/src/types/ros-msgs.gen.ts
+++ b/frontend/src/renderer/src/types/ros-msgs.gen.ts
@@ -1,5 +1,18 @@
 /* eslint-disable */
 // These files were generated using "ros-typescript-generator"
+export interface AutogiroInterfacesBattery {
+  battery_percent: number;
+}
+
+export interface AutogiroInterfacesBrake {
+  brake: boolean;
+}
+
+export interface AutogiroInterfacesDacValues {
+  left_dac: number;
+  right_dac: number;
+}
+
 export interface AutogiroInterfacesFanSpeed {
   fan_percent_0: number;
   fan_percent_1: number;
@@ -7,6 +20,48 @@ export interface AutogiroInterfacesFanSpeed {
   fan_percent_3: number;
 }
 
+export interface AutogiroInterfacesFingerprint {
+  user_id: number;
+}
+
+export interface AutogiroInterfacesLidar {
+  state: AutogiroInterfacesLidarState;
+}
+
+export enum AutogiroInterfacesLidarState {
+  OFF = 0,
+  ON = 1,
+}
+
+export interface AutogiroInterfacesLight {
+  state: AutogiroInterfacesLightState;
+}
+
+export enum AutogiroInterfacesLightState {
+  OFF = 0,
+  STEADY = 1,
+  FLASH = 2,
+}
+
+export interface AutogiroInterfacesMotors {
+  right_mph: number;
+  left_mph: number;
+}
+
+export interface AutogiroInterfacesRefSpeed {
+  left_speed: number;
+  right_speed: number;
+  lat_disp: number;
+  long_disp: number;
+}
+
+export interface AutogiroInterfacesSensorError {
+  joystick_adc_error: boolean;
+  ultrasonic_adc_error: boolean;
+  fingerprint_error: boolean;
+  imu_error: boolean;
+}
+
 export interface AutogiroInterfacesSensors {
   lat_disp: number;
   long_disp: number;
diff --git a/src/autogiro_interfaces/CMakeLists.txt b/src/autogiro_interfaces/CMakeLists.txt
index 6da94bb..88dfc8f 100644
--- a/src/autogiro_interfaces/CMakeLists.txt
+++ b/src/autogiro_interfaces/CMakeLists.txt
@@ -8,6 +8,15 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/Status.msg"
   "msg/FanSpeed.msg"
   "msg/Sensors.msg"
+  "msg/RefSpeed.msg"
+  "msg/Battery.msg"
+  "msg/Brake.msg"
+  "msg/DacValues.msg"
+  "msg/Fingerprint.msg"
+  "msg/Lidar.msg"
+  "msg/Light.msg"
+  "msg/Motors.msg"
+  "msg/SensorError.msg"
 )
 
 ament_package()
diff --git a/src/autogiro_interfaces/msg/Battery.msg b/src/autogiro_interfaces/msg/Battery.msg
new file mode 100644
index 0000000..b218b40
--- /dev/null
+++ b/src/autogiro_interfaces/msg/Battery.msg
@@ -0,0 +1 @@
+int8 battery_percent
diff --git a/src/autogiro_interfaces/msg/Brake.msg b/src/autogiro_interfaces/msg/Brake.msg
new file mode 100644
index 0000000..21a7a3a
--- /dev/null
+++ b/src/autogiro_interfaces/msg/Brake.msg
@@ -0,0 +1 @@
+bool brake
diff --git a/src/autogiro_interfaces/msg/DacValues.msg b/src/autogiro_interfaces/msg/DacValues.msg
new file mode 100644
index 0000000..e9b3742
--- /dev/null
+++ b/src/autogiro_interfaces/msg/DacValues.msg
@@ -0,0 +1,2 @@
+int16 left_dac
+int16 right_dac
diff --git a/src/autogiro_interfaces/msg/Fingerprint.msg b/src/autogiro_interfaces/msg/Fingerprint.msg
new file mode 100644
index 0000000..0e13b14
--- /dev/null
+++ b/src/autogiro_interfaces/msg/Fingerprint.msg
@@ -0,0 +1 @@
+uint8 user_id
diff --git a/src/autogiro_interfaces/msg/Lidar.msg b/src/autogiro_interfaces/msg/Lidar.msg
new file mode 100644
index 0000000..26b982e
--- /dev/null
+++ b/src/autogiro_interfaces/msg/Lidar.msg
@@ -0,0 +1,3 @@
+uint8 OFF=0
+uint8 ON=1
+uint8 state
diff --git a/src/autogiro_interfaces/msg/Light.msg b/src/autogiro_interfaces/msg/Light.msg
new file mode 100644
index 0000000..2a5ef4b
--- /dev/null
+++ b/src/autogiro_interfaces/msg/Light.msg
@@ -0,0 +1,4 @@
+uint8 OFF=0
+uint8 STEADY=1
+uint8 FLASH=2
+uint8 state
diff --git a/src/autogiro_interfaces/msg/Motors.msg b/src/autogiro_interfaces/msg/Motors.msg
new file mode 100644
index 0000000..8a51497
--- /dev/null
+++ b/src/autogiro_interfaces/msg/Motors.msg
@@ -0,0 +1,2 @@
+int8 right_mph
+int8 left_mph
diff --git a/src/autogiro_interfaces/msg/RefSpeed.msg b/src/autogiro_interfaces/msg/RefSpeed.msg
new file mode 100644
index 0000000..948043a
--- /dev/null
+++ b/src/autogiro_interfaces/msg/RefSpeed.msg
@@ -0,0 +1,4 @@
+int8 left_speed
+int8 right_speed
+int8 lat_disp
+int8 long_disp
diff --git a/src/autogiro_interfaces/msg/SensorError.msg b/src/autogiro_interfaces/msg/SensorError.msg
new file mode 100644
index 0000000..6b11719
--- /dev/null
+++ b/src/autogiro_interfaces/msg/SensorError.msg
@@ -0,0 +1,4 @@
+bool joystick_adc_error
+bool ultrasonic_adc_error
+bool fingerprint_error
+bool imu_error