diff --git a/Swerve-Standard/inc/robot.h b/Swerve-Standard/inc/robot.h
index 4c6de26..ec2d643 100644
--- a/Swerve-Standard/inc/robot.h
+++ b/Swerve-Standard/inc/robot.h
@@ -11,12 +11,19 @@ typedef enum Robot_State_e {
     ENABLED
 } Robot_State_e;
 
+typedef enum Locked_State_e {
+    ANGLED,
+    STRAIGHT,
+    RANDOM
+} Locked_State_e;
+
 typedef struct {
     // chassis motion
     float x_speed;
     float y_speed;
     float omega;
     uint8_t IS_SPINTOP_ENABLED;
+    Locked_State_e locked_state;
 
     // power management
     uint16_t power_index;
diff --git a/Swerve-Standard/src/chassis_task.c b/Swerve-Standard/src/chassis_task.c
index 40bc404..ea6c1e4 100644
--- a/Swerve-Standard/src/chassis_task.c
+++ b/Swerve-Standard/src/chassis_task.c
@@ -7,10 +7,12 @@
 #include "referee_system.h"
 #include "swerve_locomotion.h"
 #include "rate_limiter.h"
+#include "pid.h"
 
 extern Robot_State_t g_robot_state;
 extern Remote_t g_remote;
 extern Referee_System_t Referee_System;
+extern DJI_Motor_Handle_t *g_yaw;
 
 DJI_Motor_Handle_t *g_azimuth_motors[NUMBER_OF_MODULES];
 DJI_Motor_Handle_t *g_drive_motors[NUMBER_OF_MODULES];
@@ -18,6 +20,9 @@ swerve_constants_t g_swerve_constants;
 swerve_chassis_state_t g_chassis_state;
 float measured_angles[NUMBER_OF_MODULES];
 
+float gimbal_angle_difference;
+PID_t angle_locking_pid;
+
 rate_limiter_t chassis_vel_limiters[4];
 rate_limiter_t chassis_omega_limiter;
 
@@ -38,10 +43,10 @@ void Chassis_Task_Init()
             },
         .velocity_pid =
             {
-                .kp = 300.0f,
+                .kp = 100.0f,
                 .ki = 0.0f,
-                .kd = 100.0f,
-                .kf = 2000.0f,
+                .kd = 0.0f,
+                .kf = 000.0f,
                 .feedforward_limit = 5000.0f,
                 .integral_limit = 5000.0f,
                 .output_limit = GM6020_MAX_VOLTAGE_INT,
@@ -104,6 +109,9 @@ void Chassis_Task_Init()
     }
     #define SWERVE_MAX_OMEGA_ACCEL (5.0f)
     rate_limiter_init(&chassis_omega_limiter, SWERVE_MAX_OMEGA_ACCEL);
+
+    // Initialize PID for locking
+    PID_Init(&angle_locking_pid, 1, 0, 0, 2 * PI * 1, 0, 0.05);
 }
 
 void Chassis_Ctrl_Loop()
@@ -146,11 +154,49 @@ void Chassis_Ctrl_Loop()
     // g_chassis_state.v_x = g_chassis_state.v_x * cos(theta) - g_chassis_state.v_y * sin(theta);
     // g_chassis_state.v_y = g_chassis_state.v_x * sin(theta) + g_chassis_state.v_y * cos(theta);
 
-    // If spintop enabled, chassis omega set to spintop value
+    // If spintop enabled, chassis omega set to spintop value 
+    // gimbal_angle_difference = g_robot_state.gimbal.yaw_angle;
+    // float chassis_omega_new_target;
+    // if (g_robot_state.chassis.IS_SPINTOP_ENABLED) {
+    //     g_chassis_state.omega = rate_limiter_iterate(&chassis_omega_limiter, Rescale_Chassis_Velocity());
+    // } 
+    // else if (g_robot_state.chassis.locked_state == STRAIGHT) {
+    //     __MAP_ANGLE_TO_UNIT_CIRCLE(gimbal_angle_difference);
+
+    //     gimbal_angle_difference = __MOD_F(gimbal_angle_difference, PI / 2);
+    //     if (gimbal_angle_difference > PI / 4) { // might be easier to go by the way of spin top direction
+    //         gimbal_angle_difference = -1 * ((PI / 2) - gimbal_angle_difference);
+    //     }
+    //     chassis_omega_new_target = PID(&angle_locking_pid, -gimbal_angle_difference);
+    //     __MAX_LIMIT(chassis_omega_new_target, -1 * g_spintop_omega, g_spintop_omega); // might not need this
+    //     g_chassis_state.omega = rate_limiter_iterate(&chassis_omega_limiter, chassis_omega_new_target);
+    // } 
+    // else if (g_robot_state.chassis.locked_state == ANGLED) {
+    //     __MAP_ANGLE_TO_UNIT_CIRCLE(gimbal_angle_difference);
+
+    //     gimbal_angle_difference += PI / 4;
+    //     gimbal_angle_difference = __MOD_F(gimbal_angle_difference, PI / 2);
+    //     if (gimbal_angle_difference > PI / 4) { 
+    //         gimbal_angle_difference = -1 * ((PI / 2) - gimbal_angle_difference);
+    //     }
+    //     chassis_omega_new_target = PID(&angle_locking_pid, -gimbal_angle_difference);
+    //     __MAX_LIMIT(chassis_omega_new_target, -1 * g_spintop_omega, g_spintop_omega); 
+    //     g_chassis_state.omega = rate_limiter_iterate(&chassis_omega_limiter, chassis_omega_new_target);
+    // } 
+    // else { // random locking
+    //     g_chassis_state.omega = rate_limiter_iterate(&chassis_omega_limiter, 0);
+    // }
+    float tmpgimbal_angle_difference = DJI_Motor_Get_Absolute_Angle(g_yaw);
+    //float chassis_omega_new_target;
+
+    // if (g_remote.controller.left_switch )
     if (g_robot_state.chassis.IS_SPINTOP_ENABLED) {
         g_chassis_state.omega = rate_limiter_iterate(&chassis_omega_limiter, Rescale_Chassis_Velocity());
     } else {
-        g_chassis_state.omega = rate_limiter_iterate(&chassis_omega_limiter, g_robot_state.chassis.omega * SWERVE_MAX_ANGLUAR_SPEED);
+        __MAP_ANGLE_TO_UNIT_CIRCLE(tmpgimbal_angle_difference);
+        gimbal_angle_difference = tmpgimbal_angle_difference;
+        g_chassis_state.omega = PID(&angle_locking_pid, gimbal_angle_difference);
+
     }
 
     // Calculate the kinematics of the chassis
