diff --git a/src/algo/src/Swerve_Locomotion.c b/src/algo/src/Swerve_Locomotion.c
index 924d17e..265b70b 100644
--- a/src/algo/src/Swerve_Locomotion.c
+++ b/src/algo/src/Swerve_Locomotion.c
@@ -7,7 +7,6 @@
 Swerve_Module_t g_swerve_fl, g_swerve_rl, g_swerve_rr, g_swerve_fr;
 Swerve_Module_t *swerve_modules[NUMBER_OF_MODULES] = {&g_swerve_fl, &g_swerve_rl, &g_swerve_rr, &g_swerve_fr};
 float last_swerve_angle[NUMBER_OF_MODULES] = {0.0f, 0.0f, 0.0f, 0.0f};
-Kalman_Filter_t power_KF = {.Prev_P = 1.0f, .Q = 0.0001, .R = 5.0f};
 
 /**
  * @brief Inverse kinematics matrix for a 4 module swerve, defined counterclockwise from the front left
@@ -42,14 +41,14 @@ void Set_Module_Output(Swerve_Module_t *swerve_module, Module_State_t desired_st
 void Swerve_Init()
 {
     // define constants for each module in an array [0] == fl, [1] == rl, [2] == rr, [3] == fr
-    int azimuth_can_bus_array[NUMBER_OF_MODULES] = {1, 1, 2, 2};
+    int azimuth_can_bus_array[NUMBER_OF_MODULES] = {2, 2, 2, 2};
     int azimuth_speed_controller_id_array[NUMBER_OF_MODULES] = {1, 2, 3, 4};
-    int azimuth_offset_array[NUMBER_OF_MODULES] = {1990, 730, 6060, 1362};
+    int azimuth_offset_array[NUMBER_OF_MODULES] = {2050, 1940, 1430, 150};
     Motor_Reversal_t azimuth_motor_reversal_array[NUMBER_OF_MODULES] = {MOTOR_REVERSAL_REVERSED, MOTOR_REVERSAL_REVERSED, MOTOR_REVERSAL_REVERSED, MOTOR_REVERSAL_REVERSED};
 
-    int drive_can_bus_array[NUMBER_OF_MODULES] = {1, 1, 2, 2};
+    int drive_can_bus_array[NUMBER_OF_MODULES] = {1, 2, 2, 2};
     int drive_speed_controller_id_array[NUMBER_OF_MODULES] = {1, 2, 3, 4};
-    Motor_Reversal_t drive_motor_reversal_array[NUMBER_OF_MODULES] = {MOTOR_REVERSAL_REVERSED, MOTOR_REVERSAL_REVERSED, MOTOR_REVERSAL_NORMAL, MOTOR_REVERSAL_NORMAL};
+    Motor_Reversal_t drive_motor_reversal_array[NUMBER_OF_MODULES] = {MOTOR_REVERSAL_NORMAL, MOTOR_REVERSAL_REVERSED, MOTOR_REVERSAL_REVERSED, MOTOR_REVERSAL_NORMAL};
 
     // init common PID configuration for azimuth motors
     Motor_Config_t azimuth_motor_config = {
diff --git a/src/app/src/debug_task.c b/src/app/src/debug_task.c
index ff39cbb..71add01 100644
--- a/src/app/src/debug_task.c
+++ b/src/app/src/debug_task.c
@@ -15,10 +15,9 @@
 extern Robot_State_t g_robot_state;
 extern DJI_Motor_Handle_t *g_yaw;
 extern IMU_t g_imu;
-extern Swerve_Module_t g_swerve_fl;
+extern Swerve_Module_t g_swerve_rr;
 extern Remote_t g_remote; 
 extern Launch_Target_t g_launch_target;
-extern uint64_t t;
 extern Daemon_Instance_t *g_daemon_instances[3];
 extern Daemon_Instance_t *g_remote_daemon;
 extern Daemon_Instance_t *g_referee_daemon_instance_ptr;
@@ -53,7 +52,7 @@ void Debug_Task_Loop(void)
     if (counter > 0xFFFFFFFF) {
         counter = 0;
     }
-    DEBUG_PRINTF(&huart6, ">calc:%d\n>ref:%d\n",g_launch_target.calculated_heat,Referee_Robot_State.Shooter_Heat_1);
-    DEBUG_PRINTF(&huart6, ">feed_angle:%f\n>total_angle:%f\n",g_launch_target.feed_angle,g_motor_feed->stats->total_angle_rad);
+    DEBUG_PRINTF(&huart6, ">fl:%d\n>rl:%d\n>rr:%d\n>fr:%d\n",g_swerve_rr.drive_motor->velocity_pid->ref);
+    //DEBUG_PRINTF(&huart6, ">feed_angle:%f\n>total_angle:%f\n",g_launch_target.feed_angle,g_motor_feed->stats->total_angle_rad);
 #endif
 }
\ No newline at end of file
diff --git a/src/app/src/gimbal_task.c b/src/app/src/gimbal_task.c
index 4a02998..ac05334 100644
--- a/src/app/src/gimbal_task.c
+++ b/src/app/src/gimbal_task.c
@@ -42,7 +42,7 @@ void Gimbal_Task_Init()
     Motor_Config_t yaw_motor_config = {
         .can_bus = 1,
         .speed_controller_id = 3,
-        .offset = 3690,
+        .offset = 2400,
         .control_mode = POSITION_VELOCITY_SERIES,
         .motor_reversal = MOTOR_REVERSAL_NORMAL,
         .use_external_feedback = 1,
@@ -67,7 +67,7 @@ void Gimbal_Task_Init()
     };
 
     Motor_Config_t pitch_motor_config = {
-        .can_bus = 2,
+        .can_bus = 1,
         .speed_controller_id = 2,
         .offset = 4460,
         .use_external_feedback = 1,
diff --git a/src/app/src/launch_task.c b/src/app/src/launch_task.c
index cbd46a6..a496312 100644
--- a/src/app/src/launch_task.c
+++ b/src/app/src/launch_task.c
@@ -16,8 +16,8 @@ void Feed_Angle_Calc(void);
 
 void Launch_Task_Init() {
     Motor_Config_t flywheel_left_config = {
-        .can_bus = 2,
-        .speed_controller_id = 1,
+        .can_bus = 1,
+        .speed_controller_id = 4,
         .offset = 0,
         .control_mode = VELOCITY_CONTROL,
         .motor_reversal = MOTOR_REVERSAL_REVERSED,
@@ -29,8 +29,8 @@ void Launch_Task_Init() {
     };
 
     Motor_Config_t flywheel_right_config = {
-        .can_bus = 2,
-        .speed_controller_id = 2,
+        .can_bus = 1,
+        .speed_controller_id = 5,
         .offset = 0,
         .control_mode = VELOCITY_CONTROL,
         .motor_reversal = MOTOR_REVERSAL_NORMAL,
@@ -43,7 +43,7 @@ void Launch_Task_Init() {
 
     Motor_Config_t feed_speed_config = {
         .can_bus = 1,
-        .speed_controller_id = 3,
+        .speed_controller_id = 2,
         .offset = 0,
         .control_mode = VELOCITY_CONTROL | POSITION_CONTROL,
         .motor_reversal = MOTOR_REVERSAL_NORMAL,
diff --git a/src/app/src/robot.c b/src/app/src/robot.c
index 99d3165..6235932 100644
--- a/src/app/src/robot.c
+++ b/src/app/src/robot.c
@@ -14,6 +14,7 @@
 #include "buzzer.h"
 #include "ui.h"
 #include "user_math.h"
+#include "Swerve_Locomotion.h"
 
 extern DJI_Motor_Handle_t *g_yaw;
 
diff --git a/src/app/src/ui_task.c b/src/app/src/ui_task.c
index 14c53bc..c2a6a49 100644
--- a/src/app/src/ui_task.c
+++ b/src/app/src/ui_task.c
@@ -2,7 +2,7 @@
 
 void UI_Task_Loop(void)
 {
-    ui_self_id = 3;//Referee_Robot_State.ID;
+    ui_self_id = Referee_Robot_State.ID;
     if (!g_robot_state.UI_enabled)
     {
         ui_remove_indicator_0();