diff --git a/MicrocontrollerCode/include/JoystickFunctions.h b/MicrocontrollerCode/include/JoystickFunctions.h
index 24ebffc..fe19bf4 100644
--- a/MicrocontrollerCode/include/JoystickFunctions.h
+++ b/MicrocontrollerCode/include/JoystickFunctions.h
@@ -15,6 +15,14 @@ struct RefSpeed {
     int8_t rightSpeed;     ///< Speed of the right wheel.
 };
 
+/**
+ * Struct representing the reference displace
+ */
+struct RefDisplacement {
+    int8_t longDisp;      ///< Forward/Backward displacement with + indicating forward
+    int8_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
@@ -22,6 +30,13 @@ struct RefSpeed {
  */
 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
diff --git a/MicrocontrollerCode/src/JoystickFunctions.cpp b/MicrocontrollerCode/src/JoystickFunctions.cpp
index 2031ee5..ef0371d 100644
--- a/MicrocontrollerCode/src/JoystickFunctions.cpp
+++ b/MicrocontrollerCode/src/JoystickFunctions.cpp
@@ -9,6 +9,28 @@
 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);
+    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
diff --git a/MicrocontrollerCode/src/main.cpp b/MicrocontrollerCode/src/main.cpp
index ed4273f..38888f1 100644
--- a/MicrocontrollerCode/src/main.cpp
+++ b/MicrocontrollerCode/src/main.cpp
@@ -104,10 +104,14 @@ void loop() {
     //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{};
@@ -147,7 +151,7 @@ void loop() {
 
     microRosTick();
 
-    transmitMsg(omegaRef, usDistances, pirSensors, fanSpeeds, imuData);
+    transmitMsg(thetaRef,omegaRef, usDistances, pirSensors, fanSpeeds, imuData);
 
     if (currentMillis - lastErrorTime >= error_timer) {
         lastErrorTime = currentMillis;
@@ -160,7 +164,7 @@ void loop() {
     #elif ROS_DEBUG
 
 
-    transmitMsg(omegaRef);
+    transmitMsg(thetaRef,omegaRef);
 
 
     #elif DEBUG
diff --git a/MicrocontrollerCode/src/microRosFunctions.cpp b/MicrocontrollerCode/src/microRosFunctions.cpp
index df10906..5127ea7 100644
--- a/MicrocontrollerCode/src/microRosFunctions.cpp
+++ b/MicrocontrollerCode/src/microRosFunctions.cpp
@@ -373,7 +373,9 @@ static void lidar_subscription_callback(const void *msgin){
     }
 
 #ifdef ROS
-void transmitMsg(RefSpeed omegaRef, USData ultrasonicData, PIRSensors pirSensors, FanSpeeds fanSpeeds, IMUData imuData) {
+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;
@@ -406,9 +408,11 @@ void transmitMsg(RefSpeed omegaRef, USData ultrasonicData, PIRSensors pirSensors
 
 #elif ROS_DEBUG
 
-void transmitMsg(RefSpeed omegaRef){
+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)));
 }