@@ -232,4 +278,4 @@ float Rescale_Chassis_Velocity(void) {
     float spin_coeff = chassis_rad * g_spintop_omega / (translation_speed * 2.0f + chassis_rad * g_spintop_omega);
     float target_omega = g_spintop_omega * spin_coeff;
     return target_omega;
-}
\ No newline at end of file
+}
diff --git a/Swerve-Standard/src/robot.c b/Swerve-Standard/src/robot.c
index 5231d74..19874f9 100644
--- a/Swerve-Standard/src/robot.c
+++ b/Swerve-Standard/src/robot.c
@@ -133,6 +133,7 @@ void Process_Remote_Input()
     {
         g_robot_state.IS_SUPER_CAPACITOR_ENABLED = 1;
     } else {
+
         g_robot_state.IS_SUPER_CAPACITOR_ENABLED = 0;
     }
 
@@ -156,16 +157,37 @@ void Process_Remote_Input()
         g_robot_state.chassis.IS_SPINTOP_ENABLED ^= 0x01;
     }
 
+    if (g_remote.keyboard.R) {
+        g_robot_state.chassis.locked_state = RANDOM;
+    }
+
+    if (g_remote.keyboard.E) {
+        g_robot_state.chassis.locked_state = STRAIGHT;
+    }
+
+    if (g_remote.keyboard.Q) {
+        g_robot_state.chassis.locked_state = ANGLED;
+    }
+
     if (g_remote.controller.left_switch == UP) { // Left switch high to enable spintop
         //g_robot_state.chassis.IS_SPINTOP_ENABLED = 1;
         //g_robtot_state.launch.IS_FIRING_ENABLED = 1;
         g_robot_state.launch.IS_AUTO_AIMING_ENABLED = 1;
     } else {
-        //g_robot_state.chassis.IS_SPINTOP_ENABLED = 0;
+        g_robot_state.chassis.IS_SPINTOP_ENABLED = 0;
         //g_robot_state.launch.IS_FIRING_ENABLED = 0;
         g_robot_state.launch.IS_AUTO_AIMING_ENABLED = 0;
     }
 
+    if (g_remote.controller.left_switch == MID)
+    {
+        g_robot_state.chassis.IS_SPINTOP_ENABLED = 1;
+    }
+    else
+    {
+        g_robot_state.chassis.IS_SPINTOP_ENABLED = 0;
+    }
+
     // Update previous states keyboard
     g_input_state.prev_B = g_remote.keyboard.B;
     g_input_state.prev_G = g_remote.keyboard.G;
diff --git a/control-base/algo/inc/user_math.h b/control-base/algo/inc/user_math.h
index 727d53d..e6ef97d 100644
--- a/control-base/algo/inc/user_math.h
+++ b/control-base/algo/inc/user_math.h
@@ -70,4 +70,14 @@
 #define __SLEW_RATE_LIMIT(curr, delta, ramp) \
     curr = (1 - (ramp)) * (curr) + (ramp) * (delta)
 
+#define __ABS(x)         \
+    (x < 0 ? -1 * x : x) \
+
+// floor for floats
+#define __FLOOR_F(x)                                                                      \
+    (x < 0 && __ABS(x - ((int) x)) > 0.0001 ? (float)((int) x) - 1.0 : (float)((int) x))  \
+
+#define __MOD_F(a, b)           \
+    a - b * __FLOOR_F((a / b))  \
+
 #endif // USER_MATH_H
\ No newline at end of file