diff --git a/taproot b/taproot
index 22b708e6..ffbeda8a 160000
--- a/taproot
+++ b/taproot
@@ -1 +1 @@
-Subproject commit 22b708e6e1833464914ff7b2279b0cd7214e023f
+Subproject commit ffbeda8a64b2c063abcd615fce1cb7a48d5ccabf
diff --git a/ut-robomaster/project.xml b/ut-robomaster/project.xml
index 7a9c89cf..525dd801 100644
--- a/ut-robomaster/project.xml
+++ b/ut-robomaster/project.xml
@@ -32,6 +32,7 @@
     <option name="taproot:modm-project.xml:modm_hal_modules">
       modm:platform:i2c:2
       modm:platform:rtt
+      modm:platform:spi:2
     </option>
 
     <option name="taproot:modm-project.xml:modm_hal_options">
diff --git a/ut-robomaster/src/communication/cv_board.cpp b/ut-robomaster/src/communication/cv_board.cpp
index db545381..584b1b51 100644
--- a/ut-robomaster/src/communication/cv_board.cpp
+++ b/ut-robomaster/src/communication/cv_board.cpp
@@ -24,6 +24,11 @@ void CVBoard::messageReceiveCallback(const ReceivedSerialMessage& message)
         case CV_MESSAGE_TYPE_TURRET_AIM:
             decodeTurretData(message);
             break;
+        // case CV_MESSAGE_TYPE_ECHO:
+        // echoData(message);
+        case CV_MESSAGE_TYPE_POSITION_REQUEST:
+            decodePositionRequest(message);
+            break;
         default:
             break;
     }
@@ -35,6 +40,22 @@ void CVBoard::sendMessage()
     sendColorData();
 }
 
+//  void CVBoard::echoData(const ReceivedSerialMessage& message)
+// {
+//     std::string data_string;
+//     memcpy(&data_string, &message.data, sizeof(message.header.dataLength));
+
+//     DJISerial::SerialMessage<sizeof(data_string)> msg;
+//     msg.messageType = CV_MESSAGE_TYPE_ECHO;
+
+//     std::string* data = reinterpret_cast<std::string*>(msg.data);
+
+//     data = &data_string;
+
+//     msg.setCRC16();
+//     drivers->uart.write(UART_PORT, reinterpret_cast<uint8_t*>(&msg), sizeof(msg));
+// }
+
 void CVBoard::sendOdometryData()
 {
     DJISerial::SerialMessage<sizeof(OdometryData)> message;
@@ -77,6 +98,17 @@ bool CVBoard::decodeTurretData(const ReceivedSerialMessage& message)
     return false;
 }
 
+bool CVBoard::decodePositionRequest(const ReceivedSerialMessage& message)
+{
+    if (message.header.dataLength == sizeof(lastPositionRequest))
+    {
+        memcpy(&lastPositionRequest, &message.data, sizeof(lastPositionRequest));
+        return true;
+    }
+    return false;
+}
+
 bool CVBoard::isOnline() const { return !offlineTimeout.isExpired(); }
 const TurretData& CVBoard::getTurretData() const { return lastTurretData; }
+const PositionRequest& CVBoard::getPositionRequest() const { return lastPositionRequest; }
 }  // namespace communication
diff --git a/ut-robomaster/src/communication/cv_board.hpp b/ut-robomaster/src/communication/cv_board.hpp
index 89e00d0e..9949cade 100644
--- a/ut-robomaster/src/communication/cv_board.hpp
+++ b/ut-robomaster/src/communication/cv_board.hpp
@@ -43,6 +43,11 @@ class CVBoard : public tap::communication::serial::DJISerial
      */
     void sendMessage();
 
+    /**
+     * Echo the message sent by the CV board to the CV board
+     */
+    void echoData(const ReceivedSerialMessage& message);
+
     /**
      * Sends odometry data to the CV board
      */
@@ -61,6 +66,18 @@ class CVBoard : public tap::communication::serial::DJISerial
      */
     bool decodeTurretData(const ReceivedSerialMessage& message);
 
+    /**
+     * Decode a request to move to a position.
+     * @param message message from CV board
+     * @return true if valid, false otherwise
+     */
+    bool decodePositionRequest(const ReceivedSerialMessage& message);
+
+    /**
+     * Fetch the position request from the CV board
+     */
+    const PositionRequest& getPositionRequest() const;
+
     /**
      * @return true if a message has been received within the last OFFLINE_TIMEOUT_MS milliseconds,
      * false if otherwise
@@ -84,6 +101,11 @@ class CVBoard : public tap::communication::serial::DJISerial
     /** Last turret aiming data received from the CV board */
     TurretData lastTurretData;
 
+    PositionRequest lastPositionRequest = {
+        .x_requested = -999.9f,
+        .y_requested = -999.9f,
+        .time_sent = 0xdeadbeef};
+
     /** UART port used to communicate with the CV board */
     static constexpr Uart::UartPort UART_PORT = Uart::Uart1;
 
diff --git a/ut-robomaster/src/communication/cv_message.hpp b/ut-robomaster/src/communication/cv_message.hpp
index 263164df..146f406c 100644
--- a/ut-robomaster/src/communication/cv_message.hpp
+++ b/ut-robomaster/src/communication/cv_message.hpp
@@ -1,6 +1,8 @@
 #pragma once
 #include <stdint.h>
 
+#include <string>
+
 namespace communication
 {
 
@@ -9,6 +11,8 @@ enum MessageTypes : uint8_t
     CV_MESSAGE_TYPE_ODOMETRY_DATA = 1,
     CV_MESSAGE_TYPE_TURRET_AIM = 2,
     CV_MESSAGE_TYPE_COLOR_DATA = 3,
+    CV_MESSAGE_TYPE_ECHO = 4,
+    CV_MESSAGE_TYPE_POSITION_REQUEST = 5,
 };
 
 enum ColorTypes : uint8_t
@@ -35,6 +39,13 @@ struct TurretData
     bool hasTarget;
 } modm_packed;
 
+struct PositionRequest
+{
+    float x_requested;
+    float y_requested;
+    uint64_t time_sent;
+} modm_packed;
+
 struct OdometryData
 {
     float xPos;
diff --git a/ut-robomaster/src/main.cpp b/ut-robomaster/src/main.cpp
index 21f43fe3..b48046cf 100644
--- a/ut-robomaster/src/main.cpp
+++ b/ut-robomaster/src/main.cpp
@@ -1,5 +1,6 @@
 #include "tap/architecture/periodic_timer.hpp"
 #include "tap/architecture/profiler.hpp"
+#include "tap/motor/motorsim/dji_motor_sim_handler.hpp"
 
 #include "robots/robot_constants.hpp"
 #include "robots/robot_control.hpp"
@@ -24,7 +25,7 @@ static void initializeIo(src::Drivers *drivers)
     drivers->schedulerTerminalHandler.init();
     drivers->djiMotorTerminalSerialHandler.init();
     drivers->bmi088.initialize(IMU_SAMPLE_FREQUENCY, IMU_KP, IMU_KI);
-    drivers->bmi088.requestRecalibration();
+    drivers->bmi088.requestCalibration();
 }
 
 // Anything that you would like to be called place here. It will be called
@@ -33,7 +34,7 @@ static void initializeIo(src::Drivers *drivers)
 static void updateIo(src::Drivers *drivers)
 {
 #ifdef PLATFORM_HOSTED
-    tap::motorsim::SimHandler::updateSims();
+    tap::motor::motorsim::DjiMotorSimHandler::getInstance()->updateSims();
 #endif
 
     drivers->canRxHandler.pollCanData();
diff --git a/ut-robomaster/src/robots/hero/hero_constants.hpp b/ut-robomaster/src/robots/hero/hero_constants.hpp
index 0af76a76..39345cd7 100644
--- a/ut-robomaster/src/robots/hero/hero_constants.hpp
+++ b/ut-robomaster/src/robots/hero/hero_constants.hpp
@@ -2,6 +2,7 @@
 
 #include "tap/algorithms/smooth_pid.hpp"
 #include "tap/communication/can/can_bus.hpp"
+#include "tap/communication/serial/ref_serial.hpp"
 #include "tap/motor/dji_motor.hpp"
 
 #include "modm/container/pair.hpp"
@@ -13,6 +14,7 @@ using motor_controller::PidConstants;
 using tap::can::CanBus;
 using namespace tap::motor;
 using namespace motors;
+using tap::communication::serial::RefSerial;
 
 // General constants ------------------------------------------------
 
@@ -41,10 +43,12 @@ constexpr float CAMERA_TO_PITCH = 0.0f;    // distance from main camera lens to
 constexpr float NOZZLE_TO_PITCH = 0.0f;    // distance from barrel nozzle to pitch axis (m)
 constexpr float CAMERA_TO_BARRELS = 0.0f;  // vertical ctc offset from camera lens to barrel (m)
 constexpr float CAMERA_X_OFFSET = 0.0f;    // horizontal offset of main camera lens (m)
-constexpr float PITCH_MIN = -0.3349f;      // rad
-constexpr float PITCH_MAX = 0.3534f;       // rad
+constexpr float PITCH_MIN = 0.15f;         // rad
+constexpr float PITCH_MAX = 0.60f;         // rad
+const float YAW_OFFSET = 0;                // deprecated on hero until gear ratios are fixed
+const float PITCH_OFFSET = 1.90f;
 
-static constexpr float YAW_REDUCTION = 1.0f;
+static constexpr float YAW_REDUCTION = 2.005459491f;
 static constexpr float PITCH_REDUCTION = 1.0f;
 
 // Tuning Constants -----------------------------------
@@ -95,8 +99,8 @@ static constexpr float YAW_INPUT_SCALE = 4.0f;
 static constexpr float PITCH_INPUT_SCALE = 5.0f;
 #endif
 
-static constexpr float MOUSE_SENS_YAW = 0.0045f;
-static constexpr float MOUSE_SENS_PITCH = 0.002f;
+static constexpr float MOUSE_SENS_YAW = 0.0090f;
+static constexpr float MOUSE_SENS_PITCH = 0.0025f;
 
 // Motor Constants -------------------------------------
 
@@ -105,28 +109,60 @@ constexpr CanBus CAN_TURRET = CanBus::CAN_BUS1;
 constexpr CanBus CAN_SHOOTER = CanBus::CAN_BUS2;
 
 // chassis
-const MotorConfig WHEEL_LF{M3508, MOTOR2, CAN_WHEELS, true, "left front wheel", PID_WHEELS, {}};
-const MotorConfig WHEEL_RF{M3508, MOTOR1, CAN_WHEELS, false, "right front wheel", PID_WHEELS, {}};
-const MotorConfig WHEEL_LB{M3508, MOTOR3, CAN_WHEELS, true, "left back wheel", PID_WHEELS, {}};
-const MotorConfig WHEEL_RB{M3508, MOTOR4, CAN_WHEELS, false, "right back wheel", PID_WHEELS, {}};
-
-// flywheels
 const MotorConfig
-    FLYWHEEL_L{M3508_NOGEARBOX, MOTOR3, CAN_SHOOTER, false, "flywheel left", PID_FLYWHEEL, {}};
+    WHEEL_LF{M3508, MOTOR2, CAN_WHEELS, true, "left front wheel", PID_WHEELS, {}, false};
+const MotorConfig
+    WHEEL_RF{M3508, MOTOR1, CAN_WHEELS, false, "right front wheel", PID_WHEELS, {}, false};
+const MotorConfig
+    WHEEL_LB{M3508, MOTOR3, CAN_WHEELS, true, "left back wheel", PID_WHEELS, {}, false};
 const MotorConfig
-    FLYWHEEL_R{M3508_NOGEARBOX, MOTOR4, CAN_SHOOTER, true, "flywheel right", PID_FLYWHEEL, {}};
+    WHEEL_RB{M3508, MOTOR4, CAN_WHEELS, false, "right back wheel", PID_WHEELS, {}, false};
+
+// flywheels
+const MotorConfig FLYWHEEL_L{
+    M3508_NOGEARBOX,
+    MOTOR3,
+    CAN_SHOOTER,
+    false,
+    "flywheel left",
+    PID_FLYWHEEL,
+    {},
+    false};
+const MotorConfig FLYWHEEL_R{
+    M3508_NOGEARBOX,
+    MOTOR4,
+    CAN_SHOOTER,
+    true,
+    "flywheel right",
+    PID_FLYWHEEL,
+    {},
+    false};
 
 // agitator
-const MotorConfig AGITATOR{M3508, MOTOR1, CAN_SHOOTER, false, "agitator", PID_AGITATOR, {}};
-const MotorConfig FEEDER{M2006, MOTOR2, CAN_SHOOTER, false, "feeder", PID_FEEDER, {}};
+const MotorConfig AGITATOR{
+    M3508,
+    MOTOR1,
+    CAN_SHOOTER,
+    false,
+    "agitator",
+    PID_AGITATOR,
+    {},
+    false};  // water wheel
+const MotorConfig FEEDER{M2006, MOTOR2, CAN_SHOOTER, false, "feeder", PID_FEEDER, {}, false};
 
 // turret
+const MotorConfig YAW_L{
+    M3508,
+    MOTOR5,
+    CAN_TURRET,
+    false,
+    "yaw left",
+    {2.5f, 60.0f, 0.0f},
+    {45.0f, 0.0f, 0.0f},
+    false};
+const MotorConfig YAW_R{M3508, MOTOR6, CAN_TURRET, false, "yaw right", {}, {}, false};
 const MotorConfig
-    YAW_L{M3508, MOTOR5, CAN_TURRET, false, "yaw left", {2.5f, 60.0f, 0.0f}, {45.0f, 0.0f, 0.0f}};
-const MotorConfig YAW_R{M3508, MOTOR6, CAN_TURRET, false, "yaw right", {}, {}};
-const MotorConfig PITCH{GM6020, MOTOR7, CAN_TURRET, false, "pitch", PID_VELOCITY_DEFAULT, {}};
-const float YAW_OFFSET = 0;
-const float PITCH_OFFSET = 0;
+    PITCH{GM6020, MOTOR7, CAN_TURRET, false, "pitch", PID_VELOCITY_DEFAULT, {}, false};
 
 // Velocities ----------------------------
 
@@ -153,4 +189,21 @@ const float UNJAM_SPEED = 12.0f;          // rev/s
 
 // Heat Buffers ---------------------------
 
-const uint16_t BARREL_HEAT_BUFFER = 100.0f;
+const uint16_t BARREL_HEAT_BUFFER_1V1 = 100;
+const uint16_t BARREL_HEAT_BUFFER_3V3[10] = {
+    100,  // Level 1
+    100,  // Level 2
+    100,  // Level 3
+    100,  // Level 4
+    100,  // Level 5
+    100,  // Level 6
+    100,  // Level 7
+    100,  // Level 8
+    100,  // Level 9
+    100   // Level 10
+};
+
+constexpr bool DEBUG_HEAT_BUFFER_ENABLED = true;
+const RefSerial::Rx::GameType DEBUG_HEAT_BUFFER_GAME_TYPE =
+    RefSerial::Rx::GameType::ROBOMASTER_RMUL_3V3;
+const uint8_t DEBUG_HEAT_BUFFER_ROBOT_LEVEL = 1;
\ No newline at end of file
diff --git a/ut-robomaster/src/robots/robot_constants.hpp b/ut-robomaster/src/robots/robot_constants.hpp
index f3d30450..0f13b737 100644
--- a/ut-robomaster/src/robots/robot_constants.hpp
+++ b/ut-robomaster/src/robots/robot_constants.hpp
@@ -10,8 +10,8 @@ const uint32_t REFRESH_PERIOD = 2;              // ms
 constexpr float DT = REFRESH_PERIOD / 1000.0f;  // refresh delta time (s)
 
 const float ANALOG_DEAD_ZONE = 0.1f;
-const float KEYBOARD_ACCEL = 5.0f;
-const float KEYBOARD_DECEL = 5.0f;
+const float KEYBOARD_ACCEL = 3.5f;
+const float KEYBOARD_DECEL = 3.5f;
 const float CHASSIS_AUTOALIGN_FACTOR = 0.5f;
 const float CHASSIS_AUTOALIGN_ANGLE = M_PI;
 
diff --git a/ut-robomaster/src/robots/sentry/sentry_constants.hpp b/ut-robomaster/src/robots/sentry/sentry_constants.hpp
index b4c0d2d2..7901875a 100644
--- a/ut-robomaster/src/robots/sentry/sentry_constants.hpp
+++ b/ut-robomaster/src/robots/sentry/sentry_constants.hpp
@@ -2,6 +2,7 @@
 
 #include "tap/algorithms/smooth_pid.hpp"
 #include "tap/communication/can/can_bus.hpp"
+#include "tap/communication/serial/ref_serial.hpp"
 #include "tap/motor/dji_motor.hpp"
 
 #include "modm/container/pair.hpp"
@@ -13,6 +14,7 @@ using motor_controller::PidConstants;
 using tap::can::CanBus;
 using namespace tap::motor;
 using namespace motors;
+using tap::communication::serial::RefSerial;
 
 // General constants ------------------------------------------------
 
@@ -25,8 +27,8 @@ const Sound SOUND_STARTUP = SOUND_SMB_POWERUP;
 
 // chassis ------------
 static constexpr int WHEELS = 4;
-static constexpr float WHEEL_DISTANCE_X = 0.391f;  // meters
-static constexpr float WHEEL_DISTANCE_Y = 0.315f;  // meters
+static constexpr float WHEEL_DISTANCE_X = 0.346f;  // meters
+static constexpr float WHEEL_DISTANCE_Y = 0.346f;  // meters
 static constexpr float WHEEL_RADIUS = 0.1524f;     // meters
 static constexpr float WHEEL_LXY = (WHEEL_DISTANCE_X + WHEEL_DISTANCE_Y) / 2.0f;
 
@@ -100,14 +102,25 @@ constexpr CanBus CAN_TURRET = CanBus::CAN_BUS1;
 constexpr CanBus CAN_SHOOTER = CanBus::CAN_BUS2;
 
 // chassis
-const MotorConfig WHEEL_LF{M3508, MOTOR2, CAN_WHEELS, true, "left front wheel", PID_WHEELS, {}};
-const MotorConfig WHEEL_RF{M3508, MOTOR1, CAN_WHEELS, false, "right front wheel", PID_WHEELS, {}};
-const MotorConfig WHEEL_LB{M3508, MOTOR3, CAN_WHEELS, true, "left back wheel", PID_WHEELS, {}};
-const MotorConfig WHEEL_RB{M3508, MOTOR4, CAN_WHEELS, false, "right back wheel", PID_WHEELS, {}};
+const MotorConfig
+    WHEEL_LF{M3508, MOTOR2, CAN_WHEELS, true, "left front wheel", PID_WHEELS, {}, false};
+const MotorConfig
+    WHEEL_RF{M3508, MOTOR1, CAN_WHEELS, false, "right front wheel", PID_WHEELS, {}, false};
+const MotorConfig
+    WHEEL_LB{M3508, MOTOR3, CAN_WHEELS, true, "left back wheel", PID_WHEELS, {}, false};
+const MotorConfig
+    WHEEL_RB{M3508, MOTOR4, CAN_WHEELS, false, "right back wheel", PID_WHEELS, {}, false};
 
 // flywheels
-const MotorConfig
-    FLYWHEEL_TL{M3508_NOGEARBOX, MOTOR3, CAN_SHOOTER, true, "flywheel top left", PID_FLYWHEEL, {}};
+const MotorConfig FLYWHEEL_TL{
+    M3508_NOGEARBOX,
+    MOTOR3,
+    CAN_SHOOTER,
+    true,
+    "flywheel top left",
+    PID_FLYWHEEL,
+    {},
+    false};
 const MotorConfig FLYWHEEL_TR{
     M3508_NOGEARBOX,
     MOTOR4,
@@ -115,7 +128,8 @@ const MotorConfig FLYWHEEL_TR{
     false,
     "flywheel top right",
     PID_FLYWHEEL,
-    {}};
+    {},
+    false};
 const MotorConfig FLYWHEEL_BL{
     M3508_NOGEARBOX,
     MOTOR5,
@@ -123,7 +137,8 @@ const MotorConfig FLYWHEEL_BL{
     false,
     "flywheel bottom left",
     PID_FLYWHEEL,
-    {}};
+    {},
+    false};
 const MotorConfig FLYWHEEL_BR{
     M3508_NOGEARBOX,
     MOTOR6,
@@ -131,15 +146,19 @@ const MotorConfig FLYWHEEL_BR{
     true,
     "flywheel bottom right",
     PID_FLYWHEEL,
-    {}};
+    {},
+    false};
 
 // agitator
-const MotorConfig AGITATOR_L{M2006, MOTOR1, CAN_SHOOTER, false, "agitator left", PID_AGITATOR, {}};
-const MotorConfig AGITATOR_R{M2006, MOTOR2, CAN_SHOOTER, true, "agitator right", PID_AGITATOR, {}};
+const MotorConfig
+    AGITATOR_L{M2006, MOTOR1, CAN_SHOOTER, false, "agitator left", PID_AGITATOR, {}, false};
+const MotorConfig
+    AGITATOR_R{M2006, MOTOR2, CAN_SHOOTER, true, "agitator right", PID_AGITATOR, {}, false};
 
 // turret
-const MotorConfig YAW{GM6020, MOTOR6, CAN_TURRET, false, "yaw", PID_VELOCITY_DEFAULT, {}};
-const MotorConfig PITCH{GM6020, MOTOR7, CAN_TURRET, false, "pitch", PID_VELOCITY_DEFAULT, {}};
+const MotorConfig YAW{GM6020, MOTOR6, CAN_TURRET, false, "yaw", PID_VELOCITY_DEFAULT, {}, false};
+const MotorConfig
+    PITCH{GM6020, MOTOR7, CAN_TURRET, false, "pitch", PID_VELOCITY_DEFAULT, {}, false};
 const float YAW_OFFSET = 0;
 const float PITCH_OFFSET = 0;
 
@@ -167,4 +186,21 @@ const float UNJAM_SPEED = 15.0f;          // rev/s
 
 // Heat Buffers -------------------------------------
 
-const uint16_t BARREL_HEAT_BUFFER = 20.0f;
+const uint16_t BARREL_HEAT_BUFFER_1V1 = 50;
+const uint16_t BARREL_HEAT_BUFFER_3V3[10] = {
+    40,  // Level 1
+    50,  // Level 2
+    50,  // Level 3
+    50,  // Level 4
+    50,  // Level 5
+    50,  // Level 6
+    50,  // Level 7
+    50,  // Level 8
+    50,  // Level 9
+    50   // Level 10
+};
+
+constexpr bool DEBUG_HEAT_BUFFER_ENABLED = true;
+const RefSerial::Rx::GameType DEBUG_HEAT_BUFFER_GAME_TYPE =
+    RefSerial::Rx::GameType::ROBOMASTER_RMUL_3V3;
+const uint8_t DEBUG_HEAT_BUFFER_ROBOT_LEVEL = 1;
\ No newline at end of file
diff --git a/ut-robomaster/src/robots/sentry/sentry_control.hpp b/ut-robomaster/src/robots/sentry/sentry_control.hpp
index f521071c..c90c1f91 100644
--- a/ut-robomaster/src/robots/sentry/sentry_control.hpp
+++ b/ut-robomaster/src/robots/sentry/sentry_control.hpp
@@ -34,7 +34,7 @@ class SentryControl : CommonControlManual
     CommandAgitatorContinuous rotateAgitatorL{drivers, &agitatorL, BarrelId::STANDARD1};
     CommandAgitatorContinuous rotateAgitatorR{drivers, &agitatorR, BarrelId::STANDARD2};
 
-    CommandSentryPosition sentryPosition{drivers, &chassis};
+    CommandSentryPosition sentryPosition{drivers, &chassis, &turret};
     CommandSentryAim sentryAim{drivers, &turret};
 
     // Mappings
diff --git a/ut-robomaster/src/robots/standard/standard_constants.hpp b/ut-robomaster/src/robots/standard/standard_constants.hpp
index 86f44722..5a3270ec 100644
--- a/ut-robomaster/src/robots/standard/standard_constants.hpp
+++ b/ut-robomaster/src/robots/standard/standard_constants.hpp
@@ -2,6 +2,7 @@
 
 #include "tap/algorithms/smooth_pid.hpp"
 #include "tap/communication/can/can_bus.hpp"
+#include "tap/communication/serial/ref_serial.hpp"
 #include "tap/motor/dji_motor.hpp"
 
 #include "modm/container/pair.hpp"
@@ -13,6 +14,7 @@ using motor_controller::PidConstants;
 using tap::can::CanBus;
 using namespace tap::motor;
 using namespace motors;
+using tap::communication::serial::RefSerial;
 
 // General constants ------------------------------------------------
 
@@ -34,7 +36,7 @@ static constexpr float WHEEL_LXY = (WHEEL_DISTANCE_X + WHEEL_DISTANCE_Y) / 2.0f;
 static constexpr int FLYWHEELS = 2;
 
 // turret ------------
-constexpr float PITCH_MIN = 0.10f;            // rad
+constexpr float PITCH_MIN = 0.20f;            // rad
 constexpr float PITCH_MAX = 0.79f;            // rad
 constexpr float CAMERA_TO_PITCH = 0.13555f;   // distance from main camera lens to pitch axis (m)
 constexpr float NOZZLE_TO_PITCH = 0.18151f;   // distance from barrel nozzle to pitch axis (m)
@@ -42,7 +44,8 @@ constexpr float CAMERA_TO_BARRELS = 0.0427f;  // vertical ctc offset from camera
 constexpr float CAMERA_X_OFFSET = -0.0335f;   // horizontal offset of main camera lens (m)
 
 // static constexpr float YAW_OFFSET = 0.0293f;  // external encoder + motor encoder angles
-static constexpr float YAW_OFFSET = 0.0f;
+static constexpr float YAW_OFFSET = 4.30f;
+// static constexpr float YAW_OFFSET = -2.3f;
 static constexpr float PITCH_OFFSET = 2.79f;
 static constexpr float YAW_REDUCTION = 2.0f;
 static constexpr float PITCH_REDUCTION = 1.0f;
@@ -95,8 +98,8 @@ static constexpr float YAW_INPUT_SCALE = 10.0f;
 static constexpr float PITCH_INPUT_SCALE = 5.0f;
 #endif
 
-static constexpr float MOUSE_SENS_YAW = 0.1f;
-static constexpr float MOUSE_SENS_PITCH = 0.1f;
+static constexpr float MOUSE_SENS_YAW = 0.05f;
+static constexpr float MOUSE_SENS_PITCH = 0.05f;
 
 // Motor constants --------------------------------
 
@@ -105,27 +108,52 @@ constexpr CanBus CAN_TURRET = CanBus::CAN_BUS1;
 constexpr CanBus CAN_SHOOTER = CanBus::CAN_BUS2;
 
 // chassis
-const MotorConfig WHEEL_LF{M3508, MOTOR2, CAN_WHEELS, true, "left front wheel", PID_WHEELS, {}};
-const MotorConfig WHEEL_RF{M3508, MOTOR1, CAN_WHEELS, false, "right front wheel", PID_WHEELS, {}};
-const MotorConfig WHEEL_LB{M3508, MOTOR3, CAN_WHEELS, true, "left back wheel", PID_WHEELS, {}};
-const MotorConfig WHEEL_RB{M3508, MOTOR4, CAN_WHEELS, false, "right back wheel", PID_WHEELS, {}};
-
-// flywheels
 const MotorConfig
-    FLYWHEEL_L{M3508_NOGEARBOX, MOTOR3, CAN_SHOOTER, true, "flywheel left", PID_FLYWHEEL, {}};
+    WHEEL_LF{M3508, MOTOR2, CAN_WHEELS, true, "left front wheel", PID_WHEELS, {}, false};
+const MotorConfig
+    WHEEL_RF{M3508, MOTOR1, CAN_WHEELS, false, "right front wheel", PID_WHEELS, {}, false};
 const MotorConfig
-    FLYWHEEL_R{M3508_NOGEARBOX, MOTOR4, CAN_SHOOTER, false, "flywheel right", PID_FLYWHEEL, {}};
+    WHEEL_LB{M3508, MOTOR3, CAN_WHEELS, true, "left back wheel", PID_WHEELS, {}, false};
+const MotorConfig
+    WHEEL_RB{M3508, MOTOR4, CAN_WHEELS, false, "right back wheel", PID_WHEELS, {}, false};
+
+// flywheels
+const MotorConfig FLYWHEEL_L{
+    M3508_NOGEARBOX,
+    MOTOR3,
+    CAN_SHOOTER,
+    true,
+    "flywheel left",
+    PID_FLYWHEEL,
+    {},
+    false};
+const MotorConfig FLYWHEEL_R{
+    M3508_NOGEARBOX,
+    MOTOR4,
+    CAN_SHOOTER,
+    false,
+    "flywheel right",
+    PID_FLYWHEEL,
+    {},
+    false};
 
 // agitator
-const MotorConfig AGITATOR{M3508, MOTOR1, CAN_SHOOTER, false, "agitator", PID_AGITATOR, {}};
+const MotorConfig AGITATOR{M3508, MOTOR1, CAN_SHOOTER, false, "agitator", PID_AGITATOR, {}, false};
 
 // turret
 // {30.0f, 0.0f, 0.0f}
 // {60.0f, 0.0f, 1.0f}
-const MotorConfig
-    YAW_L{M3508, MOTOR5, CAN_TURRET, false, "yaw left", {2.5f, 60.0f, 0.0f}, {45.0f, 0.0f, 0.0f}};
-const MotorConfig YAW_R{M3508, MOTOR6, CAN_TURRET, false, "yaw right", {}, {}};
-const MotorConfig PITCH{GM6020, MOTOR7, CAN_TURRET, false, "pitch", PID_VELOCITY_DEFAULT, {}};
+const MotorConfig YAW_L{
+    M3508,
+    MOTOR5,
+    CAN_TURRET,
+    false,
+    "yaw left",
+    {2.5f, 60.0f, 0.0f},
+    {45.0f, 0.0f, 0.0f},
+    false};
+const MotorConfig YAW_R{M3508, MOTOR6, CAN_TURRET, false, "yaw right", {}, {}, false};
+const MotorConfig PITCH{GM6020, MOTOR7, CAN_TURRET, false, "pitch", PID_VELOCITY_DEFAULT, {}, true};
 
 // Velocities -------------------------------------
 
@@ -144,11 +172,28 @@ const float FLYWHEEL_SPEED = 160.0f;
 const float BALLS_PER_SEC = 17.0f;
 const float BALLS_PER_REV = 18.0f;
 
-const float JAM_TRIGGER_RATIO = 0.2;      // measured speed to driven speed ratio
+const float JAM_TRIGGER_RATIO = 0.2f;     // measured speed to driven speed ratio
 const float JAM_TRIGGER_DURATION = 0.1f;  // s
 const float UNJAM_DURATION = 0.1f;        // s
 const float UNJAM_SPEED = 30.0f;          // rev/s
 
 // Heat Buffers -------------------------------------
 
-const uint16_t BARREL_HEAT_BUFFER = 50.0f;
+const uint16_t BARREL_HEAT_BUFFER_1V1 = 50;
+const uint16_t BARREL_HEAT_BUFFER_3V3[10] = {
+    40,  // Level 1
+    50,  // Level 2
+    50,  // Level 3
+    50,  // Level 4
+    50,  // Level 5
+    50,  // Level 6
+    50,  // Level 7
+    50,  // Level 8
+    50,  // Level 9
+    50   // Level 10
+};
+
+constexpr bool DEBUG_HEAT_BUFFER_ENABLED = true;
+const RefSerial::Rx::GameType DEBUG_HEAT_BUFFER_GAME_TYPE =
+    RefSerial::Rx::GameType::ROBOMASTER_RMUL_3V3;
+const uint8_t DEBUG_HEAT_BUFFER_ROBOT_LEVEL = 1;
\ No newline at end of file
diff --git a/ut-robomaster/src/robots/standard/standard_control.hpp b/ut-robomaster/src/robots/standard/standard_control.hpp
index d9aa5d20..3a6eb91d 100644
--- a/ut-robomaster/src/robots/standard/standard_control.hpp
+++ b/ut-robomaster/src/robots/standard/standard_control.hpp
@@ -1,5 +1,6 @@
 #include "robots/common/common_control_manual.hpp"
 #include "subsystems/agitator/command_agitator_continuous.hpp"
+#include "subsystems/turret/command_recalibrate_turret.hpp"
 
 class StandardControl : CommonControlManual
 {
@@ -29,6 +30,8 @@ class StandardControl : CommonControlManual
         BarrelId::STANDARD1,
         true};
 
+    CommandRecalibrateTurret recalibrateTurret_mmb{drivers, &turret};
+
     // Mappings
     HoldCommandMapping leftMouseDown{
         drivers,
@@ -39,4 +42,9 @@ class StandardControl : CommonControlManual
         drivers,
         {&rotateAgitator_SwitchUp, &rotateFlywheel_SwitchMid},
         RemoteMapState(Remote::Switch::LEFT_SWITCH, Remote::SwitchState::UP)};
+
+    PressCommandMapping rightMousePress{
+        drivers,
+        {&recalibrateTurret_mmb},
+        RemoteMapState(RemoteMapState::MouseButton::RIGHT)};
 };
\ No newline at end of file
diff --git a/ut-robomaster/src/subsystems/agitator/agitator_subsystem.cpp b/ut-robomaster/src/subsystems/agitator/agitator_subsystem.cpp
index 6bad9af2..91250a5c 100644
--- a/ut-robomaster/src/subsystems/agitator/agitator_subsystem.cpp
+++ b/ut-robomaster/src/subsystems/agitator/agitator_subsystem.cpp
@@ -46,11 +46,12 @@ void AgitatorSubsystem::refresh()
     return;
 #endif
 
-    setAmputated(!hardwareOk());
+    // setAmputated(!hardwareOk());
 
     float time = getTimeMilliseconds() / 1000.0f;
     float velocity = getShapedVelocity(time, 1.0f, 0.0f, ballsPerSecond);
-    bool killSwitch = drivers->isKillSwitched() || isAmputated() || !flywheel->isActive();
+    // bool killSwitch = drivers->isKillSwitched() || isAmputated() || !flywheel->isActive();
+    bool killSwitch = drivers->isKillSwitched();
 
     agitator.setActive(!killSwitch);
     agitator.updateVelocity(velocity);
diff --git a/ut-robomaster/src/subsystems/agitator/command_agitator_burst.cpp b/ut-robomaster/src/subsystems/agitator/command_agitator_burst.cpp
index 1a68a147..ef9bf454 100644
--- a/ut-robomaster/src/subsystems/agitator/command_agitator_burst.cpp
+++ b/ut-robomaster/src/subsystems/agitator/command_agitator_burst.cpp
@@ -1,5 +1,6 @@
 #include "command_agitator_burst.hpp"
 
+using tap::communication::serial::RefSerialData;
 namespace commands
 {
 void CommandAgitatorBurst::initialize() { initialPosition = agitator->getPosition(); }
@@ -15,8 +16,50 @@ bool CommandAgitatorBurst::isFinished() const
         return true;
     }
 
+    uint16_t barrel_heat_buffer = 0.0;
+    if (drivers->refSerial.getRefSerialReceivingData())
+    {
+        tap::communication::serial::RefSerialData::Rx::GameType game_type =
+            drivers->refSerial.getGameData().gameType;
+        uint8_t robot_level = drivers->refSerial.getRobotData().robotLevel;
+        if (game_type == RefSerialData::Rx::GameType::UNKNOWN)
+        {
+            drivers->rtt << "game type unknown";
+            if (DEBUG_HEAT_BUFFER_ENABLED)
+            {
+                drivers->rtt << " overwriting game info to game_type: "
+                             << (int)DEBUG_HEAT_BUFFER_GAME_TYPE
+                             << " and robot_level: " << DEBUG_HEAT_BUFFER_ROBOT_LEVEL;
+                game_type = DEBUG_HEAT_BUFFER_GAME_TYPE;
+                robot_level = DEBUG_HEAT_BUFFER_ROBOT_LEVEL;
+            }
+            drivers->rtt.endl();
+        }
+
+        if (game_type ==
+            tap::communication::serial::RefSerialData::Rx::GameType::ROBOMASTER_RMUL_1V1)
+        {
+            barrel_heat_buffer = BARREL_HEAT_BUFFER_1V1;
+        }
+        else if (
+            game_type ==
+            tap::communication::serial::RefSerialData::Rx::GameType::ROBOMASTER_RMUL_3V3)
+        {
+            if (robot_level <= 0 || robot_level > 10)
+            {
+                drivers->rtt << "ERROR UNRECOGNIZED ROBOT LEVEL: " << robot_level
+                             << ". DEFAULTING TO " << barrel_heat_buffer << " HEAT BUFFER";
+                drivers->rtt.endl();
+            }
+            else
+            {
+                barrel_heat_buffer = BARREL_HEAT_BUFFER_3V3[robot_level - 1];
+            }
+        }
+    }
+
     if (drivers->refSerial.getRefSerialReceivingData() &&
-        power_limiter::getRemainingHeat(drivers, barrelId) <= BARREL_HEAT_BUFFER)
+        power_limiter::getRemainingHeat(drivers, barrelId) <= barrel_heat_buffer)
     {
         return true;
     }
diff --git a/ut-robomaster/src/subsystems/agitator/command_agitator_continuous.cpp b/ut-robomaster/src/subsystems/agitator/command_agitator_continuous.cpp
index 8ffb842a..7b6f698b 100644
--- a/ut-robomaster/src/subsystems/agitator/command_agitator_continuous.cpp
+++ b/ut-robomaster/src/subsystems/agitator/command_agitator_continuous.cpp
@@ -1,5 +1,6 @@
 #include "command_agitator_continuous.hpp"
 
+using tap::communication::serial::RefSerialData;
 namespace commands
 {
 using tap::communication::serial::Remote;
@@ -12,6 +13,59 @@ void CommandAgitatorContinuous::initialize()
 
 void CommandAgitatorContinuous::execute()
 {
+    uint16_t barrel_heat_buffer = 0.0;
+    if (drivers->refSerial.getRefSerialReceivingData())
+    {
+        tap::communication::serial::RefSerialData::Rx::GameType game_type =
+            drivers->refSerial.getGameData().gameType;
+        uint8_t robot_level = drivers->refSerial.getRobotData().robotLevel;
+        if (game_type == RefSerialData::Rx::GameType::UNKNOWN)
+        {
+            drivers->rtt << "game type unknown";
+            if (DEBUG_HEAT_BUFFER_ENABLED)
+            {
+                drivers->rtt << " overwriting game info to game_type: "
+                             << (int)DEBUG_HEAT_BUFFER_GAME_TYPE
+                             << " and robot_level: " << DEBUG_HEAT_BUFFER_ROBOT_LEVEL;
+                game_type = DEBUG_HEAT_BUFFER_GAME_TYPE;
+                robot_level = DEBUG_HEAT_BUFFER_ROBOT_LEVEL;
+            }
+            drivers->rtt.endl();
+        }
+
+        if (game_type ==
+            tap::communication::serial::RefSerialData::Rx::GameType::ROBOMASTER_RMUL_1V1)
+        {
+            barrel_heat_buffer = BARREL_HEAT_BUFFER_1V1;
+        }
+        else if (
+            game_type ==
+            tap::communication::serial::RefSerialData::Rx::GameType::ROBOMASTER_RMUL_3V3)
+        {
+            if (robot_level <= 0 || robot_level > 10)
+            {
+                drivers->rtt << "ERROR UNRECOGNIZED ROBOT LEVEL: " << robot_level
+                             << ". DEFAULTING TO " << barrel_heat_buffer << " HEAT BUFFER";
+                drivers->rtt.endl();
+            }
+            else
+            {
+                barrel_heat_buffer = BARREL_HEAT_BUFFER_3V3[robot_level - 1];
+            }
+        }
+    }
+
+    if (drivers->refSerial.getRefSerialReceivingData())
+    {
+        drivers->rtt << "barrel heat: " << power_limiter::getRemainingHeat(drivers, barrelId)
+                     << "; heat buffer: " << barrel_heat_buffer;
+        if (power_limiter::getRemainingHeat(drivers, barrelId) < barrel_heat_buffer)
+        {
+            drivers->rtt << " OVERHEATED";
+        }
+        drivers->rtt.endl();
+    }
+    // UNCOMMENT BARREL OVERHEAT ONCE WE FIX IT LMAO
     if (isJammed)
     {
         if (unjammingTimeout.isExpired())
@@ -24,7 +78,7 @@ void CommandAgitatorContinuous::execute()
     }
     else if (  // barrel overheat
         drivers->refSerial.getRefSerialReceivingData() &&
-        power_limiter::getRemainingHeat(drivers, barrelId) <= BARREL_HEAT_BUFFER)
+        power_limiter::getRemainingHeat(drivers, barrelId) < barrel_heat_buffer)
     {
         jamTriggerTimeout.restart(JAM_TRIGGER_DURATION * 1000);
         agitator->setBallsPerSecond(0.0f);
@@ -45,6 +99,7 @@ void CommandAgitatorContinuous::execute()
 
         agitator->setBallsPerSecond(BALLS_PER_SEC);
     }
+    // agitator->setBallsPerSecond(BALLS_PER_SEC);
 }
 
 void CommandAgitatorContinuous::end(bool) { agitator->setBallsPerSecond(0.0f); }
diff --git a/ut-robomaster/src/subsystems/chassis/chassis_subsystem.cpp b/ut-robomaster/src/subsystems/chassis/chassis_subsystem.cpp
index e4bfa65e..0d71b82d 100644
--- a/ut-robomaster/src/subsystems/chassis/chassis_subsystem.cpp
+++ b/ut-robomaster/src/subsystems/chassis/chassis_subsystem.cpp
@@ -26,6 +26,7 @@ void ChassisSubsystem::initialize()
     {
         wheels[i].initialize();
     }
+    logTimer.stop();
 }
 
 void ChassisSubsystem::refresh()
@@ -106,7 +107,11 @@ void ChassisSubsystem::input(Vector2f move, float spin)
         overdrive -= correction;
     }
 
+#if defined(TARGET_SENTRY)
+    setOmniVelocities(v, wZ);
+#else
     setMecanumWheelVelocities(v, wZ);
+#endif
 }
 
 void ChassisSubsystem::setMecanumWheelVelocities(Vector2f v, float wZ)
@@ -118,6 +123,14 @@ void ChassisSubsystem::setMecanumWheelVelocities(Vector2f v, float wZ)
     targetWheelVels[3] = (-v.y - v.x - wZ * WHEEL_LXY) / WHEEL_RADIUS;  // rad/s
 }
 
+void ChassisSubsystem::setOmniVelocities(Vector2f v, float wZ)
+{
+    targetWheelVels[0] = (v.y + v.x + wZ) / WHEEL_RADIUS;  // rad/s
+    targetWheelVels[1] = (v.y - v.x - wZ) / WHEEL_RADIUS;  // rad/s
+    targetWheelVels[2] = (v.y - v.x + wZ) / WHEEL_RADIUS;  // rad/s
+    targetWheelVels[3] = (v.y + v.x - wZ) / WHEEL_RADIUS;  // rad/s
+}
+
 Vector3f ChassisSubsystem::measureVelocity()
 {
     float w1 = wheels[0].measureVelocity();  // rev/s
diff --git a/ut-robomaster/src/subsystems/chassis/chassis_subsystem.hpp b/ut-robomaster/src/subsystems/chassis/chassis_subsystem.hpp
index dcc70d18..f45c5be8 100644
--- a/ut-robomaster/src/subsystems/chassis/chassis_subsystem.hpp
+++ b/ut-robomaster/src/subsystems/chassis/chassis_subsystem.hpp
@@ -9,6 +9,7 @@
 #include "utils/power_limiter/power_limiter.hpp"
 
 #include "drivers.hpp"
+using tap::arch::MilliTimeout;
 
 namespace subsystems::chassis
 {
@@ -36,11 +37,14 @@ class ChassisSubsystem : public Subsystem
     /// @return x,y is linear velocity (m/s) and z is angular velocity (rad/s)
     Vector3f measureVelocity();
 
+    void setOmniVelocities(Vector2f v, float wZ);
+
 private:
     src::Drivers* drivers;
     power_limiter::PowerLimiter powerLimiter;
     MotorController wheels[WHEELS];
     float targetWheelVels[WHEELS] = {0.0f, 0.0f, 0.0f, 0.0f};
+    MilliTimeout logTimer;
 
     /// @brief Calculate and set wheel velocities for desired robot motion (based on
     /// https://research.ijcaonline.org/volume113/number3/pxc3901586.pdf).
diff --git a/ut-robomaster/src/subsystems/chassis/command_move_chassis.cpp b/ut-robomaster/src/subsystems/chassis/command_move_chassis.cpp
index f3945d05..048cca0d 100644
--- a/ut-robomaster/src/subsystems/chassis/command_move_chassis.cpp
+++ b/ut-robomaster/src/subsystems/chassis/command_move_chassis.cpp
@@ -41,7 +41,7 @@ void CommandMoveChassis::execute()
     // override spin input while beyblading
     if (beyblade)
     {
-        inputSpin = 1.0f;
+        inputSpin = 0.5f;
     }
 
     // rotate movement vector relative to turret
@@ -61,7 +61,14 @@ bool CommandMoveChassis::applyKeyboardInput(Vector2f &inputMove, float &inputSpi
 {
     Remote *remote = &drivers->remote;
 
-    inputSpin = 0.0f;  // no keyboard spin controls
+    if (remote->keyPressed(Remote::Key::E)) {
+        inputSpin = 0.5f;
+    } else if (remote->keyPressed(Remote::Key::Q)) {
+        inputSpin = -0.5f;
+    } else {
+        inputSpin = 0.0f;
+    }
+    // inputSpin = 0.0f;  // no keyboard spin controls
 
     Vector2f rawMoveInput = Vector2f(
         remote->keyPressed(Remote::Key::D) - remote->keyPressed(Remote::Key::A),
diff --git a/ut-robomaster/src/subsystems/chassis/command_sentry_position.cpp b/ut-robomaster/src/subsystems/chassis/command_sentry_position.cpp
index 7feef19e..8605da46 100644
--- a/ut-robomaster/src/subsystems/chassis/command_sentry_position.cpp
+++ b/ut-robomaster/src/subsystems/chassis/command_sentry_position.cpp
@@ -3,27 +3,150 @@
 namespace commands
 {
 
+using namespace communication;
+// old ard code
 void CommandSentryPosition::initialize() { moveTimer.stop(); }
 
+// void CommandSentryPosition::initialize() { keyboardInputMove = Vector2f(0.0f); }
+
 void CommandSentryPosition::execute()
 {
-    // wait until game starts then wait some more to avoid excess drifting
-    if (drivers->isGameActive() && moveTimer.isStopped())
+    // float yawAngle = turret->getTargetLocalYaw();
+    // Vector2f inputMove = Vector2f(0.0f);
+    // float inputSpin = 0.0f;
+
+    // // get keyboard input
+    // float keyboardInputSpin;
+    // bool hasKeyboardInput = applyKeyboardInput(keyboardInputMove, keyboardInputSpin);
+
+    // // get joystick input
+    // Vector2f joystickInputMove;
+    // float joystickInputSpin;
+    // bool hasJoystickInput = applyJoystickInput(joystickInputMove, joystickInputSpin);
+
+    // // decide which input source to use (keyboard has inertia)
+    // if (!hasKeyboardInput && hasJoystickInput)
+    // {
+    //     inputMove = joystickInputMove;
+    //     inputSpin = joystickInputSpin;
+    // }
+    // else
+    // {
+    //     inputMove = keyboardInputMove;
+    //     inputSpin = keyboardInputSpin;
+    // }
+
+    // // auto-align chassis to turret when moving
+    // if (inputMove.getLengthSquared() > 0.0f && inputSpin == 0.0f)
+    // {
+    //     inputSpin = calculateAutoAlignCorrection(yawAngle, CHASSIS_AUTOALIGN_ANGLE) *
+    //                 CHASSIS_AUTOALIGN_FACTOR;
+    // }
+
+    // // override spin input while beyblading
+    // if (beyblade)
+    // {
+    //     inputSpin = 1.0f;
+    // }
+
+    // // rotate movement vector relative to turret
+    // if (turretRelative)
+    // {
+    //     inputMove = inputMove.rotate(yawAngle);
+    // }
+
+    // chassis->input(inputMove, inputSpin);
+
+    //     if (drivers->isGameActive() && moveTimer.isStopped())
+    //     {
+    //         moveTimer.restart(5'000);  // 5 secs
+    //     }
+
+    //     if (!moveTimer.isExpired())
+    //     {
+    //         chassis->input(Vector2f(0.0f), 1.0f);  // spin!
+    //         return;
+    //     }
+
+    //     PositionRequest pos = drivers->cvBoard.getPositionRequest();
+    //     drivers->rtt << "x_pos = " << pos.x_requested << ", y_pos = " << pos.y_requested
+    //                  << ", time_sent = " << (float)pos.time_sent;
+
+    //     // speen
+
+    // Remote *remote = &drivers->remote;
+    // float pwr = remote->getChannel(Remote::Channel::RIGHT_VERTICAL);
+    // float truePwr = pwr < ANALOG_DEAD_ZONE && !drivers->isKillSwitched() ? 0 : pwr * 0.5;
+    // chassis->input(Vector2f(0.0f), truePwr);
+
+    chassis->input(Vector2f(0.0f), 0.0f);
+}
+
+void CommandSentryPosition::end(bool) { chassis->input(Vector2f(0.0f), 0.0f); }
+
+bool CommandSentryPosition::isFinished() const { return false; }
+
+bool CommandSentryPosition::applyKeyboardInput(Vector2f &inputMove, float &inputSpin)
+{
+    Remote *remote = &drivers->remote;
+
+    inputSpin = 0.0f;  // no keyboard spin controls
+
+    Vector2f rawMoveInput = Vector2f(
+        remote->keyPressed(Remote::Key::D) - remote->keyPressed(Remote::Key::A),
+        remote->keyPressed(Remote::Key::W) - remote->keyPressed(Remote::Key::S));
+
+    float rawInputLen = rawMoveInput.getLength();
+
+    if (rawInputLen > 0.0f)
     {
-        moveTimer.restart(10'000);  // 10s
+        Vector2f moveDir = rawMoveInput / rawInputLen;  // normalize input
+        inputMove += moveDir * KEYBOARD_ACCEL * DT;     // incorporate input
+        inputMove /= max(1.0f, inputMove.getLength());  // clamp length
     }
-
-    if (!moveTimer.isExpired())
+    else
     {
-        chassis->input(Vector2f(0.0f), 0.0f);
-        return;
+        // decelerate when input stops
+        float len = inputMove.getLength();
+        if (len > 0.0f)
+        {
+            inputMove *= max(1.0f - KEYBOARD_DECEL * DT / len, 0.0f);
+        }
     }
 
-    // speen
-    chassis->input(Vector2f(0.0f), 1.0f);
+    return rawMoveInput != Vector2f(0.0f);
 }
 
-void CommandSentryPosition::end(bool) { chassis->input(Vector2f(0.0f), 0.0f); }
-bool CommandSentryPosition::isFinished() const { return false; }
+bool CommandSentryPosition::applyJoystickInput(Vector2f &inputMove, float &inputSpin)
+{
+    Remote *remote = &drivers->remote;
+
+    inputMove = Vector2f(
+        remote->getChannel(Remote::Channel::RIGHT_HORIZONTAL),
+        remote->getChannel(Remote::Channel::RIGHT_VERTICAL));
+
+    inputSpin = remote->getChannel(Remote::Channel::WHEEL);
+
+    float inputMoveLen = inputMove.getLength();
+    if (inputMoveLen < ANALOG_DEAD_ZONE)
+    {
+        inputMove = Vector2f(0.0f);
+    }
+    else
+    {
+        inputMove /= max(1.0f, inputMoveLen);  // clamp length
+    }
+
+    if (abs(inputSpin) < ANALOG_DEAD_ZONE)
+    {
+        inputSpin = 0.0f;
+    }
+
+    // apply quadratic input ramping
+    inputMove *= inputMove.getLength();
+    inputSpin *= abs(inputSpin);
+
+    return inputMove != Vector2f(0.0f) || inputSpin != 0.0f;
+}
 
 }  // namespace commands
diff --git a/ut-robomaster/src/subsystems/chassis/command_sentry_position.hpp b/ut-robomaster/src/subsystems/chassis/command_sentry_position.hpp
index 69aa2653..546fc89c 100644
--- a/ut-robomaster/src/subsystems/chassis/command_sentry_position.hpp
+++ b/ut-robomaster/src/subsystems/chassis/command_sentry_position.hpp
@@ -1,9 +1,15 @@
 #pragma once
 
-#include "tap/architecture/timeout.hpp"
+// #include "tap/architecture/timeout.hpp"
 #include "tap/control/command.hpp"
 
-#include "chassis_subsystem.hpp"
+#include "robots/robot_constants.hpp"
+// this is older sentry code dont know why it had chopped path
+// #include "chassis_subsystem.hpp"
+#include "subsystems/chassis/chassis_subsystem.hpp"
+#include "subsystems/turret/turret_subsystem.hpp"
+#include "utils/chassis_auto_align.hpp"
+
 #include "drivers.hpp"
 
 namespace commands
@@ -11,14 +17,23 @@ namespace commands
 using namespace tap::communication::serial;
 using namespace modm;
 using subsystems::chassis::ChassisSubsystem;
+using subsystems::turret::TurretSubsystem;
 using tap::arch::MilliTimeout;
 
 class CommandSentryPosition : public tap::control::Command
 {
 public:
-    CommandSentryPosition(src::Drivers *drivers, ChassisSubsystem *chassis)
+    CommandSentryPosition(
+        src::Drivers *drivers,
+        ChassisSubsystem *chassis,
+        TurretSubsystem *turret,
+        bool turretRelative = false,
+        bool beyblade = false)
         : drivers(drivers),
-          chassis(chassis)
+          chassis(chassis),
+          turret(turret),
+          turretRelative(turretRelative),
+          beyblade(beyblade)
     {
         addSubsystemRequirement(chassis);
     }
@@ -37,5 +52,13 @@ class CommandSentryPosition : public tap::control::Command
     src::Drivers *drivers;
     ChassisSubsystem *chassis;
     MilliTimeout moveTimer;
+    TurretSubsystem *turret;
+
+    Vector2f keyboardInputMove = Vector2f(0.0f);
+    const bool turretRelative = false;
+    const bool beyblade = false;
+
+    bool applyKeyboardInput(Vector2f &moveOut, float &spinOut);
+    bool applyJoystickInput(Vector2f &moveOut, float &spinOut);
 };
 }  // namespace commands
\ No newline at end of file
diff --git a/ut-robomaster/src/subsystems/flywheel/flywheel_subsystem.cpp b/ut-robomaster/src/subsystems/flywheel/flywheel_subsystem.cpp
index b0aa4cc9..5a60d31f 100644
--- a/ut-robomaster/src/subsystems/flywheel/flywheel_subsystem.cpp
+++ b/ut-robomaster/src/subsystems/flywheel/flywheel_subsystem.cpp
@@ -41,7 +41,7 @@ void FlywheelSubsystem::refresh()
 
     for (int i = 0; i < FLYWHEELS; i++)
     {
-        motors[i].setActive(!killSwitch && !isAmputated());
+        motors[i].setActive(!killSwitch);
         motors[i].updateVelocity(velocity);
     }
 }
diff --git a/ut-robomaster/src/subsystems/odometry/observer_displacement.cpp b/ut-robomaster/src/subsystems/odometry/observer_displacement.cpp
index c08528fd..d49ad64a 100644
--- a/ut-robomaster/src/subsystems/odometry/observer_displacement.cpp
+++ b/ut-robomaster/src/subsystems/odometry/observer_displacement.cpp
@@ -29,8 +29,10 @@ bool ChassisDisplacementObserver::getVelocityChassisDisplacement(
 
     // TODO: Depending on when this subsystem gets initialized,
     //   the first time this function runs, deltaT might be large
-    auto nowTime = imu->getPrevIMUDataReceivedTime();  // Units of us
-    auto dt = (nowTime - lastTime) / 1e6f;             // Want units of s
+    // auto nowTime = tap::arch::clock::getTimeMicroseconds();  // units of us
+    auto nowTime = tap::arch::clock::getTimeMilliseconds();  // ms
+    auto dt = (nowTime - lastTime) / 1000.0f;                // sec
+    // auto dt = (nowTime - lastTime) / 1e6f;  // Want units of s
 
     // z is 0 since we're moving on the x-y plane and gravity affects z
     Vector3f nowAcc{imu->getAx(), imu->getAy(), 0.0f};
diff --git a/ut-robomaster/src/subsystems/odometry/observer_displacement.hpp b/ut-robomaster/src/subsystems/odometry/observer_displacement.hpp
index 42626172..379801fb 100644
--- a/ut-robomaster/src/subsystems/odometry/observer_displacement.hpp
+++ b/ut-robomaster/src/subsystems/odometry/observer_displacement.hpp
@@ -2,6 +2,7 @@
 #define SUBSYSTEMS_ODOMETRY_OBSERVER_DISPLACEMENT_HPP_
 
 #include "tap/algorithms/odometry/chassis_displacement_observer_interface.hpp"
+#include "tap/architecture/clock.hpp"
 
 #include "subsystems/chassis/chassis_subsystem.hpp"
 
diff --git a/ut-robomaster/src/subsystems/turret/command_recalibrate_turret.cpp b/ut-robomaster/src/subsystems/turret/command_recalibrate_turret.cpp
new file mode 100644
index 00000000..544c651c
--- /dev/null
+++ b/ut-robomaster/src/subsystems/turret/command_recalibrate_turret.cpp
@@ -0,0 +1,19 @@
+#include "command_recalibrate_turret.hpp"
+
+namespace commands
+{
+
+void CommandRecalibrateTurret::initialize() {}
+
+void CommandRecalibrateTurret::execute()
+{
+    turret->setBaseYaw(turret->getMotorAngle());
+    turret->setTargetWorldAngles(
+        turret->getCurrentLocalYaw() + turret->getChassisYaw(),
+        turret->getCurrentLocalPitch());
+}
+
+void CommandRecalibrateTurret::end(bool) {}
+
+bool CommandRecalibrateTurret::isFinished() const { return false; }
+}  // namespace commands
\ No newline at end of file
diff --git a/ut-robomaster/src/subsystems/turret/command_recalibrate_turret.hpp b/ut-robomaster/src/subsystems/turret/command_recalibrate_turret.hpp
new file mode 100644
index 00000000..1e10ca7f
--- /dev/null
+++ b/ut-robomaster/src/subsystems/turret/command_recalibrate_turret.hpp
@@ -0,0 +1,39 @@
+#pragma once
+
+#include "tap/communication/serial/remote.hpp"
+#include "tap/control/command.hpp"
+
+#include "robots/robot_constants.hpp"
+#include "subsystems/turret/turret_subsystem.hpp"
+
+#include "drivers.hpp"
+
+namespace commands
+{
+using subsystems::turret::TurretSubsystem;
+
+class CommandRecalibrateTurret : public tap::control::Command
+{
+public:
+    CommandRecalibrateTurret(src::Drivers* drivers, TurretSubsystem* turret)
+        : drivers(drivers),
+          turret(turret)
+    {
+        addSubsystemRequirement(turret);
+    }
+
+    void initialize() override;
+
+    void execute() override;
+
+    void end(bool interrupted) override;
+
+    bool isFinished() const override;
+
+    const char* getName() const override { return "recalibrate turret command"; }
+
+private:
+    src::Drivers* drivers;
+    TurretSubsystem* turret;
+};
+}  // namespace commands
\ No newline at end of file
diff --git a/ut-robomaster/src/subsystems/turret/command_sentry_aim.cpp b/ut-robomaster/src/subsystems/turret/command_sentry_aim.cpp
index 895c9b91..3e754f75 100644
--- a/ut-robomaster/src/subsystems/turret/command_sentry_aim.cpp
+++ b/ut-robomaster/src/subsystems/turret/command_sentry_aim.cpp
@@ -7,6 +7,32 @@ void CommandSentryAim::initialize() {}
 void CommandSentryAim::execute()
 {
     // turret->setTargetWorldAngles(turret->getChassisYaw(), 0.0f);
+    // if (turret->getTargetLocalYaw() <= -55.0f)
+    // {
+    //     turret->setTargetWorldAngles(50.0f, 10.0f);
+    // }
+    // else
+    // {
+    // turret->setTargetWorldAngles(-50.0f, -10.0f);
+    // }
+
+    if (!drivers->isKillSwitched())
+    {
+        turret->setTargetWorldAngles(50.0f, 10.0f);
+    }
+
+    /*
+        PLAN (for suhas):
+        - see if setTargetWorldAngles() works as intended
+        - create targetWorldYaw/pitch variables
+        - twYaw should switch between -200 and 200 depending on what its set to
+            - if the angle is -200 set it to 200
+            - if the angle is 200 set it to -200
+            - else, keep current targetWorldAngle
+        - time how long it takes for pitch motor to oscillate
+        - use a timer to oscillate pitch to move ~10 deg to scan, or if that doesn't work, use the
+        - same logic to scan
+    */
 }
 
 void CommandSentryAim::end(bool) {}
diff --git a/ut-robomaster/src/subsystems/turret/double_yaw_motor.cpp b/ut-robomaster/src/subsystems/turret/double_yaw_motor.cpp
index 9b06d677..11f3609d 100644
--- a/ut-robomaster/src/subsystems/turret/double_yaw_motor.cpp
+++ b/ut-robomaster/src/subsystems/turret/double_yaw_motor.cpp
@@ -34,27 +34,28 @@ void DoubleYawMotor::reset()
 
 void DoubleYawMotor::updateMotorAngle()
 {
-    float encoderAngle = static_cast<float>(motor1.getEncoderUnwrapped()) /
-                         DjiMotor::ENC_RESOLUTION / M3508.gearRatio / YAW_REDUCTION;
-    currentAngle.setValue(encoderAngle);
+    float encoderAngle =
+        static_cast<float>(motor1.getInternalEncoder().getEncoder().getUnwrappedValue()) /
+        DjiMotorEncoder::ENC_RESOLUTION / M3508.gearRatio / YAW_REDUCTION;
+    currentAngle.setWrappedValue(encoderAngle);
 }
 
 void DoubleYawMotor::setAngle(float desiredAngle, float dt)
 {
-    setpoint.setValue(desiredAngle / M_TWOPI);
+    setpoint.setWrappedValue(desiredAngle / M_TWOPI);
 
-    float positionError =
-        ContiguousFloat(currentAngle.getValue(), 0, 1.0f).difference(setpoint.getValue());
+    float positionError = WrappedFloat(currentAngle.getWrappedValue(), 0, 1.0f)
+                              .minDifference(setpoint.getWrappedValue());
 
     // account for chassis rotation
-    float disturbance = drivers->bmi088.getGz() / 360.0f;  // rev / s
-    disturbance *= 2.0f;                                   // seems to help?
+    float disturbance = drivers->bmi088.getGz() / 2.0f / PI;  // rev / s
+    disturbance *= 2.0f;                                      // seems to help?
 
     float targetVelocity = positionPid.update(positionError, dt, false) - disturbance;
     setVelocity(targetVelocity, dt);
 }
 
-float DoubleYawMotor::getAngle() { return currentAngle.getValue() * M_TWOPI; }
+float DoubleYawMotor::getAngle() { return currentAngle.getWrappedValue() * M_TWOPI; }
 
 void DoubleYawMotor::setVelocity(float velocity, float dt)
 {
@@ -64,8 +65,8 @@ void DoubleYawMotor::setVelocity(float velocity, float dt)
 
 float DoubleYawMotor::getCurrentVelocity()
 {
-    float rpm1 = motor1.getShaftRPM();                                       // rev / m
-    float rpm2 = motor2.getShaftRPM();                                       // rev / m
+    float rpm1 = motor1.getInternalEncoder().getShaftRPM();                  // rev / m
+    float rpm2 = motor2.getInternalEncoder().getShaftRPM();                  // rev / m
     float currentVelocity = (rpm1 + rpm2) / 2.0f / 60.0f / M3508.gearRatio;  // rev / s
     return currentVelocity;
 }
@@ -78,6 +79,6 @@ void DoubleYawMotor::setOutput(float output)
 
 float DoubleYawMotor::getOutput() { return motor1.getOutputDesired() / M3508.maxOutput; }
 
-float DoubleYawMotor::getSetpoint() { return setpoint.getValue(); }
+float DoubleYawMotor::getSetpoint() { return setpoint.getWrappedValue(); }
 bool DoubleYawMotor::isOnline() { return motor1.isMotorOnline() && motor2.isMotorOnline(); }
 }  // namespace subsystems::turret
\ No newline at end of file
diff --git a/ut-robomaster/src/subsystems/turret/double_yaw_motor.hpp b/ut-robomaster/src/subsystems/turret/double_yaw_motor.hpp
index 4e270ecf..96655bec 100644
--- a/ut-robomaster/src/subsystems/turret/double_yaw_motor.hpp
+++ b/ut-robomaster/src/subsystems/turret/double_yaw_motor.hpp
@@ -1,7 +1,7 @@
 #pragma once
 
-#include "tap/algorithms/contiguous_float.hpp"
 #include "tap/algorithms/smooth_pid.hpp"
+#include "tap/algorithms/wrapped_float.hpp"
 #include "tap/motor/motor_interface.hpp"
 
 #include "utils/motors/motor_constants.hpp"
@@ -38,7 +38,7 @@ class DoubleYawMotor
     DjiMotor motor2;
     motor_controller::Pid velocityPid;
     motor_controller::Pid positionPid;
-    ContiguousFloat setpoint;
-    ContiguousFloat currentAngle;
+    WrappedFloat setpoint;
+    WrappedFloat currentAngle;
 };
 }  // namespace subsystems::turret
\ No newline at end of file
diff --git a/ut-robomaster/src/subsystems/turret/turret_motor.cpp b/ut-robomaster/src/subsystems/turret/turret_motor.cpp
index 8273a68a..e6c075e1 100644
--- a/ut-robomaster/src/subsystems/turret/turret_motor.cpp
+++ b/ut-robomaster/src/subsystems/turret/turret_motor.cpp
@@ -9,8 +9,14 @@ TurretMotor::TurretMotor(
     src::Drivers *drivers,
     MotorConfig motorConfig,
     const SmoothPidConfig &pidConfig)
-    : drivers(drivers),
-      motor(drivers, motorConfig.id, motorConfig.canBus, motorConfig.inverted, motorConfig.name),
+    : motor(
+          drivers,
+          motorConfig.id,
+          motorConfig.canBus,
+          motorConfig.inverted,
+          motorConfig.name,
+          motorConfig.currentControl),
+      drivers(drivers),
       pid(pidConfig),
       setpoint(0.0f, 0.0f, M_TWOPI),
       currentAngle(0.0f, 0.0f, M_TWOPI)
@@ -27,32 +33,35 @@ void TurretMotor::reset()
 
 void TurretMotor::updateMotorAngle()
 {
-    uint16_t encoderValue = motor.getEncoderWrapped();
+    // uint16_t encoderValue = motor.getEncoderWrapped();
+    uint16_t encoderValue = motor.getInternalEncoder().getEncoder().getUnwrappedValue();
     if (lastUpdatedEncoderValue != encoderValue)
     {
         lastUpdatedEncoderValue = encoderValue;
 
         unwrappedAngle = static_cast<float>(encoderValue) * M_TWOPI /
-                         static_cast<float>(DjiMotor::ENC_RESOLUTION);
-        currentAngle.setValue(unwrappedAngle);
+                         static_cast<float>(DjiMotorEncoder::ENC_RESOLUTION);
+        currentAngle.setWrappedValue(unwrappedAngle);
     }
 }
 
 void TurretMotor::setAngle(float desiredAngle, float dt)
 {
-    setpoint.setValue(desiredAngle);
+    setpoint.setWrappedValue(desiredAngle);
 
-    float positionControllerError =
-        ContiguousFloat(currentAngle.getValue(), 0, M_TWOPI).difference(setpoint.getValue());
-    float output =
-        pid.runController(positionControllerError, (M_TWOPI / 60.0f) * motor.getShaftRPM(), dt);
+    float positionControllerError = WrappedFloat(currentAngle.getWrappedValue(), 0, M_TWOPI)
+                                        .minDifference(setpoint.getWrappedValue());
+    float output = pid.runController(
+        positionControllerError,
+        (M_TWOPI / 60.0f) * motor.getInternalEncoder().getShaftRPM(),
+        dt);
 
     motor.setDesiredOutput(output);
 }
 
-float TurretMotor::getAngle() { return currentAngle.getValue(); }
+float TurretMotor::getAngle() { return currentAngle.getWrappedValue(); }
 
-float TurretMotor::getSetpoint() { return setpoint.getValue(); }
+float TurretMotor::getSetpoint() { return setpoint.getWrappedValue(); }
 
 bool TurretMotor::isOnline() { return motor.isMotorOnline(); }
 }  // namespace subsystems::turret
\ No newline at end of file
diff --git a/ut-robomaster/src/subsystems/turret/turret_motor.hpp b/ut-robomaster/src/subsystems/turret/turret_motor.hpp
index 12fcc8d2..7cbf65a0 100644
--- a/ut-robomaster/src/subsystems/turret/turret_motor.hpp
+++ b/ut-robomaster/src/subsystems/turret/turret_motor.hpp
@@ -1,7 +1,7 @@
 #pragma once
 
-#include "tap/algorithms/contiguous_float.hpp"
 #include "tap/algorithms/smooth_pid.hpp"
+#include "tap/algorithms/wrapped_float.hpp"
 #include "tap/motor/motor_interface.hpp"
 
 #include "utils/motors/motor_constants.hpp"
@@ -27,13 +27,15 @@ class TurretMotor
     float getSetpoint();
     bool isOnline();
 
+    DjiMotor motor;
+
 private:
     src::Drivers *drivers;
-    DjiMotor motor;
+    // DjiMotor motor;
     SmoothPid pid;
-    ContiguousFloat setpoint;
+    WrappedFloat setpoint;
     float unwrappedAngle = 0;
-    ContiguousFloat currentAngle;
+    WrappedFloat currentAngle;
     float lastUpdatedEncoderValue = 0;
 };
 }  // namespace subsystems::turret
\ No newline at end of file
diff --git a/ut-robomaster/src/subsystems/turret/turret_subsystem.cpp b/ut-robomaster/src/subsystems/turret/turret_subsystem.cpp
index 37f22cf3..232090f1 100644
--- a/ut-robomaster/src/subsystems/turret/turret_subsystem.cpp
+++ b/ut-robomaster/src/subsystems/turret/turret_subsystem.cpp
@@ -1,11 +1,14 @@
 #include "turret_subsystem.hpp"
 
+#include "tap/communication/serial/remote.hpp"
+
 #include "modm/math.hpp"
 #include "robots/robot_constants.hpp"
 #include "subsystems/subsystem.hpp"
 
 namespace subsystems::turret
 {
+using tap::communication::serial::Remote;
 TurretSubsystem::TurretSubsystem(src::Drivers* drivers)
     : Subsystem(drivers),
       drivers(drivers),
@@ -29,23 +32,29 @@ void TurretSubsystem::initialize()
 
 void TurretSubsystem::refresh()
 {
-    setAmputated(!hardwareOk());
-
+    // drivers->rtt << "absoluate yaw enc: " << yawEncoder.getAngle() << "\n";
     yaw.updateMotorAngle();
     pitch.updateMotorAngle();
 
+    drivers->rtt << "pitch: " << pitch.getAngle();
+    drivers->rtt.endl();
+
 #if defined(TARGET_STANDARD) || defined(TARGET_HERO)
     yawEncoder.update();
 
-    if (!isCalibrated && !isAmputated() && yawEncoder.isOnline())
+    if (!isCalibrated && yawEncoder.isOnline())
     {
+#if defined(TARGET_HERO)
+        baseYaw = -yaw.getAngle();
+#else
         baseYaw = yawEncoder.getAngle() - YAW_OFFSET - yaw.getAngle();
+#endif
         isCalibrated = true;
 
         setTargetWorldAngles(getCurrentLocalYaw() + getChassisYaw(), getCurrentLocalPitch());
     }
 #else
-    if (!isCalibrated && !isAmputated())
+    if (!isCalibrated)
     {
         baseYaw = -YAW_OFFSET;
         isCalibrated = true;
@@ -54,10 +63,14 @@ void TurretSubsystem::refresh()
     }
 #endif
 
-    if (isCalibrated && !drivers->isKillSwitched() && !isAmputated())
+    if (isCalibrated && !drivers->isKillSwitched())
     {
         yaw.setAngle(-baseYaw + getTargetLocalYaw(), DT);
+        // pitch.motor.setDesiredOutput(GM6020.maxOutput - 1);
         pitch.setAngle((PITCH_OFFSET + getTargetLocalPitch()) * PITCH_REDUCTION, DT);
+        drivers->rtt << "target pitch angle "
+                     << (PITCH_OFFSET + getTargetLocalPitch()) * PITCH_REDUCTION;
+        drivers->rtt.endl();
     }
     else
     {
@@ -66,6 +79,10 @@ void TurretSubsystem::refresh()
     }
 }
 
+void TurretSubsystem::setBaseYaw(float by) { baseYaw = by; }
+
+float TurretSubsystem::getMotorAngle() { return -yaw.getAngle(); }
+
 void TurretSubsystem::inputTargetData(Vector3f position, Vector3f velocity, Vector3f acceleration)
 {
     targetPosition = position;
@@ -79,7 +96,7 @@ void TurretSubsystem::setTargetWorldAngles(float yaw, float pitch)
     targetWorldPitch = modm::min(modm::max(pitch, PITCH_MIN), PITCH_MAX);
 }
 
-float TurretSubsystem::getChassisYaw() { return modm::toRadian(drivers->bmi088.getYaw()); }
+float TurretSubsystem::getChassisYaw() { return drivers->bmi088.getYaw(); }
 
 float TurretSubsystem::getTargetLocalYaw() { return targetWorldYaw - getChassisYaw(); }
 
diff --git a/ut-robomaster/src/subsystems/turret/turret_subsystem.hpp b/ut-robomaster/src/subsystems/turret/turret_subsystem.hpp
index 630e02eb..65846b9e 100644
--- a/ut-robomaster/src/subsystems/turret/turret_subsystem.hpp
+++ b/ut-robomaster/src/subsystems/turret/turret_subsystem.hpp
@@ -1,6 +1,6 @@
 #pragma once
 
-#include "tap/algorithms/contiguous_float.hpp"
+#include "tap/algorithms/wrapped_float.hpp"
 #include "tap/control/subsystem.hpp"
 
 #include "drivers/as5600.hpp"
@@ -16,7 +16,7 @@ namespace subsystems::turret
 {
 using driver::As5600;
 using modm::Vector3f;
-using tap::algorithms::ContiguousFloat;
+using tap::algorithms::WrappedFloat;
 
 class TurretSubsystem : public Subsystem
 {
@@ -37,6 +37,9 @@ class TurretSubsystem : public Subsystem
     float getCurrentLocalYaw();
     float getCurrentLocalPitch();
     bool getIsCalibrated();
+    float getMotorAngle();
+
+    void setBaseYaw(float by);
 
 private:
     src::Drivers* drivers;
diff --git a/ut-robomaster/src/utils/motors/motor_constants.hpp b/ut-robomaster/src/utils/motors/motor_constants.hpp
index d99c6eef..0313c84f 100644
--- a/ut-robomaster/src/utils/motors/motor_constants.hpp
+++ b/ut-robomaster/src/utils/motors/motor_constants.hpp
@@ -24,6 +24,7 @@ struct MotorConfig
     const char* const name;
     PidConstants velocityPidConstants;
     PidConstants positionPidConstants;
+    bool currentControl;
 };
 
 // C620 controller (16384 = 20A)
diff --git a/ut-robomaster/src/utils/motors/motor_controller.cpp b/ut-robomaster/src/utils/motors/motor_controller.cpp
index f0cc69ba..74723ebd 100644
--- a/ut-robomaster/src/utils/motors/motor_controller.cpp
+++ b/ut-robomaster/src/utils/motors/motor_controller.cpp
@@ -50,15 +50,15 @@ bool MotorController::isOnline() { return motor.isMotorOnline(); }
 
 float MotorController::measurePosition()
 {
-    int64_t encoderVal = motor.getEncoderUnwrapped();
-    float units = static_cast<float>(encoderVal) / DjiMotor::ENC_RESOLUTION;
+    int64_t encoderVal = motor.getInternalEncoder().getEncoder().getUnwrappedValue();
+    float units = static_cast<float>(encoderVal) / DjiMotorEncoder::ENC_RESOLUTION;
     float turns = units / constants.gearRatio;  // revs
     return turns;
 }
 
 float MotorController::measureVelocity()
 {
-    int16_t rpm = motor.getShaftRPM() / constants.gearRatio;
+    int16_t rpm = motor.getInternalEncoder().getShaftRPM() / constants.gearRatio;
     float rps = rpm / 60.0f;  // revs / sec
     return rps;
 }
diff --git a/ut-robomaster/src/utils/motors/motor_controller.hpp b/ut-robomaster/src/utils/motors/motor_controller.hpp
index 79603dd5..5303c2fb 100644
--- a/ut-robomaster/src/utils/motors/motor_controller.hpp
+++ b/ut-robomaster/src/utils/motors/motor_controller.hpp
@@ -21,7 +21,7 @@ class MotorController
 public:
     MotorController(src::Drivers* drivers, const MotorConfig motor)
         : constants(motor.constants),
-          motor(drivers, motor.id, motor.canBus, motor.inverted, motor.name),
+          motor(drivers, motor.id, motor.canBus, motor.inverted, motor.name, motor.currentControl),
           velocityPid(motor.velocityPidConstants),
           positionPid(motor.positionPidConstants)
     {
diff --git a/ut-robomaster/taproot/modm/SConscript b/ut-robomaster/taproot/modm/SConscript
index de8d9678..8712510a 100644
--- a/ut-robomaster/taproot/modm/SConscript
+++ b/ut-robomaster/taproot/modm/SConscript
@@ -224,6 +224,7 @@ files = [
     env.File(r"src/modm/platform/i2c/i2c_master_2.cpp"),
     env.File(r"src/modm/platform/rtt/rtt.cpp"),
     env.File(r"src/modm/platform/spi/spi_master_1.cpp"),
+    env.File(r"src/modm/platform/spi/spi_master_2.cpp"),
     env.File(r"src/modm/platform/timer/timer_1.cpp"),
     env.File(r"src/modm/platform/timer/timer_10.cpp"),
     env.File(r"src/modm/platform/timer/timer_4.cpp"),
diff --git a/ut-robomaster/taproot/modm/scons/site_tools/openocd_remote.py b/ut-robomaster/taproot/modm/scons/site_tools/openocd_remote.py
old mode 100755
new mode 100644
diff --git a/ut-robomaster/taproot/modm/scons/site_tools/template.py b/ut-robomaster/taproot/modm/scons/site_tools/template.py
old mode 100755
new mode 100644
diff --git a/ut-robomaster/taproot/modm/src/modm/platform.hpp b/ut-robomaster/taproot/modm/src/modm/platform.hpp
index 5833e7bc..0bba1853 100644
--- a/ut-robomaster/taproot/modm/src/modm/platform.hpp
+++ b/ut-robomaster/taproot/modm/src/modm/platform.hpp
@@ -180,7 +180,9 @@
 #include "platform/rtt/rtt.hpp"
 #include "platform/spi/spi_base.hpp"
 #include "platform/spi/spi_hal_1.hpp"
+#include "platform/spi/spi_hal_2.hpp"
 #include "platform/spi/spi_master_1.hpp"
+#include "platform/spi/spi_master_2.hpp"
 #include "platform/timer/advanced_base.hpp"
 #include "platform/timer/basic_base.hpp"
 #include "platform/timer/general_purpose_base.hpp"
diff --git a/ut-robomaster/taproot/modm/src/modm/platform/spi/spi_hal_2.hpp b/ut-robomaster/taproot/modm/src/modm/platform/spi/spi_hal_2.hpp
new file mode 100644
index 00000000..41c0edd0
--- /dev/null
+++ b/ut-robomaster/taproot/modm/src/modm/platform/spi/spi_hal_2.hpp
@@ -0,0 +1,146 @@
+/*
+ * Copyright (c) 2013, Kevin Läufer
+ * Copyright (c) 2013-2018, Niklas Hauser
+ * Copyright (c) 2014, Daniel Krebs
+ * Copyright (c) 2020, Mike Wolfram
+ *
+ * This file is part of the modm project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this
+ * file, You can obtain one at http://mozilla.org/MPL/2.0/.
+ */
+// ----------------------------------------------------------------------------
+
+#ifndef MODM_STM32_SPI_HAL2_HPP
+#define MODM_STM32_SPI_HAL2_HPP
+
+#include "spi_base.hpp"
+
+namespace modm
+{
+
+namespace platform
+{
+
+/**
+ * Serial peripheral interface (SPI2)
+ *
+ * Very basic implementation that exposes more hardware features than
+ * the regular Spi classes.
+ *
+ * @author		Kevin Laeufer
+ * @ingroup		modm_platform_spi modm_platform_spi_2
+ */
+class SpiHal2 : public SpiBase
+{
+public:
+	/// Enables the clock, resets the hardware and sets the SPE bit
+	static void
+	enable();
+
+	/// Disables the hw module (by disabling its clock line)
+	static void
+	disable();
+
+	/**
+	 * Initialize Spi Peripheral
+	 *
+	 * Enables clocks, the UART peripheral
+	 * Sets baudrate and parity.
+	 */
+	static void
+	initialize(	Prescaler prescaler,
+				MasterSelection masterSelection = MasterSelection::Master,
+				DataMode dataMode   = DataMode::Mode0,
+				DataOrder dataOrder = DataOrder::MsbFirst,
+				DataSize  dataSize  = DataSize::Bit8);
+
+	static void
+	setDataMode(DataMode dataMode);
+
+	static void
+	setDataOrder(DataOrder dataOrder);
+
+	static void
+	setDataSize(DataSize dataSize);
+
+	static void
+	setMasterSelection(MasterSelection masterSelection);
+
+	/// Returns true if data has been received
+	static bool
+	isReceiveRegisterNotEmpty();
+
+	/// Returns true if data can be written
+	static bool
+	isTransmitRegisterEmpty();
+
+	/**
+	 * Write up to 16 Bit to the data register
+	 *
+	 * @warning 	This method does NOT do any sanity checks!!
+	 *				It is your responsibility to check if the register
+	 *				is empty!
+	 */
+	static void
+	write(uint16_t data);
+
+	/**
+	 * Write 8 Bit to the data register
+	 *
+	 * @warning 	This method does NOT do any sanity checks!!
+	 *				It is your responsibility to check if the register
+	 *				is empty!
+	 */
+	static void
+	write(uint8_t data);
+
+	/**
+	 * Returns the value of the data register
+	 *
+	 * @warning 	This method does NOT do any sanity checks!!
+	 *				It is your responsibility to check if the register
+	 *				contains something useful!
+	 */
+	static void
+	read(uint8_t &data);
+
+	/**
+	 * Returns the value of the data register
+	 *
+	 * @warning 	This method does NOT do any sanity checks!!
+	 *				It is your responsibility to check if the register
+	 *				contains something useful!
+	 */
+	static void
+	read(uint16_t &data);
+
+	static void
+	enableInterruptVector(bool enable, uint32_t priority);
+
+	static void
+	enableInterrupt(Interrupt_t interrupt);
+
+	static void
+	disableInterrupt(Interrupt_t interrupt);
+
+	static InterruptFlag_t
+	getInterruptFlags();
+
+	/**
+	 *
+	 *
+	 * @warning 	Not all InterruptFlags can be cleared this way.
+	 */
+	static void
+	acknowledgeInterruptFlag(InterruptFlag_t flags);
+};
+
+} // namespace platform
+
+} // namespace modm
+
+#include "spi_hal_2_impl.hpp"
+
+#endif // MODM_STM32_SPI_HAL2_HPP
\ No newline at end of file
diff --git a/ut-robomaster/taproot/modm/src/modm/platform/spi/spi_hal_2_impl.hpp b/ut-robomaster/taproot/modm/src/modm/platform/spi/spi_hal_2_impl.hpp
new file mode 100644
index 00000000..65568310
--- /dev/null
+++ b/ut-robomaster/taproot/modm/src/modm/platform/spi/spi_hal_2_impl.hpp
@@ -0,0 +1,158 @@
+/*
+ * Copyright (c) 2013, Kevin Läufer
+ * Copyright (c) 2013-2017, Niklas Hauser
+ * Copyright (c) 2014, Daniel Krebs
+ * Copyright (c) 2020, Mike Wolfram
+ *
+ * This file is part of the modm project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this
+ * file, You can obtain one at http://mozilla.org/MPL/2.0/.
+ */
+// ----------------------------------------------------------------------------
+
+#ifndef MODM_STM32_SPI_HAL2_HPP
+#	error 	"Don't include this file directly, use 'spi_hal2.hpp' instead!"
+#endif
+#include <modm/platform/clock/rcc.hpp>
+
+void inline
+modm::platform::SpiHal2::enable()
+{
+	Rcc::enable<Peripheral::Spi2>();
+	SPI2->CR1 |= SPI_CR1_SPE;		// SPI Enable
+}
+
+void inline
+modm::platform::SpiHal2::disable()
+{
+	SPI2->CR1 = 0;
+	Rcc::disable<Peripheral::Spi2>();
+}
+
+void inline
+modm::platform::SpiHal2::initialize(Prescaler prescaler,
+		MasterSelection masterSelection, DataMode dataMode,
+		DataOrder dataOrder, DataSize dataSize)
+{
+	enable();
+	// disable peripheral
+	SPI2->CR1 &= ~SPI_CR1_SPE;
+	// set parameters
+	SPI2->CR1 = 	  static_cast<uint32_t>(dataMode)
+						| static_cast<uint32_t>(dataOrder)
+						| static_cast<uint32_t>(masterSelection)
+						| static_cast<uint32_t>(prescaler)
+						| static_cast<uint32_t>(dataSize);
+	if(masterSelection == MasterSelection::Master) {
+		SPI2->CR2 |=  SPI_CR2_SSOE; // for master mode
+	}
+	// reenable peripheral
+	SPI2->CR1 |= SPI_CR1_SPE;
+}
+
+void inline
+modm::platform::SpiHal2::setDataMode(DataMode dataMode)
+{
+	SPI2->CR1 = (SPI2->CR1 & ~static_cast<uint32_t>(DataMode::All))
+										 | static_cast<uint32_t>(dataMode);
+}
+
+void inline
+modm::platform::SpiHal2::setDataOrder(DataOrder dataOrder)
+{
+	SPI2->CR1 = (SPI2->CR1 & ~static_cast<uint32_t>(DataOrder::All))
+										 | static_cast<uint32_t>(dataOrder);
+}
+
+void inline
+modm::platform::SpiHal2::setDataSize(DataSize dataSize)
+{
+	// TODO: implement as set/reset bit
+	SPI2->CR1 = (SPI2->CR1 & ~static_cast<uint32_t>(DataSize::All))
+										 | static_cast<uint32_t>(dataSize);
+}
+
+void inline
+modm::platform::SpiHal2::setMasterSelection(MasterSelection masterSelection)
+{
+	// TODO: implement as set/reset bit
+	SPI2->CR1 = (SPI2->CR1 & ~static_cast<uint32_t>(MasterSelection::All))
+										 | static_cast<uint32_t>(masterSelection);
+}
+
+inline bool
+modm::platform::SpiHal2::isReceiveRegisterNotEmpty()
+{
+	return static_cast<bool>(getInterruptFlags() & InterruptFlag::RxBufferNotEmpty);
+}
+
+inline bool
+modm::platform::SpiHal2::isTransmitRegisterEmpty()
+{
+	return static_cast<bool>(getInterruptFlags() & InterruptFlag::TxBufferEmpty);
+}
+
+void inline
+modm::platform::SpiHal2::write(uint16_t data)
+{
+	SPI2->DR = data;
+}
+
+void inline
+modm::platform::SpiHal2::write(uint8_t data)
+{
+	write(static_cast<uint16_t>(data));
+}
+
+void inline
+modm::platform::SpiHal2::read(uint8_t &data)
+{
+	data = static_cast<uint8_t>(SPI2->DR);
+}
+
+void inline
+modm::platform::SpiHal2::read(uint16_t &data)
+{
+	data = static_cast<uint16_t>(SPI2->DR);
+}
+
+void inline
+modm::platform::SpiHal2::enableInterruptVector(bool enable, uint32_t priority)
+{
+	if (enable) {
+		// Set priority for the interrupt vector
+		NVIC_SetPriority(SPI2_IRQn, priority);
+		// register IRQ at the NVIC
+		NVIC_EnableIRQ(SPI2_IRQn);
+	}
+	else {
+		NVIC_DisableIRQ(SPI2_IRQn);
+	}
+}
+
+void inline
+modm::platform::SpiHal2::enableInterrupt(Interrupt_t interrupt)
+{
+	SPI2->CR2 |= interrupt.value;
+}
+
+void inline
+modm::platform::SpiHal2::disableInterrupt(Interrupt_t interrupt)
+{
+	SPI2->CR2 &= ~interrupt.value;
+}
+
+modm::platform::SpiHal2::InterruptFlag_t inline
+modm::platform::SpiHal2::getInterruptFlags()
+{
+	return InterruptFlag_t(SPI2->SR);
+}
+
+void inline
+modm::platform::SpiHal2::acknowledgeInterruptFlag(InterruptFlag_t /*flags*/)
+{
+	// TODO: implement; see STM32F3 reference manual p. 736
+	// SPI2->SR = flags.value;
+}
\ No newline at end of file
diff --git a/ut-robomaster/taproot/modm/src/modm/platform/spi/spi_master_2.cpp b/ut-robomaster/taproot/modm/src/modm/platform/spi/spi_master_2.cpp
new file mode 100644
index 00000000..8621cab2
--- /dev/null
+++ b/ut-robomaster/taproot/modm/src/modm/platform/spi/spi_master_2.cpp
@@ -0,0 +1,145 @@
+/*
+ * Copyright (c) 2009, Martin Rosekeit
+ * Copyright (c) 2009-2012, Fabian Greif
+ * Copyright (c) 2010, Georgi Grinshpun
+ * Copyright (c) 2012-2017, Niklas Hauser
+ * Copyright (c) 2013, Kevin Läufer
+ * Copyright (c) 2014, Sascha Schade
+ *
+ * This file is part of the modm project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this
+ * file, You can obtain one at http://mozilla.org/MPL/2.0/.
+ */
+// ----------------------------------------------------------------------------
+
+#include "spi_master_2.hpp"
+
+// Bit0: single transfer state
+// Bit1: block transfer state
+uint8_t
+modm::platform::SpiMaster2::state(0);
+
+uint8_t
+modm::platform::SpiMaster2::count(0);
+
+void *
+modm::platform::SpiMaster2::context(nullptr);
+
+modm::Spi::ConfigurationHandler
+modm::platform::SpiMaster2::configuration(nullptr);
+// ----------------------------------------------------------------------------
+
+uint8_t
+modm::platform::SpiMaster2::acquire(void *ctx, ConfigurationHandler handler)
+{
+	if (context == nullptr)
+	{
+		context = ctx;
+		count = 1;
+		// if handler is not nullptr and is different from previous configuration
+		if (handler and configuration != handler) {
+			configuration = handler;
+			configuration();
+		}
+		return 1;
+	}
+
+	if (ctx == context)
+		return ++count;
+
+	return 0;
+}
+
+uint8_t
+modm::platform::SpiMaster2::release(void *ctx)
+{
+	if (ctx == context)
+	{
+		if (--count == 0)
+			context = nullptr;
+	}
+	return count;
+}
+// ----------------------------------------------------------------------------
+
+modm::ResumableResult<uint8_t>
+modm::platform::SpiMaster2::transfer(uint8_t data)
+{
+	// this is a manually implemented "fast resumable function"
+	// there is no context or nesting protection, since we don't need it.
+	// there are only two states encoded into 1 bit (LSB of state):
+	//   1. waiting to start, and
+	//   2. waiting to finish.
+
+	// LSB != Bit0 ?
+	if ( !(state & Bit0) )
+	{
+		// wait for previous transfer to finish
+		if (!SpiHal2::isTransmitRegisterEmpty())
+			return {modm::rf::Running};
+
+		// start transfer by copying data into register
+		SpiHal2::write(data);
+
+		// set LSB = Bit0
+		state |= Bit0;
+	}
+
+	if (!SpiHal2::isReceiveRegisterNotEmpty())
+		return {modm::rf::Running};
+
+	SpiHal2::read(data);
+
+	// transfer finished
+	state &= ~Bit0;
+	return {modm::rf::Stop, data};
+}
+
+modm::ResumableResult<void>
+modm::platform::SpiMaster2::transfer(
+		const uint8_t * tx, uint8_t * rx, std::size_t length)
+{
+	// this is a manually implemented "fast resumable function"
+	// there is no context or nesting protection, since we don't need it.
+	// there are only two states encoded into 1 bit (Bit1 of state):
+	//   1. initialize index, and
+	//   2. wait for 1-byte transfer to finish.
+
+	// we need to globally remember which byte we are currently transferring
+	static std::size_t index = 0;
+
+	// we are only interested in Bit1
+	switch(state & Bit1)
+	{
+		case 0:
+			// we will only visit this state once
+			state |= Bit1;
+
+			// initialize index and check range
+			index = 0;
+			while (index < length)
+			{
+		default:
+		{
+				// if tx == 0, we use a dummy byte 0x00
+				// else we copy it from the array
+				// call the resumable function
+				modm::ResumableResult<uint8_t> result = transfer(tx ? tx[index] : 0);
+
+				// if the resumable function is still running, so are we
+				if (result.getState() > modm::rf::NestingError)
+					return {modm::rf::Running};
+
+				// if rx != 0, we copy the result into the array
+				if (rx) rx[index] = result.getResult();
+		}
+				index++;
+			}
+
+			// clear the state
+			state &= ~Bit1;
+			return {modm::rf::Stop};
+	}
+}
\ No newline at end of file
diff --git a/ut-robomaster/taproot/modm/src/modm/platform/spi/spi_master_2.hpp b/ut-robomaster/taproot/modm/src/modm/platform/spi/spi_master_2.hpp
new file mode 100644
index 00000000..7ac2a3ba
--- /dev/null
+++ b/ut-robomaster/taproot/modm/src/modm/platform/spi/spi_master_2.hpp
@@ -0,0 +1,152 @@
+/*
+ * Copyright (c) 2009-2011, Fabian Greif
+ * Copyright (c) 2010, Martin Rosekeit
+ * Copyright (c) 2011-2017, Niklas Hauser
+ * Copyright (c) 2012, Georgi Grinshpun
+ * Copyright (c) 2013, Kevin Läufer
+ * Copyright (c) 2014, Sascha Schade
+ *
+ * This file is part of the modm project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this
+ * file, You can obtain one at http://mozilla.org/MPL/2.0/.
+ */
+// ----------------------------------------------------------------------------
+
+#ifndef MODM_STM32_SPI_MASTER2_HPP
+#define MODM_STM32_SPI_MASTER2_HPP
+
+#include <modm/architecture/interface/spi_master.hpp>
+#include <modm/platform/gpio/connector.hpp>
+#include <modm/math/algorithm/prescaler.hpp>
+#include "spi_hal_2.hpp"
+
+namespace modm
+{
+
+namespace platform
+{
+
+/**
+ * Serial peripheral interface (SPI2).
+ *
+ * Simple unbuffered implementation.
+ *
+ * @author	Niklas Hauser
+ * @ingroup	modm_platform_spi modm_platform_spi_2
+ */
+class SpiMaster2 : public modm::SpiMaster
+{
+	static uint8_t state;
+	static uint8_t count;
+	static void *context;
+	static ConfigurationHandler configuration;
+public:
+	using Hal = SpiHal2;
+
+	/// Spi Data Mode, Mode0 is the most common mode
+	enum class
+	DataMode : uint32_t
+	{
+		Mode0 = 0b00,			///< clock normal,   sample on rising  edge
+		Mode1 = SPI_CR1_CPHA,	///< clock normal,   sample on falling edge
+		Mode2 = SPI_CR1_CPOL,	///< clock inverted, sample on falling  edge
+		Mode3 = SPI_CR1_CPOL | SPI_CR1_CPHA
+		///< clock inverted, sample on rising edge
+	};
+
+	/// Spi Data Order, MsbFirst is the most common mode
+	enum class
+	DataOrder : uint32_t
+	{
+		MsbFirst = 0b0,
+		LsbFirst = SPI_CR1_LSBFIRST
+	};
+
+	using DataSize = Hal::DataSize;
+
+public:
+	template< template<Peripheral _> class... Signals >
+	static void
+	connect()
+	{
+		using Connector = GpioConnector<Peripheral::Spi2, Signals...>;
+		using Sck = typename Connector::template GetSignal<Gpio::Signal::Sck>;
+		using Mosi = typename Connector::template GetSignal<Gpio::Signal::Mosi>;
+		using Miso = typename Connector::template GetSignal<Gpio::Signal::Miso>;
+
+		// Connector::disconnect();
+		Sck::setOutput(Gpio::OutputType::PushPull);
+		Mosi::setOutput(Gpio::OutputType::PushPull);
+		Miso::setInput(Gpio::InputType::Floating);
+		Connector::connect();
+	}
+
+	// start documentation inherited
+	template< class SystemClock, baudrate_t baudrate, percent_t tolerance=pct(5) >
+	static void
+	initialize()
+	{
+		constexpr auto result = modm::Prescaler::from_power(SystemClock::Spi2, baudrate, 2, 256);
+		assertBaudrateInTolerance< result.frequency, baudrate, tolerance >();
+
+		// translate the prescaler into the bitmapping
+		constexpr SpiHal2::Prescaler prescaler{result.index << SPI_CR1_BR_Pos};
+
+		// initialize the Spi
+		SpiHal2::initialize(prescaler);
+		state = 0;
+	}
+
+	static modm_always_inline void
+	setDataMode(DataMode mode)
+	{
+		SpiHal2::setDataMode(static_cast<SpiHal2::DataMode>(mode));
+	}
+
+	static modm_always_inline void
+	setDataOrder(DataOrder order)
+	{
+		SpiHal2::setDataOrder(static_cast<SpiHal2::DataOrder>(order));
+	}
+	static modm_always_inline void
+	setDataSize(DataSize size)
+	{
+		SpiHal2::setDataSize(static_cast<SpiHal2::DataSize>(size));
+	}
+
+
+	static uint8_t
+	acquire(void *ctx, ConfigurationHandler handler = nullptr);
+
+	static uint8_t
+	release(void *ctx);
+
+
+	static uint8_t
+	transferBlocking(uint8_t data)
+	{
+		return RF_CALL_BLOCKING(transfer(data));
+	}
+
+	static void
+	transferBlocking(const uint8_t *tx, uint8_t *rx, std::size_t length)
+	{
+		RF_CALL_BLOCKING(transfer(tx, rx, length));
+	}
+
+
+	static modm::ResumableResult<uint8_t>
+	transfer(uint8_t data);
+
+	static modm::ResumableResult<void>
+	transfer(const uint8_t *tx, uint8_t *rx, std::size_t length);
+	// end documentation inherited
+};
+
+} // namespace platform
+
+} // namespace modm
+
+#endif // MODM_STM32_SPI_MASTER2_HPP
\ No newline at end of file
diff --git a/ut-robomaster/taproot/project.xml b/ut-robomaster/taproot/project.xml
index e119fbfa..fbf407a5 100644
--- a/ut-robomaster/taproot/project.xml
+++ b/ut-robomaster/taproot/project.xml
@@ -62,6 +62,7 @@
     <module>modm:platform:adc:3</module>
     <module>modm:platform:i2c:2</module>
     <module>modm:platform:rtt</module>
+    <module>modm:platform:spi:2</module>
 
   </modules>
 </library>
\ No newline at end of file
diff --git a/ut-robomaster/taproot/sim-modm/hosted-darwin/modm/scons/site_tools/template.py b/ut-robomaster/taproot/sim-modm/hosted-darwin/modm/scons/site_tools/template.py
old mode 100755
new mode 100644
diff --git a/ut-robomaster/taproot/sim-modm/hosted-linux/modm/scons/site_tools/template.py b/ut-robomaster/taproot/sim-modm/hosted-linux/modm/scons/site_tools/template.py
old mode 100755
new mode 100644
diff --git a/ut-robomaster/taproot/sim-modm/hosted-windows/modm/scons/site_tools/template.py b/ut-robomaster/taproot/sim-modm/hosted-windows/modm/scons/site_tools/template.py
old mode 100755
new mode 100644
diff --git a/ut-robomaster/taproot/src/tap/algorithms/MahonyAHRS.cpp b/ut-robomaster/taproot/src/tap/algorithms/MahonyAHRS.cpp
index 985f1b66..6cea9545 100644
--- a/ut-robomaster/taproot/src/tap/algorithms/MahonyAHRS.cpp
+++ b/ut-robomaster/taproot/src/tap/algorithms/MahonyAHRS.cpp
@@ -58,7 +58,6 @@ Mahony::Mahony()
     integralFBx = 0.0f;
     integralFBy = 0.0f;
     integralFBz = 0.0f;
-    anglesComputed = 0;
     invSampleFreq = 1.0f / DEFAULT_SAMPLE_FREQ;
     roll = 0.0f;
     pitch = 0.0f;
@@ -87,11 +86,6 @@ void Mahony::update(
         return;
     }
 
-    // Convert gyroscope degrees/sec to radians/sec
-    gx *= 0.0174533f;
-    gy *= 0.0174533f;
-    gz *= 0.0174533f;
-
     // Compute feedback only if accelerometer measurement valid
     // (avoids NaN in accelerometer normalisation)
     if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f)))
@@ -189,7 +183,6 @@ void Mahony::update(
     q1 *= recipNorm;
     q2 *= recipNorm;
     q3 *= recipNorm;
-    anglesComputed = 0;
 }
 
 //-------------------------------------------------------------------------------------------
@@ -200,11 +193,6 @@ void Mahony::updateIMU(float gx, float gy, float gz, float ax, float ay, float a
     float recipNorm;
     float qa, qb, qc;
 
-    // Convert gyroscope degrees/sec to radians/sec
-    gx *= 0.0174533f;
-    gy *= 0.0174533f;
-    gz *= 0.0174533f;
-
     // Compute feedback only if accelerometer measurement valid
     // (avoids NaN in accelerometer normalisation)
     if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f)))
@@ -271,7 +259,7 @@ void Mahony::updateIMU(float gx, float gy, float gz, float ax, float ay, float a
     q1 *= recipNorm;
     q2 *= recipNorm;
     q3 *= recipNorm;
-    anglesComputed = 0;
+    computeAngles();
 }
 
 //-------------------------------------------------------------------------------------------
@@ -281,7 +269,6 @@ void Mahony::computeAngles()
     roll = atan2f(q0 * q1 + q2 * q3, 0.5f - q1 * q1 - q2 * q2);
     pitch = asinf(-2.0f * (q1 * q3 - q0 * q2));
     yaw = atan2f(q1 * q2 + q0 * q3, 0.5f - q2 * q2 - q3 * q3);
-    anglesComputed = 1;
 }
 
 template <typename From, typename To>
diff --git a/ut-robomaster/taproot/src/tap/algorithms/MahonyAHRS.h b/ut-robomaster/taproot/src/tap/algorithms/MahonyAHRS.h
index 20c741ec..463c6b3a 100644
--- a/ut-robomaster/taproot/src/tap/algorithms/MahonyAHRS.h
+++ b/ut-robomaster/taproot/src/tap/algorithms/MahonyAHRS.h
@@ -13,6 +13,8 @@
 // 29/09/2011    SOH Madgwick    Initial release
 // 02/10/2011    SOH Madgwick    Optimised for reduced CPU load
 // 09/06/2020    Matthew Arnold  Update style, use safer casting
+// 04/30/2022    Matthew Arnold  Change input/outputs from degrees to radians
+// 03/06/2025    Chinmay Murthy  Make getters const
 //
 //=============================================================================================
 #ifndef MAHONY_AHRS_H_
@@ -20,6 +22,8 @@
 
 #include <cmath>
 
+#include "modm/math/geometry/angle.hpp"
+
 //--------------------------------------------------------------------------------------------
 // Variable declaration
 
@@ -32,7 +36,6 @@ class Mahony
     float integralFBx, integralFBy, integralFBz;  // integral error terms scaled by Ki
     float invSampleFreq;
     float roll, pitch, yaw;
-    char anglesComputed;
     static float invSqrt(float x);
     void computeAngles();
 
@@ -56,7 +59,6 @@ class Mahony
         integralFBx = 0.0f;
         integralFBy = 0.0f;
         integralFBz = 0.0f;
-        anglesComputed = 0;
         roll = 0.0f;
         pitch = 0.0f;
         yaw = 0.0f;
@@ -72,37 +74,9 @@ class Mahony
         float my,
         float mz);
     void updateIMU(float gx, float gy, float gz, float ax, float ay, float az);
-    float getRoll()
-    {
-        if (!anglesComputed) computeAngles();
-        return roll * 57.29578f;
-    }
-    float getPitch()
-    {
-        if (!anglesComputed) computeAngles();
-        return pitch * 57.29578f;
-    }
-    float getYaw()
-    {
-        if (!anglesComputed) computeAngles();
-        float yawDegrees = yaw * 57.29578f;
-        return fmod(yawDegrees + 360.0f, 360.0f);
-    }
-    float getRollRadians()
-    {
-        if (!anglesComputed) computeAngles();
-        return roll;
-    }
-    float getPitchRadians()
-    {
-        if (!anglesComputed) computeAngles();
-        return pitch;
-    }
-    float getYawRadians()
-    {
-        if (!anglesComputed) computeAngles();
-        return yaw;
-    }
+    float getRoll() const { return roll; }
+    float getPitch() const { return pitch; }
+    float getYaw() const { return fmod(yaw + M_TWOPI, M_TWOPI); }
 };
 
 #endif  // MAHONY_AHRS_H_
diff --git a/ut-robomaster/taproot/src/tap/algorithms/butterworth.hpp b/ut-robomaster/taproot/src/tap/algorithms/butterworth.hpp
new file mode 100644
index 00000000..6051dd71
--- /dev/null
+++ b/ut-robomaster/taproot/src/tap/algorithms/butterworth.hpp
@@ -0,0 +1,484 @@
+/*****************************************************************************/
+/********** !!! WARNING: CODE GENERATED BY TAPROOT. DO NOT EDIT !!! **********/
+/*****************************************************************************/
+
+/*
+ * Copyright (c) 2024-2025 Advanced Robotics at the University of Washington <robomstr@uw.edu>
+ *
+ * This file is part of Taproot.
+ *
+ * Taproot is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Taproot is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Taproot.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+#ifndef TAPROOT_BUTTERWORTH_HPP_
+#define TAPROOT_BUTTERWORTH_HPP_
+#include <array>
+#include <cmath>
+#include <complex>
+#include <cstdint>
+
+#include "modm/math/geometry/angle.hpp"
+
+/**
+ * @class Butterworth
+ * @brief Implementation of Butterworth filter design in the discrete domain.
+ *
+ * This header file provides a comprehensive implementation of Butterworth filters,
+ * including low-pass, high-pass, band-pass, and band-stop filters. The Butterworth
+ * filter is known for its maximally flat frequency response in the passband, making
+ * it ideal for applications requiring minimal signal distortion.
+ *
+ * The implementation includes:
+ * - Conversion of poles and zeros from the Laplace domain to the Z domain using
+ *   the bilinear transform.
+ * - Expansion of polynomial coefficients from a set of poles or zeros.
+ * - Evaluation of the frequency response of the filter at a given frequency.
+ * - A templated Butterworth filter class for designing filters of arbitrary order
+ *   and type.
+ *
+ * The design process includes pre-warping of frequencies for the bilinear transform,
+ * generation of prototype poles, and scaling of coefficients.
+ *
+ * The following transforms map a lowpass prototype into other filter types (s-domain):
+ *
+ * - **Lowpass to Lowpass**:
+ *   \f$ s \rightarrow \frac{s}{\Omega_c} \f$
+ *
+ * - **Lowpass to Highpass**:
+ *   \f$ s \rightarrow \frac{\Omega_c}{s} \f$
+ *
+ * - **Lowpass to Bandpass**:
+ *   \f$ s \rightarrow \frac{s^2 + \Omega_0^2}{B s} \f$
+ *
+ * - **Lowpass to Bandstop**:
+ *   \f$ s \rightarrow \frac{B s}{s^2 + \Omega_0^2} \f$
+ *
+ * Where:
+ * \f$ \Omega_0 = \sqrt{\Omega_l \cdot \Omega_h}, \quad B = \Omega_h - \Omega_l \f$
+ *
+ * After analog transformation, apply the bilinear transform:
+ * \f[ s = \frac{2}{T} \cdot \frac{z - 1}{z + 1} \f]
+ *
+ * @note
+ *    This implementation is designed for C++20 ``constexpr``, enabling
+ *    compile-time computation of filter configurations.
+ *
+ * @warning
+ *    High-order filters can introduce high phase delays and should be used with caution.
+ *    For most applications, low-pass and high-pass filters of order 2 or lower are sufficient.
+ *
+ *    If results are suspicious, verify filter coefficients using external tools
+ *    such as MATLAB or Python (e.g., SciPy).
+ *
+ *
+ * @section Usage
+ *
+ * To use this implementation, include this header file and instantiate the
+ * ``Butterworth`` class with the desired filter order, type, and parameters.
+ * Then, pass those coefficients into a ``DiscreteFilter``.
+ *
+ * @code
+ *    static constexpr Butterworth<1, LOWPASS> filter(wc, Ts);
+ *    auto naturalCoeffs = filter.getNaturalResponseCoefficients();
+ *    auto forcedCoeffs = filter.getForcedResponseCoefficients();
+ *    DiscreteFilter<2> Filter(naturalCoeffs, forcedCoeffs);
+ *
+ * @endcode
+ *
+ * @author Aiden Prevey
+ * @date 4/29/2025
+ * @version 2.0
+ */
+
+namespace tap
+{
+namespace algorithms
+{
+enum FilterType : uint8_t
+{
+    LOWPASS = 0b00,
+    HIGHPASS = 0b01,
+    BANDPASS = 0b10,
+    BANDSTOP = 0b11,
+};
+
+/**
+ * used to transform poles from the laplace domain to the
+ * Z domain for discrete time using the bilinear transform
+ *
+ * @param [in] s a pole or zero from the laplace domain
+ * @param [in] Ts the sample time
+ * @return a complex number corresponding to the Z domain location
+ */
+constexpr std::complex<double> s2z(std::complex<double> s, double Ts)
+{
+    return (1.0 + (Ts / 2) * s) / (1.0 - (Ts / 2) * s);
+}
+
+/**
+ * used to multiply out a series of zeros to obtain a list of coefficients
+ *
+ * @param [in] zeros a vector of complex poles or zeros to multiply out, works
+ * for any polynomial in the form of (x - z1)(x - z2)...(x - zn) where z1, z2, ... zn
+ * are the value of zeros.
+ *
+ * @return a vector of coefficients for the polynomial with the 0th index being the
+ * constant term and the last index being the leading coefficient
+ * @note the coefficients are returned as doubles, the imaginary part of the
+ * complex number is ignored
+ */
+template <uint8_t ORDER>
+constexpr std::array<double, ORDER + 1> expandPolynomial(
+    std::array<std::complex<double>, ORDER> zeros)
+{
+    std::array<std::complex<double>, ORDER + 1> coefficients{
+        std::complex<double>(1.0, 0.0)};  // Start with 1
+
+    for (size_t i = 0; i < ORDER; i++)
+    {
+        // Multiply current polynomial by (x - zero)
+        std::array<std::complex<double>, ORDER + 1> newCoefficients{std::complex<double>(0.0, 0.0)};
+        for (size_t j = 0; j < i + 1; j++)
+        {
+            newCoefficients[j] -=
+                coefficients[j] * zeros[i];             // Multiply current coefficient by zero
+            newCoefficients[j + 1] += coefficients[j];  // Shift current coefficient
+        }
+        coefficients.swap(newCoefficients);
+    }
+    std::array<double, ORDER + 1> stripped_coefficients{};
+    // Extract the real part of each coefficient, the imaginary appears to be an
+    // artifact of double
+    for (size_t i = 0; i < ORDER + 1; i++)
+    {
+        stripped_coefficients[i] = coefficients[i].real();
+    }
+
+    return stripped_coefficients;
+}
+
+/**
+ * used to evaluate the frequency response of a filter at a given frequency
+ * @param [in] b the numerator coefficients of the filter
+ * @param [in] a the denominator coefficients of the filter
+ * @param [in] w the frequency to evaluate at in rad/s
+ * @param [in] Ts the sample time
+ * @return the frequency response of the filter at the given frequency
+ *
+ */
+
+template <uint8_t ORDER>
+constexpr std::complex<double> evaluateFrequencyResponse(
+    const std::array<double, ORDER + 1> &b,
+    const std::array<double, ORDER + 1> &a,
+    double w,
+    double Ts)
+{
+    std::complex<double> j(0, 1);  // imaginary unit i = sqrt(-1)
+    std::complex<double> numerator(0, 0);
+    std::complex<double> denominator(0, 0);
+
+    double omega = w * Ts;  // omega is in terms of rad/sample
+
+    // Evaluate numerator: b0 + b1 * e^{-jω} + b2 * e^{-j2ω} + ...
+    for (size_t k = 0; k < b.size(); ++k)
+    {
+        numerator += b[k] * (std::cos(omega * static_cast<double>(k)) -
+                             j * std::sin(omega * static_cast<double>(k)));
+    }
+
+    // Evaluate denominator: a0 + a1 * e^{-jω} + a2 * e^{-j2ω} + ...
+    for (size_t k = 0; k < a.size(); ++k)
+    {
+        denominator += a[k] * (std::cos(omega * static_cast<double>(k)) -
+                               j * std::sin(omega * static_cast<double>(k)));
+    }
+
+    return numerator / denominator;
+}
+/**
+ * used to calculate the magnitude of a complex number, created for constexpr useage
+ * @param [in] z the complex number to calculate the magnitude of
+ * @return the magnitude of the complex number
+ */
+constexpr double complex_abs(std::complex<double> z)
+{
+    return std::sqrt(z.real() * z.real() + z.imag() * z.imag());
+}
+
+/**
+ * used to calculate the square root of a complex number, created for constexpr useage
+ * @param [in] z the complex number to calculate the square root of
+ * @return the square root of the complex number
+ */
+
+constexpr std::complex<double> complex_sqrt(std::complex<double> z)
+{
+    double r = std::sqrt(complex_abs(z));
+    double theta = std::atan2(z.imag(), z.real()) * 0.5;
+    return {r * std::cos(theta), r * std::sin(theta)};
+}
+
+template <uint8_t ORDER, FilterType Type = LOWPASS, typename T = float>
+class Butterworth
+{
+public:
+    /**
+     * @param[in] wc   for LOW/HIGHPASS: cutoff ωc.
+     *                 for BANDPASS/BANDSTOP: lower edge ωl.
+     * @param[in] Ts   sample time.
+     * @param[in] type filter type, LOWPASS, HIGHPASS, BANDPASS, BANDSTOP.
+     *                 defaults to LOWPASS.
+     * @param[in] wh   upper edge ωh (only used for band filters).
+     */
+    constexpr Butterworth(double wc, double Ts, double wh = 0.0)
+        : naturalResponseCoefficients(),
+          forcedResponseCoefficients()
+    {
+        const int n = ORDER;
+
+        // For band filters we treat wc as ωl
+        double wl = wc;
+        double whp = wh;
+        std::array<std::complex<double>, 2 * ORDER> bandpass_stop_poles;
+
+        // pre-warp all edges for bilinear transform
+        wl = (2.0 / Ts) * std::tan(wl * (Ts / 2.0));
+        whp = (2.0 / Ts) * std::tan(whp * (Ts / 2.0));
+
+        // generate N prototype poles on unit circle
+        std::array<std::complex<double>, COEFFICIENTS - 1> poles;
+        for (int k = 0; k < n; ++k)
+        {
+            double theta = M_PI * (2.0 * k + 1) / (2.0 * n) + M_PI / 2.0;
+            poles[k] = std::complex<double>(std::cos(theta), std::sin(theta));
+        }
+
+        std::array<std::complex<double>, COEFFICIENTS - 1> zPoles;
+
+        // apply the appropriate s-domain transform to each pole
+        switch (Type)
+        {
+            case LOWPASS:
+            {
+                // poles are multiplied by wl to scale them to the desired cutoff frequency
+                for (int j = 0; j < n; ++j) poles[j] = poles[j] * wl;
+
+                // now map each analog pole into the z-plane
+                for (int i = 0; i < COEFFICIENTS - 1; ++i) zPoles[i] = s2z(poles[i], Ts);
+                break;
+            }
+            case HIGHPASS:
+            {
+                // applys the inverse transform of the butterworth lowpass filter
+                for (int j = 0; j < n; ++j)
+                {
+                    poles[j] = wl / poles[j];
+                }
+                // now map each analog pole into the z-plane
+                for (int i = 0; i < COEFFICIENTS - 1; ++i) zPoles[i] = s2z(poles[i], Ts);
+                break;
+            }
+            // In the case of a bandpass or a bandstop the amount of poles doubles.
+            case BANDPASS:
+            {
+                // check for validity of the filter edges
+                if (whp <= wl)
+                {
+                    throw std::invalid_argument("wh must be > wl for BANDPASS");
+                }
+                /*
+                 *  transform in the form of s → (s² + Ω₀²) / (B · s)
+                 *  where:
+                 *      Ω₀ = √(Ω_low * Ω_high)      // Center frequency (rad/sec)
+                 *      B  = Ω_high - Ω_low         // Bandwidth (rad/sec)
+                 */
+                double B = whp - wl;
+                double W0sq = whp * wl;
+
+                for (int j = 0; j < ORDER; ++j)
+                {
+                    std::complex<double> p = poles[j];
+
+                    std::complex<double> discriminant = (p * B) * (p * B) - 4.0 * W0sq;
+                    std::complex<double> root = complex_sqrt(discriminant);
+
+                    bandpass_stop_poles[2 * j] = (p * B + root) * 0.5;
+                    bandpass_stop_poles[2 * j + 1] = (p * B - root) * 0.5;
+                }
+
+                // now map each analog pole into the z-plane
+                for (int i = 0; i < COEFFICIENTS - 1; ++i)
+                    zPoles[i] = s2z(bandpass_stop_poles[i], Ts);
+
+                break;
+            }
+
+            case BANDSTOP:
+            {
+                // check for validity of the filter edges
+                if (whp <= wl)
+                {
+                    throw std::invalid_argument("wh must be > wl for BANDSTOP");
+                }
+                /*
+                 *  transform in the form of  s → B · s / (s² + Ω₀²)
+                 *  where:
+                 *      Ω₀ = √(Ω_low * Ω_high)      // Center frequency (rad/sec)
+                 *      B  = Ω_high - Ω_low         // Bandwidth (rad/sec)
+                 */
+                double B = whp - wl;
+                double W0sq = whp * wl;
+                for (int j = 0; j < n; ++j)
+                {
+                    std::complex<double> p = poles[j];
+
+                    std::complex<double> discriminant = B * B - (4.0 * -p * W0sq);
+                    std::complex<double> root = complex_sqrt(discriminant);
+
+                    bandpass_stop_poles[2 * j] = (B + root) / (2.0 * p);
+                    bandpass_stop_poles[2 * j + 1] = (B - root) / (2.0 * p);
+                }
+
+                // now map each analog pole into the z-plane
+                for (int i = 0; i < COEFFICIENTS - 1; ++i)
+                    zPoles[i] = s2z(bandpass_stop_poles[i], Ts);
+
+                break;
+            }
+            default:
+                throw std::invalid_argument("Unknown filter type");
+        }
+
+        std::array<std::complex<double>, COEFFICIENTS - 1> zZeros;
+
+        switch (Type)
+        {
+            case LOWPASS:
+                // zeros: for Butterworth lowpass all z-zeros at z = –1
+                zZeros.fill(std::complex<double>(-1, 0));
+                break;
+            case HIGHPASS:
+                // zeros: for butterworth highpass all z-zeros are at z = 1
+                zZeros.fill(std::complex<double>(1, 0));
+                break;
+            case BANDPASS:
+                // zeros: for butterworth bandpass all z-zeros are at z = ±1
+                for (int i = 0; i < COEFFICIENTS - 1; ++i)
+                {
+                    zZeros[i] =
+                        (i % 2 == 0) ? std::complex<double>(1, 0) : std::complex<double>(-1, 0);
+                }
+                break;
+            case BANDSTOP:
+            {
+                /* zeros: for butterworth bandstop are in a circle with radius 1 at the
+                 * center of the z-domain with an angle in radians per sample of the
+                 * center frequency. The zeros are complex conjugates of each other.
+                 */
+
+                /* the notch (center) frequency in radians/sample */
+                double omega0 = std::sqrt(wl * whp) * Ts;
+
+                double realPart = std::cos(omega0);
+                double imagPart = std::sin(omega0);
+
+                std::complex<double> zeroPlus(realPart, imagPart);    // e^(+jω0)
+                std::complex<double> zeroMinus(realPart, -imagPart);  // e^(-jω0)
+
+                for (int i = 0; i < COEFFICIENTS - 1; i += 2)
+                {
+                    zZeros[i] = zeroPlus;                                 // first of pair
+                    if (i + 1 < COEFFICIENTS) zZeros[i + 1] = zeroMinus;  // second of pair
+                }
+            }
+            break;
+        }
+
+        // get expanded polynomials
+        auto b = expandPolynomial<COEFFICIENTS - 1>(zZeros);
+        auto a = expandPolynomial<COEFFICIENTS - 1>(zPoles);
+
+        // scale the gain properly
+        switch (Type)
+        {
+            case LOWPASS:
+            {
+                // Eval at DC
+                auto freqResp = evaluateFrequencyResponse<COEFFICIENTS - 1>(b, a, 0, Ts);
+                auto mag = complex_abs(freqResp);
+                double scale = 1 / mag;
+                for (auto &coef : b) coef *= scale;
+                break;
+            }
+            case HIGHPASS:
+            {
+                // Eval at niquest
+                auto freqResp = evaluateFrequencyResponse<COEFFICIENTS - 1>(b, a, M_PI / Ts, Ts);
+                auto mag = complex_abs(freqResp);
+                double scale = 1 / mag;
+                for (auto &coef : b) coef *= scale;
+                break;
+            }
+            case BANDPASS:
+            {
+                // Eval at center frequency
+                auto freqResp =
+                    evaluateFrequencyResponse<COEFFICIENTS - 1>(b, a, std::sqrt(wl * whp), Ts);
+                auto mag = complex_abs(freqResp);
+                double scale = 1 / mag;
+                for (auto &coef : b) coef *= scale;
+                break;
+            }
+            case BANDSTOP:
+            {
+                // Eval at dc gain
+                auto freqResp = evaluateFrequencyResponse<COEFFICIENTS - 1>(b, a, 0, Ts);
+                auto mag = complex_abs(freqResp);
+                double scale = 1 / mag;
+                for (auto &coef : b) coef *= scale;
+                break;
+            }
+        }
+
+        // store in member arrays (reverse order)
+        for (size_t i = 0; i < COEFFICIENTS; ++i)
+        {
+            naturalResponseCoefficients[COEFFICIENTS - i - 1] = a[i];
+            forcedResponseCoefficients[COEFFICIENTS - i - 1] = b[i];
+        }
+    }
+
+    static constexpr int COEFFICIENTS = (1 + ((Type & 0b10) != 0)) * ORDER + 1;
+
+    std::array<T, COEFFICIENTS> getNaturalResponseCoefficients() const
+    {
+        return naturalResponseCoefficients;
+    }
+
+    std::array<T, COEFFICIENTS> getForcedResponseCoefficients() const
+    {
+        return forcedResponseCoefficients;
+    }
+
+private:
+    std::array<T, COEFFICIENTS> naturalResponseCoefficients;
+    std::array<T, COEFFICIENTS> forcedResponseCoefficients;
+};
+
+}  // namespace algorithms
+
+}  // namespace tap
+
+#endif  // TAPROOT_BUTTERWORTH_HPP_
\ No newline at end of file
diff --git a/ut-robomaster/taproot/src/tap/algorithms/contiguous_float.cpp b/ut-robomaster/taproot/src/tap/algorithms/contiguous_float.cpp
deleted file mode 100644
index d49bec0a..00000000
--- a/ut-robomaster/taproot/src/tap/algorithms/contiguous_float.cpp
+++ /dev/null
@@ -1,191 +0,0 @@
-/*****************************************************************************/
-/********** !!! WARNING: CODE GENERATED BY TAPROOT. DO NOT EDIT !!! **********/
-/*****************************************************************************/
-
-/*
- * Copyright (c) 2020-2021 Advanced Robotics at the University of Washington <robomstr@uw.edu>
- *
- * This file is part of Taproot.
- *
- * Taproot is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * Taproot is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with Taproot.  If not, see <https://www.gnu.org/licenses/>.
- */
-
-#include "contiguous_float.hpp"
-
-#include <cmath>
-
-namespace tap
-{
-namespace algorithms
-{
-ContiguousFloat::ContiguousFloat(const float value, const float lowerBound, const float upperBound)
-{
-    this->value = value;
-
-    this->lowerBound = lowerBound;
-    this->upperBound = upperBound;
-
-    this->validateBounds();
-    this->reboundValue();
-}
-
-float ContiguousFloat::reboundValue()
-{
-    if (value < lowerBound)
-    {
-        value = upperBound + fmodf(value - lowerBound, upperBound - lowerBound);
-    }
-    else if (value > upperBound)
-    {
-        value = lowerBound + fmodf(value - upperBound, upperBound - lowerBound);
-    }
-
-    return value;
-}
-
-float ContiguousFloat::unwrapBelow() const { return lowerBound - (upperBound - value); }
-
-float ContiguousFloat::unwrapAbove() const { return upperBound + (value - lowerBound); }
-
-float ContiguousFloat::difference(const float otherValue) const
-{
-    return difference(ContiguousFloat(otherValue, lowerBound, upperBound));
-}
-
-float ContiguousFloat::difference(const ContiguousFloat& otherValue) const
-{
-    // Find the shortest path to the target (smallest difference)
-    float aboveDiff = otherValue.getValue() - this->unwrapAbove();
-    float belowDiff = otherValue.getValue() - this->unwrapBelow();
-    float stdDiff = otherValue.getValue() - this->getValue();
-
-    float finalDiff = stdDiff;
-
-    if (fabs(aboveDiff) < fabs(belowDiff) && fabs(aboveDiff) < fabs(stdDiff))
-    {
-        finalDiff = aboveDiff;
-    }
-    else if (fabs(belowDiff) < fabs(aboveDiff) && fabs(belowDiff) < fabs(stdDiff))
-    {
-        finalDiff = belowDiff;
-    }
-
-    return finalDiff;
-}
-
-void ContiguousFloat::shiftBounds(const float shiftMagnitude)
-{
-    upperBound += shiftMagnitude;
-    lowerBound += shiftMagnitude;
-    reboundValue();
-}
-
-void ContiguousFloat::shiftValue(const float shiftMagnitude)
-{
-    value += shiftMagnitude;
-    reboundValue();
-}
-
-float ContiguousFloat::limitValue(
-    const ContiguousFloat& valueToLimit,
-    const float min,
-    const float max,
-    int* status)
-{
-    ContiguousFloat minContig(min, valueToLimit.lowerBound, valueToLimit.upperBound);
-    ContiguousFloat maxContig(max, valueToLimit.lowerBound, valueToLimit.upperBound);
-    return limitValue(valueToLimit, minContig, maxContig, status);
-}
-
-float ContiguousFloat::limitValue(
-    const ContiguousFloat& valueToLimit,
-    const ContiguousFloat& min,
-    const ContiguousFloat& max,
-    int* status)
-{
-    if (min.getValue() == max.getValue())
-    {
-        return valueToLimit.getValue();
-    }
-    if ((min.getValue() < max.getValue() &&
-         (valueToLimit.getValue() > max.getValue() || valueToLimit.getValue() < min.getValue())) ||
-        (min.getValue() > max.getValue() && valueToLimit.getValue() > max.getValue() &&
-         valueToLimit.getValue() < min.getValue()))
-    {
-        float targetMinDifference = fabs(valueToLimit.difference(min));
-        float targetMaxDifference = fabs(valueToLimit.difference(max));
-
-        if (targetMinDifference < targetMaxDifference)
-        {
-            *status = 1;
-            return min.getValue();
-        }
-        else
-        {
-            *status = 2;
-            return max.getValue();
-        }
-    }
-    else
-    {
-        *status = 0;
-        return valueToLimit.getValue();
-    }
-}
-
-// Getters/Setters ----------------
-// Value
-float ContiguousFloat::getValue() const { return value; }
-
-void ContiguousFloat::setValue(const float newValue)
-{
-    value = newValue;
-    this->reboundValue();
-}
-
-// Upper bound
-float ContiguousFloat::getUpperBound() const { return upperBound; }
-
-void ContiguousFloat::setUpperBound(const float newValue)
-{
-    upperBound = newValue;
-
-    this->validateBounds();
-    this->reboundValue();
-}
-
-// Lower bound
-float ContiguousFloat::getLowerBound() const { return lowerBound; }
-
-void ContiguousFloat::setLowerBound(const float newValue)
-{
-    lowerBound = newValue;
-
-    this->validateBounds();
-    this->reboundValue();
-}
-
-void ContiguousFloat::validateBounds()
-{
-    if (lowerBound > upperBound)
-    {
-        float tmp = this->lowerBound;
-        this->lowerBound = this->upperBound;
-        this->upperBound = tmp;
-    }
-}
-
-}  // namespace algorithms
-
-}  //  namespace tap
diff --git a/ut-robomaster/taproot/src/tap/algorithms/contiguous_float.hpp b/ut-robomaster/taproot/src/tap/algorithms/contiguous_float.hpp
deleted file mode 100644
index 4ee5f516..00000000
--- a/ut-robomaster/taproot/src/tap/algorithms/contiguous_float.hpp
+++ /dev/null
@@ -1,218 +0,0 @@
-/*****************************************************************************/
-/********** !!! WARNING: CODE GENERATED BY TAPROOT. DO NOT EDIT !!! **********/
-/*****************************************************************************/
-
-/*
- * Copyright (c) 2020-2021 Advanced Robotics at the University of Washington <robomstr@uw.edu>
- *
- * This file is part of Taproot.
- *
- * Taproot is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * Taproot is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with Taproot.  If not, see <https://www.gnu.org/licenses/>.
- */
-
-#ifndef TAPROOT_CONTIGUOUS_FLOAT_HPP_
-#define TAPROOT_CONTIGUOUS_FLOAT_HPP_
-
-namespace tap
-{
-namespace algorithms
-{
-/**
- * Wraps a float to allow easy comparison and manipulation of sensor readings
- * that wrap (e.g. -180 to 180).
- *
- * For bounds 0 - 10, logically:
- *   - 10 + 1 == 1
- *   - 0 - 1 == 9
- *   - 0 == 10
- *
- * Credit to: https://github.com/Team488/SeriouslyCommonLib/blob/af2ce83a830299a8ab3773bec9b8ccc6ab
- *            5a3367/src/main/java/xbot/common/math/ContiguousDouble.java
- */
-class ContiguousFloat
-{
-public:
-    ContiguousFloat(const float value, const float lowerBound, const float upperBound);
-
-    /**
-     * Shifts the value so that it still represents the same position but is
-     * within the current bounds.
-     *
-     * @return the new value for chaining functions.
-     */
-    float reboundValue();
-
-    /**
-     * Computes the difference between two values (other - this), accounting for
-     * wrapping. Treats the given 'other' value as a number within the same bounds
-     * as the current instance.
-     *
-     * @param[in] otherValue the other value to compare against.
-     * @return the computed difference.
-     */
-    float difference(const float otherValue) const;
-
-    /**
-     * Computes the difference between two values (other - this), accounting for
-     * wrapping.
-     *
-     * @param[in] otherValue the other value to compare against (must have the same bounds
-     *      as the current instance).
-     * @return the computed difference.
-     */
-    float difference(const ContiguousFloat& otherValue) const;
-
-    /**
-     * Shifts both bounds by the specified amount
-     *
-     * @param[in] shiftMagnitude the amount to add to each bound.
-     */
-    void shiftBounds(const float shiftMagnitude);
-
-    /**
-     * Shifts value by the specified amount (addition).
-     *
-     * @param[in] shiftMagnitude The amount to add to the current value.
-     */
-    void shiftValue(const float shiftMagnitude);
-
-    /**
-     * Limits the passed in contiguous float between the closest of the
-     * min or max value if outside the min and max value's wrapped range.
-     *
-     * The min and max must have the same wrapped bounds as the valueToLimit.
-     *
-     *
-     * For example given a value wrapped from -10 to 10, with the following
-     * conditions:
-     * - valueToLimit: 5, min: 1, max: 4, returns 4.
-     * - valueToLimit: 9, min: 1, max: 3, returns 1 (since valueToLimit is closest to 1).
-     * - valueToLimit: 9, min: 2, max: 1, returns 9 (since the range between min and max
-     *                 starts at 2, goes up to 9, then wraps around to 1).
-     *
-     * @param[in] valueToLimit the ContigousFloat whose value it is to limit
-     * @param[in] min the ContiguousFloat with the same bounds as valueToLimit that
-     *      valueToLimit will be limited below.
-     * @param[in] max the ContiguousFloat with the same bounds as valueToLimit that
-     *      valueToLimit will be limited above.
-     * @param[out] status the status result (what operation the limitValue function performed). The
-     * status codes are described below:
-     *  - 0: No limiting performed
-     *  - 1: Limited to min value
-     *  - 2: Limited to max value
-     * @return the limited value.
-     */
-    static float limitValue(
-        const ContiguousFloat& valueToLimit,
-        const ContiguousFloat& min,
-        const ContiguousFloat& max,
-        int* status);
-
-    /**
-     * Runs the limitValue function from above, wrapping the min and max passed in to
-     * the same bounds as those of valueToLimit's.
-     *
-     * @see limitValue.
-     * @param[in] valueToLimit the ContigousFloat whose value it is to limit
-     * @param[in] min the ContiguousFloat with the same bounds as valueToLimit that
-     *      valueToLimit will be limited below.
-     * @param[in] max the ContiguousFloat with the same bounds as valueToLimit that
-     *      valueToLimit will be limited above.
-     * @param[out] status the status result (what operation the limitValue function performed). The
-     * status codes are described below:
-     *  - 0: No limiting performed
-     *  - 1: Limited to min value
-     *  - 2: Limited to max value
-     * @return the limited value.
-     */
-    static float limitValue(
-        const ContiguousFloat& valueToLimit,
-        const float min,
-        const float max,
-        int* status);
-
-    // Getters/Setters ----------------
-
-    /**
-     * Returns the wrapped value.
-     */
-    float getValue() const;
-
-    void setValue(const float newValue);
-
-    /**
-     * Returns the value's upper bound.
-     */
-    float getUpperBound() const;
-
-    /**
-     * Sets the upper bound to newValue.
-     */
-    void setUpperBound(const float newValue);
-
-    /**
-     * Returns the value's lower bound.
-     */
-    float getLowerBound() const;
-
-    /**
-     * Sets the lower bound to newValue.
-     */
-    void setLowerBound(const float newValue);
-
-private:
-    /**
-     * The wrapped value.
-     */
-    float value;
-
-    /**
-     * The lower bound to wrap around.
-     */
-    float lowerBound;
-    /**
-     * The upper bound to wrap around.
-     */
-    float upperBound;
-
-    /**
-     * Flips the lower and upper bounds if the lower bound is larger than the
-     * upper bound.
-     */
-    void validateBounds();
-
-    /**
-     * Calculates a number representing the current value that is higher than
-     * (or equal to) the upper bound. Used to make normal numerical comparisons
-     * without needing to handle wrap cases.
-     *
-     * @return the computed value
-     */
-    float unwrapAbove() const;
-
-    /**
-     * Calculates a number representing the current value that is lower than (or
-     * equal to) the lower bound. Used to make normal numerical comparisons
-     * without needing to handle wrap cases.
-     *
-     * @return the computed value
-     */
-    float unwrapBelow() const;
-};  // class ContiguousFloat
-
-}  // namespace algorithms
-
-}  // namespace tap
-
-#endif  // TAPROOT_CONTIGUOUS_FLOAT_HPP_
diff --git a/ut-robomaster/taproot/src/tap/algorithms/discrete_filter.hpp b/ut-robomaster/taproot/src/tap/algorithms/discrete_filter.hpp
new file mode 100644
index 00000000..363ad69b
--- /dev/null
+++ b/ut-robomaster/taproot/src/tap/algorithms/discrete_filter.hpp
@@ -0,0 +1,135 @@
+/*****************************************************************************/
+/********** !!! WARNING: CODE GENERATED BY TAPROOT. DO NOT EDIT !!! **********/
+/*****************************************************************************/
+
+/*
+ * Copyright (c) 2024-2025 Advanced Robotics at the University of Washington <robomstr@uw.edu>
+ *
+ * This file is part of Taproot.
+ *
+ * Taproot is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Taproot is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Taproot.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+#ifndef TAPROOT_DISCRETE_FILTER_HPP_
+#define TAPROOT_DISCRETE_FILTER_HPP_
+
+#include <array>
+#include <cstdint>
+
+namespace tap
+{
+namespace algorithms
+{
+/**
+ * @brief DiscreteFilter class implements a discrete-time filter using the finite difference
+ * equation.
+ * @tparam SIZE The size of the filter coefficients.
+ *
+ * This class provides methods to filter input data using the finite difference equation and
+ * maintain the internal state of the filter.
+ */
+template <uint8_t SIZE, typename T = float>
+class DiscreteFilter
+{
+public:
+    /**
+     * @brief Constructor for the DiscreteFilter class.
+     * @param [in] naturalResponseCoefficients The coefficients for the natural response (a).
+     * @param [in] forcedResponseCoefficients The coefficients for the forced response (b).
+     *
+     * This constructor initializes the filter with the given coefficients and resets the filter
+     * state to zero.
+     */
+    DiscreteFilter(
+        std::array<T, SIZE> &naturalResponseCoefficients,
+        std::array<T, SIZE> &forcedResponseCoefficients)
+        : naturalResponseCoefficients(naturalResponseCoefficients),
+          forcedResponseCoefficients(forcedResponseCoefficients)
+    {
+        reset();
+    }
+
+    /**
+     * @brief Filters the input data using the finite difference equation.
+     * @param [in] dat The input data to be filtered.
+     * @return The filtered output data.
+     *
+     * This function implements a discrete-time filter using the finite difference equation.
+     * It updates the internal state of the filter based on the input data and returns the
+     * filtered output.
+     *
+     * \f$ y(n)=\frac{1}{a_{0}}\left[\sum_{\kappa=0}^{M} b_{\kappa}x(n-\kappa)-\sum_{k=1}^{N}
+     * a_{k}y(n-k)\right]. \qquad{(2)} \f$
+     *
+     */
+    T filterData(float dat)
+    {
+        for (int i = SIZE - 1; i > 0; i--)
+        {
+            forcedResponse[i] = forcedResponse[i - 1];
+        }
+        forcedResponse[0] = dat;
+
+        float sum = 0;
+        // Sum of forced response coefficients multiplied by the forced response X(n-k)
+        // (previous input data)
+        for (int i = 0; i < SIZE; i++)
+        {
+            sum += forcedResponseCoefficients[i] * forcedResponse[i];
+        }
+        // Sum of natural response coefficients multiplied by the natural response Y(n-k)
+        // (previous output data)
+        for (int i = 0; i < SIZE - 1; i++)
+        {
+            sum -= naturalResponseCoefficients[i + 1] * naturalResponse[i];
+        }
+        // Apply the 1/a_0 scaling to the output
+        sum /= naturalResponseCoefficients[0];
+
+        // Shift the natural response array to make room for the new output
+        for (int i = SIZE - 1; i > 0; i--)
+        {
+            naturalResponse[i] = naturalResponse[i - 1];
+        }
+
+        naturalResponse[0] = sum;
+
+        return naturalResponse[0];
+    }
+
+    /** @brief Returns the last filtered value*/
+    T getLastFiltered() { return naturalResponse[0]; }
+
+    /** @brief Resets the filter's state to zero, keeps the coefficients  */
+
+    T reset()
+    {
+        // Reset the filter state to zero
+        naturalResponse.fill(0.0f);
+        forcedResponse.fill(0.0f);
+        return 0.0f;
+    }
+
+private:
+    std::array<T, SIZE> naturalResponseCoefficients;
+    std::array<T, SIZE> forcedResponseCoefficients;
+    std::array<T, SIZE> naturalResponse;
+    std::array<T, SIZE> forcedResponse;
+};
+
+}  // namespace algorithms
+
+}  // namespace tap
+
+#endif  // TAPROOT_DISCRETE_FILTER_HPP_
\ No newline at end of file
diff --git a/ut-robomaster/taproot/src/tap/algorithms/linear_interpolation_predictor_contiguous.hpp b/ut-robomaster/taproot/src/tap/algorithms/linear_interpolation_predictor_contiguous.hpp
deleted file mode 100644
index 5d69b46f..00000000
--- a/ut-robomaster/taproot/src/tap/algorithms/linear_interpolation_predictor_contiguous.hpp
+++ /dev/null
@@ -1,100 +0,0 @@
-/*****************************************************************************/
-/********** !!! WARNING: CODE GENERATED BY TAPROOT. DO NOT EDIT !!! **********/
-/*****************************************************************************/
-
-/*
- * Copyright (c) 2020-2021 Advanced Robotics at the University of Washington <robomstr@uw.edu>
- *
- * This file is part of Taproot.
- *
- * Taproot is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * Taproot is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with Taproot.  If not, see <https://www.gnu.org/licenses/>.
- */
-
-#ifndef TAPROOT_LINEAR_INTERPOLATION_PREDICTOR_CONTIGUOUS_HPP_
-#define TAPROOT_LINEAR_INTERPOLATION_PREDICTOR_CONTIGUOUS_HPP_
-
-#include <cstdint>
-
-#include "contiguous_float.hpp"
-
-namespace tap::algorithms
-{
-/**
- * An object that is similar in every respect to the `LinearInterpolationPredictor`
- * object except that it uses `ContiguousFloat`'s instead.
- */
-class LinearInterpolationPredictorContiguous
-{
-public:
-    /**
-     * @param[in] lowerBound Lower bound for linear interpolation contiguous float.
-     * @param[in] upperBound Upper bound for linear interpolation contiguous float.
-     */
-    LinearInterpolationPredictorContiguous(float lowerBound, float upperBound);
-
-    /**
-     * Updates the interpolation using the newValue.
-     *
-     * @note Only call this when you receive a new value (use remote rx
-     *      counter to tell when there is new data from the remote, for
-     *      example).
-     * @note This function should be called with increasing values of `currTime`.
-     * @param[in] newValue The new data used in the interpolation.
-     * @param[in] currTime The time that this function was called.
-     */
-    void update(float newValue, uint32_t currTime);
-
-    /**
-     * Returns the current value, that is: \f$y\f$ in the equation
-     * \f$y=slope\cdot (currTime - lastUpdateCallTime) + previousValue\f$
-     * in the units of whatever value you are inputting in the `update` function.
-     *
-     * @note Slope is defined by the previous two values passed into the `update`
-     *      function, a period preceeding `lastUpdateCallTime`.
-     * @note Use a millisecond-resolution timer, e.g.
-     *      `tap::arch::clock::getTimeMilliseconds()`.
-     * @param[in] currTime The current clock time, in ms.
-     * @return The interpolated value.
-     */
-    float getInterpolatedValue(uint32_t currTime)
-    {
-        return ContiguousFloat(
-                   slope * static_cast<float>(currTime - lastUpdateCallTime) +
-                       previousValue.getValue(),
-                   previousValue.getLowerBound(),
-                   previousValue.getUpperBound())
-            .getValue();
-    }
-
-    /**
-     * Resets the predictor. The slope will be reset to 0 and the initial values
-     * and time will be used to initialize the predictor.
-     *
-     * @note It is highly recommended that you call this function before calling
-     *      `update` to "initialize" the system.
-     *
-     * @param[in] initialValue The value to set the previous value to when resetting.
-     * @param[in] initialTime The value to set the previous time to when resetting.
-     */
-    void reset(float initialValue, uint32_t initialTime);
-
-private:
-    uint32_t lastUpdateCallTime;    ///< The previous timestamp from when update was called.
-    ContiguousFloat previousValue;  ///< The previous data value.
-    float slope;  ///< The current slope, calculated using the previous and most current data.
-};  // class LinearInterpolationPredictorContiguous
-
-}  // namespace tap::algorithms
-
-#endif  // TAPROOT_LINEAR_INTERPOLATION_PREDICTOR_CONTIGUOUS_HPP_
diff --git a/ut-robomaster/taproot/src/tap/algorithms/odometry/odometry_2d_interface.hpp b/ut-robomaster/taproot/src/tap/algorithms/odometry/odometry_2d_interface.hpp
index 71332bdd..38c2f210 100644
--- a/ut-robomaster/taproot/src/tap/algorithms/odometry/odometry_2d_interface.hpp
+++ b/ut-robomaster/taproot/src/tap/algorithms/odometry/odometry_2d_interface.hpp
@@ -62,6 +62,8 @@ class Odometry2DInterface
      * @return The last time that odometry was computed (in microseconds).
      */
     virtual uint32_t getLastComputedOdometryTime() const = 0;
+
+    virtual void overrideOdometryPosition(const float positionX, const float positionY) = 0;
 };
 
 }  // namespace tap::algorithms::odometry
diff --git a/ut-robomaster/taproot/src/tap/algorithms/odometry/odometry_2d_tracker.cpp b/ut-robomaster/taproot/src/tap/algorithms/odometry/odometry_2d_tracker.cpp
index 372e1f23..868fe2ca 100644
--- a/ut-robomaster/taproot/src/tap/algorithms/odometry/odometry_2d_tracker.cpp
+++ b/ut-robomaster/taproot/src/tap/algorithms/odometry/odometry_2d_tracker.cpp
@@ -24,6 +24,7 @@
 
 #include <cmath>
 
+#include "tap/architecture/clock.hpp"
 #include "tap/control/chassis/chassis_subsystem_interface.hpp"
 
 #include "modm/math/geometry/angle.hpp"
@@ -80,4 +81,9 @@ void Odometry2DTracker::update()
     }
 }
 
+void Odometry2DTracker::overrideOdometryPosition(const float positionX, const float positionY)
+{
+    location.setPosition(positionX, positionY);
+}
+
 }  // namespace tap::algorithms::odometry
diff --git a/ut-robomaster/taproot/src/tap/algorithms/odometry/odometry_2d_tracker.hpp b/ut-robomaster/taproot/src/tap/algorithms/odometry/odometry_2d_tracker.hpp
index d4bcd2dd..f2309507 100644
--- a/ut-robomaster/taproot/src/tap/algorithms/odometry/odometry_2d_tracker.hpp
+++ b/ut-robomaster/taproot/src/tap/algorithms/odometry/odometry_2d_tracker.hpp
@@ -79,6 +79,8 @@ class Odometry2DTracker : public Odometry2DInterface
 
     inline uint32_t getLastComputedOdometryTime() const final { return lastComputedOdometryTime; }
 
+    void overrideOdometryPosition(const float positionX, const float positionY);
+
 private:
     ChassisWorldYawObserverInterface* chassisYawObserver;
     ChassisDisplacementObserverInterface* chassisDisplacementObserver;
diff --git a/ut-robomaster/taproot/src/tap/algorithms/transforms/angular_velocity.hpp b/ut-robomaster/taproot/src/tap/algorithms/transforms/angular_velocity.hpp
new file mode 100644
index 00000000..cc9db3d3
--- /dev/null
+++ b/ut-robomaster/taproot/src/tap/algorithms/transforms/angular_velocity.hpp
@@ -0,0 +1,83 @@
+/*****************************************************************************/
+/********** !!! WARNING: CODE GENERATED BY TAPROOT. DO NOT EDIT !!! **********/
+/*****************************************************************************/
+
+/*
+ * Copyright (c) 2025-2025 Advanced Robotics at the University of Washington <robomstr@uw.edu>
+ *
+ * This file is part of Taproot.
+ *
+ * Taproot is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Taproot is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Taproot.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+#ifndef TAPROOT_ANGULAR_VELOCITY_HPP_
+#define TAPROOT_ANGULAR_VELOCITY_HPP_
+
+#include "tap/algorithms/cmsis_mat.hpp"
+
+namespace tap::algorithms::transforms
+{
+class AngularVelocity
+{
+public:
+    inline AngularVelocity(const float rollVel, const float pitchVel, const float yawVel)
+        : matrix_(skewMatFromAngVel(rollVel, pitchVel, yawVel))
+    {
+    }
+
+    /* rvalue reference */
+    inline AngularVelocity(AngularVelocity&& other) : matrix_(std::move(other.matrix_)) {}
+
+    /* Costly; use rvalue reference whenever possible */
+    inline AngularVelocity(AngularVelocity& other) : matrix_(CMSISMat(other.matrix_)) {}
+
+    /* Costly; use rvalue reference whenever possible */
+    inline AngularVelocity(const CMSISMat<3, 3>& matrix) : matrix_(matrix) {}
+
+    inline AngularVelocity(CMSISMat<3, 3>&& matrix) : matrix_(std::move(matrix)) {}
+
+    /**
+     * @brief Get the roll velocity
+     */
+    inline float getRollVelocity() const { return matrix_.data[0 * 3 + 2]; }
+
+    /**
+     * @brief Get the pitch velocity
+     */
+    inline float getPitchVelocity() const { return -matrix_.data[1 * 3 + 2]; }
+
+    /**
+     * @brief Get the yaw velocity
+     */
+    inline float getYawVelocity() const { return -matrix_.data[0 * 3 + 1]; }
+
+    const inline CMSISMat<3, 3>& matrix() const { return matrix_; }
+
+    /**
+     * Generates a 3x3 skew matrix from euler angle velocities (in radians/sec)
+     */
+    inline static CMSISMat<3, 3> skewMatFromAngVel(const float wx, const float wy, const float wz)
+    {
+        return tap::algorithms::CMSISMat<3, 3>({0, -wz, wy, wz, 0, -wx, -wy, wx, 0});
+    }
+
+    friend class Transform;
+    friend class DynamicOrientation;
+
+private:
+    CMSISMat<3, 3> matrix_;
+};  // class AngularVelocity
+}  // namespace tap::algorithms::transforms
+
+#endif  // TAPROOT_ANGULAR_VELOCITY_HPP_
diff --git a/ut-robomaster/taproot/src/tap/algorithms/transforms/dynamic_orientation.hpp b/ut-robomaster/taproot/src/tap/algorithms/transforms/dynamic_orientation.hpp
new file mode 100644
index 00000000..3b6f9e6f
--- /dev/null
+++ b/ut-robomaster/taproot/src/tap/algorithms/transforms/dynamic_orientation.hpp
@@ -0,0 +1,133 @@
+/*****************************************************************************/
+/********** !!! WARNING: CODE GENERATED BY TAPROOT. DO NOT EDIT !!! **********/
+/*****************************************************************************/
+
+/*
+ * Copyright (c) 2025-2025 Advanced Robotics at the University of Washington <robomstr@uw.edu>
+ *
+ * This file is part of Taproot.
+ *
+ * Taproot is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Taproot is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Taproot.  If not, see <https://www.gnu.org/licenses/>.
+ */
+#ifndef TAPROOT_DYNAMIC_ORIENTATION_HPP_
+#define TAPROOT_DYNAMIC_ORIENTATION_HPP_
+
+#include "tap/algorithms/cmsis_mat.hpp"
+
+#include "angular_velocity.hpp"
+#include "orientation.hpp"
+
+namespace tap::algorithms::transforms
+{
+class DynamicOrientation
+{
+public:
+    inline DynamicOrientation(
+        const float roll,
+        const float pitch,
+        const float yaw,
+        const float rollVel,
+        const float pitchVel,
+        const float yawVel)
+        : orientation(Orientation::fromEulerAngles(roll, pitch, yaw)),
+          angularVelocity(AngularVelocity::skewMatFromAngVel(rollVel, pitchVel, yawVel))
+    {
+    }
+
+    inline DynamicOrientation(Orientation&& orientation, AngularVelocity&& angularVelocity)
+        : orientation(std::move(orientation.matrix_)),
+          angularVelocity(std::move(angularVelocity.matrix_))
+    {
+    }
+
+    inline DynamicOrientation(Orientation& orientation, AngularVelocity& angularVelocity)
+        : orientation(orientation.matrix_),
+          angularVelocity(angularVelocity.matrix_)
+    {
+    }
+
+    inline DynamicOrientation(
+        const CMSISMat<3, 3>&& orientation,
+        const CMSISMat<3, 3>&& angularVelocity)
+        : orientation(std::move(orientation)),
+          angularVelocity(std::move(angularVelocity))
+    {
+    }
+
+    /* Costly; use rvalue reference whenever possible */
+    inline DynamicOrientation(
+        const CMSISMat<3, 3>& orientation,
+        const CMSISMat<3, 3>& angularVelocity)
+        : orientation(orientation),
+          angularVelocity(angularVelocity)
+    {
+    }
+
+    DynamicOrientation compose(const DynamicOrientation& other) const
+    {
+        return DynamicOrientation(
+            this->orientation * other.orientation,
+            this->angularVelocity +
+                this->orientation * other.angularVelocity * this->orientation.transpose());
+    }
+
+    DynamicOrientation inverse() const
+    {
+        return DynamicOrientation(
+            this->orientation.transpose(),
+            -(this->orientation.transpose() * this->angularVelocity * this->orientation));
+    }
+
+    inline Orientation getOrientation() const { return Orientation(orientation); }
+
+    inline AngularVelocity getAngularVelocity() const { return AngularVelocity(angularVelocity); }
+
+    /**
+     * Returns roll as values between [-pi, +pi].
+     *
+     * If pitch is completely vertical (-pi / 2 or pi / 2) then roll and yaw are gimbal-locked. In
+     * this case, roll is taken to be 0.
+     */
+    inline float roll() const { return atan2(orientation.data[7], orientation.data[8]); }
+
+    inline float pitch() const { return asinf(-orientation.data[6]); }
+
+    inline float yaw() const { return atan2(orientation.data[3], orientation.data[0]); }
+
+    /**
+     * @brief Get the roll velocity
+     */
+    inline float getRollVelocity() const { return angularVelocity.data[0 * 3 + 2]; }
+
+    /**
+     * @brief Get the pitch velocity
+     */
+    inline float getPitchVelocity() const { return -angularVelocity.data[1 * 3 + 2]; }
+
+    /**
+     * @brief Get the yaw velocity
+     */
+    inline float getYawVelocity() const { return -angularVelocity.data[0 * 3 + 1]; }
+
+    friend class Transform;
+
+private:
+    CMSISMat<3, 3> orientation;
+
+    CMSISMat<3, 3> angularVelocity;
+
+};  // class DynamicOrientation
+}  // namespace tap::algorithms::transforms
+
+#endif  // TAPROOT_DYNAMIC_ORIENTATION_HPP_
diff --git a/ut-robomaster/taproot/src/tap/algorithms/transforms/dynamic_position.hpp b/ut-robomaster/taproot/src/tap/algorithms/transforms/dynamic_position.hpp
new file mode 100644
index 00000000..5f564aca
--- /dev/null
+++ b/ut-robomaster/taproot/src/tap/algorithms/transforms/dynamic_position.hpp
@@ -0,0 +1,131 @@
+/*****************************************************************************/
+/********** !!! WARNING: CODE GENERATED BY TAPROOT. DO NOT EDIT !!! **********/
+/*****************************************************************************/
+
+/*
+ * Copyright (c) 2025-2025 Advanced Robotics at the University of Washington <robomstr@uw.edu>
+ *
+ * This file is part of Taproot.
+ *
+ * Taproot is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Taproot is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Taproot.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+#ifndef TAPROOT_DYNAMIC_POSITION_HPP_
+#define TAPROOT_DYNAMIC_POSITION_HPP_
+
+#include "tap/algorithms/cmsis_mat.hpp"
+
+#include "position.hpp"
+#include "vector.hpp"
+
+namespace tap::algorithms::transforms
+{
+class DynamicPosition
+{
+public:
+    inline DynamicPosition(
+        const float x,
+        const float y,
+        const float z,
+        const float vx,
+        const float vy,
+        const float vz,
+        const float ax,
+        const float ay,
+        const float az)
+        : position({x, y, z}),
+          velocity({vx, vy, vz}),
+          acceleration({ax, ay, az})
+    {
+    }
+
+    /* Costly; use rvalue reference whenever possible */
+    inline DynamicPosition(
+        const CMSISMat<3, 1>& position,
+        const CMSISMat<3, 1>& velocity,
+        const CMSISMat<3, 1>& acceleration)
+        : position(position),
+          velocity(velocity),
+          acceleration(acceleration)
+    {
+    }
+
+    inline DynamicPosition(
+        CMSISMat<3, 1>&& position,
+        CMSISMat<3, 1>&& velocity,
+        CMSISMat<3, 1>&& acceleration)
+        : position(std::move(position)),
+          velocity(std::move(velocity)),
+          acceleration(std::move(acceleration))
+    {
+    }
+
+    DynamicPosition operator+(const DynamicPosition& other) const
+    {
+        return DynamicPosition(
+            this->position + other.position,
+            this->velocity + other.velocity,
+            this->acceleration + other.acceleration);
+    }
+
+    DynamicPosition operator-(const DynamicPosition& other) const
+    {
+        return DynamicPosition(
+            this->position - other.position,
+            this->velocity - other.velocity,
+            this->acceleration - other.acceleration);
+    }
+
+    DynamicPosition operator-() const
+    {
+        return DynamicPosition(-this->position, -this->velocity, -this->acceleration);
+    }
+
+    inline Position getPosition() const { return Position(position); }
+
+    inline Vector getVelocity() const { return Vector(velocity); }
+
+    inline Vector getAcceleration() const { return Vector(acceleration); }
+
+    inline float x() const { return position.data[0]; }
+
+    inline float y() const { return position.data[1]; }
+
+    inline float z() const { return position.data[2]; }
+
+    inline float vx() const { return velocity.data[0]; }
+
+    inline float vy() const { return velocity.data[1]; }
+
+    inline float vz() const { return velocity.data[2]; }
+
+    inline float ax() const { return acceleration.data[0]; }
+
+    inline float ay() const { return acceleration.data[1]; }
+
+    inline float az() const { return acceleration.data[2]; }
+
+    friend class Transform;
+
+private:
+    CMSISMat<3, 1> position;
+
+    CMSISMat<3, 1> velocity;
+
+    CMSISMat<3, 1> acceleration;
+
+};  // class DynamicPosition
+}  // namespace tap::algorithms::transforms
+
+#endif  // TAPROOT_DYNAMIC_POSITION_HPP_
diff --git a/ut-robomaster/taproot/src/tap/algorithms/transforms/orientation.hpp b/ut-robomaster/taproot/src/tap/algorithms/transforms/orientation.hpp
index 61e3b412..bbe6cae5 100644
--- a/ut-robomaster/taproot/src/tap/algorithms/transforms/orientation.hpp
+++ b/ut-robomaster/taproot/src/tap/algorithms/transforms/orientation.hpp
@@ -26,7 +26,6 @@
 
 #include "tap/algorithms/cmsis_mat.hpp"
 #include "tap/algorithms/math_user_utils.hpp"
-// #include "tap/algorithms/euler_angles.hpp"
 
 namespace tap::algorithms::transforms
 {
@@ -49,6 +48,11 @@ class Orientation
 
     inline Orientation(CMSISMat<3, 3>&& matrix) : matrix_(std::move(matrix)) {}
 
+    inline Orientation compose(const Orientation& other) const
+    {
+        return Orientation(this->matrix_ * other.matrix_);
+    }
+
     /**
      * Returns roll as values between [-pi, +pi].
      *
@@ -63,7 +67,25 @@ class Orientation
 
     const inline CMSISMat<3, 3>& matrix() const { return matrix_; }
 
+    /**
+     * Generates a 3x3 rotation matrix from euler angles (in radians)
+     */
+    static CMSISMat<3, 3> fromEulerAngles(const float roll, const float pitch, const float yaw)
+    {
+        return tap::algorithms::CMSISMat<3, 3>(
+            {cosf(yaw) * cosf(pitch),
+             (cosf(yaw) * sinf(pitch) * sinf(roll)) - (sinf(yaw) * cosf(roll)),
+             (cosf(yaw) * sinf(pitch) * cosf(roll)) + sinf(yaw) * sinf(roll),
+             sinf(yaw) * cosf(pitch),
+             sinf(yaw) * sinf(pitch) * sinf(roll) + cosf(yaw) * cosf(roll),
+             sinf(yaw) * sinf(pitch) * cosf(roll) - cosf(yaw) * sinf(roll),
+             -sinf(pitch),
+             cosf(pitch) * sinf(roll),
+             cosf(pitch) * cosf(roll)});
+    }
+
     friend class Transform;
+    friend class DynamicOrientation;
 
 private:
     CMSISMat<3, 3> matrix_;
diff --git a/ut-robomaster/taproot/src/tap/algorithms/transforms/position.hpp b/ut-robomaster/taproot/src/tap/algorithms/transforms/position.hpp
index 227a8fa9..4fb495d6 100644
--- a/ut-robomaster/taproot/src/tap/algorithms/transforms/position.hpp
+++ b/ut-robomaster/taproot/src/tap/algorithms/transforms/position.hpp
@@ -78,6 +78,9 @@ class Position
 
     static float distance(const Position& a, const Position& b);
 
+    friend class Transform;
+    friend class DynamicPosition;
+
 private:
     CMSISMat<3, 1> coordinates_;
 };  // class Position
diff --git a/ut-robomaster/taproot/src/tap/algorithms/transforms/transform.cpp b/ut-robomaster/taproot/src/tap/algorithms/transforms/transform.cpp
index 73ba68c3..3f810e66 100644
--- a/ut-robomaster/taproot/src/tap/algorithms/transforms/transform.cpp
+++ b/ut-robomaster/taproot/src/tap/algorithms/transforms/transform.cpp
@@ -3,7 +3,7 @@
 /*****************************************************************************/
 
 /*
- * Copyright (c) 2020-2021 Advanced Robotics at the University of Washington <robomstr@uw.edu>
+ * Copyright (c) 2022-2024 Advanced Robotics at the University of Washington <robomstr@uw.edu>
  *
  * This file is part of Taproot.
  *
@@ -26,70 +26,312 @@
 namespace tap::algorithms::transforms
 {
 Transform::Transform(const Position& translation, const Orientation& rotation)
-    : translation(translation.coordinates()),
+    : dynamic(false),
+      translation(translation.coordinates()),
+      transVel({0, 0, 0}),
+      transAcc({0, 0, 0}),
       rotation(rotation.matrix()),
-      tRotation(rotation.matrix().transpose())
+      tRotation(rotation.matrix().transpose()),
+      angVel({0, 0, 0, 0, 0, 0, 0, 0, 0})
 {
 }
 
 Transform::Transform(Position&& translation, Orientation&& rotation)
-    : translation(std::move(translation.coordinates())),
+    : dynamic(false),
+      translation(std::move(translation.coordinates())),
+      transVel({0, 0, 0}),
+      transAcc({0, 0, 0}),
       rotation(std::move(rotation.matrix())),
-      tRotation(rotation.matrix().transpose())
+      tRotation(rotation.matrix().transpose()),
+      angVel({0, 0, 0, 0, 0, 0, 0, 0, 0})
 {
 }
 
 Transform::Transform(const CMSISMat<3, 1>& translation, const CMSISMat<3, 3>& rotation)
-    : translation(translation),
+    : dynamic(false),
+      translation(translation),
+      transVel({0, 0, 0}),
+      transAcc({0, 0, 0}),
       rotation(rotation),
-      tRotation(rotation.transpose())
+      tRotation(rotation.transpose()),
+      angVel({0, 0, 0, 0, 0, 0, 0, 0, 0})
 {
 }
 
 Transform::Transform(CMSISMat<3, 1>&& translation, CMSISMat<3, 3>&& rotation)
+    : dynamic(false),
+      translation(std::move(translation)),
+      transVel({0, 0, 0}),
+      transAcc({0, 0, 0}),
+      rotation(std::move(rotation)),
+      tRotation(rotation.transpose()),
+      angVel({0, 0, 0, 0, 0, 0, 0, 0, 0})
+{
+}
+
+Transform::Transform(float x, float y, float z, float rx, float ry, float rz)
+    : dynamic(false),
+      translation({x, y, z}),
+      transVel({0, 0, 0}),
+      transAcc({0, 0, 0}),
+      rotation(Orientation::fromEulerAngles(rx, ry, rz)),
+      tRotation(rotation.transpose()),
+      angVel({0, 0, 0, 0, 0, 0, 0, 0, 0})
+{
+}
+
+Transform::Transform(
+    const Position& translation,
+    const Orientation& rotation,
+    const Vector& velocity,
+    const Vector& acceleration,
+    const Vector& angularVelocity)
+    : translation(translation.coordinates()),
+      transVel(velocity.coordinates()),
+      transAcc(acceleration.coordinates()),
+      rotation(rotation.matrix()),
+      tRotation(rotation.matrix().transpose()),
+      angVel(AngularVelocity::skewMatFromAngVel(
+          angularVelocity.x(),
+          angularVelocity.y(),
+          angularVelocity.z()))
+{
+    checkDynamic();
+}
+
+Transform::Transform(
+    Position&& translation,
+    Orientation&& rotation,
+    Vector&& velocity,
+    Vector&& acceleration,
+    Vector&& angularVelocity)
+    : translation(std::move(translation.coordinates())),
+      transVel(std::move(velocity.coordinates())),
+      transAcc(std::move(acceleration.coordinates())),
+      rotation(std::move(rotation.matrix())),
+      tRotation(rotation.matrix().transpose()),
+      angVel(AngularVelocity::skewMatFromAngVel(
+          angularVelocity.x(),
+          angularVelocity.y(),
+          angularVelocity.z()))
+{
+    checkDynamic();
+}
+
+Transform::Transform(
+    const DynamicPosition& dynamicPosition,
+    const DynamicOrientation& dynamicOrientation)
+    : dynamic(true),
+      translation(dynamicPosition.position),
+      transVel(dynamicPosition.velocity),
+      transAcc(dynamicPosition.acceleration),
+      rotation(dynamicOrientation.orientation),
+      tRotation(rotation.transpose()),
+      angVel(dynamicOrientation.angularVelocity)
+{
+    checkDynamic();
+}
+
+Transform::Transform(DynamicPosition&& dynamicPosition, DynamicOrientation&& dynamicOrientation)
+    : dynamic(true),
+      translation(std::move(dynamicPosition.position)),
+      transVel(std::move(dynamicPosition.velocity)),
+      transAcc(std::move(dynamicPosition.acceleration)),
+      rotation(std::move(dynamicOrientation.orientation)),
+      tRotation(rotation.transpose()),
+      angVel(std::move(dynamicOrientation.angularVelocity))
+{
+    checkDynamic();
+}
+
+Transform::Transform(
+    const CMSISMat<3, 1>& translation,
+    const CMSISMat<3, 3>& rotation,
+    const CMSISMat<3, 1>& velocity,
+    const CMSISMat<3, 1>& acceleration,
+    const CMSISMat<3, 3>& angularVelocity)
+    : translation(translation),
+      transVel(velocity),
+      transAcc(acceleration),
+      rotation(rotation),
+      tRotation(rotation.transpose()),
+      angVel(angularVelocity)
+{
+    checkDynamic();
+}
+
+Transform::Transform(
+    CMSISMat<3, 1>&& translation,
+    CMSISMat<3, 3>&& rotation,
+    CMSISMat<3, 1>&& velocity,
+    CMSISMat<3, 1>&& acceleration,
+    CMSISMat<3, 3>&& angularVelocity)
     : translation(std::move(translation)),
+      transVel(std::move(velocity)),
+      transAcc(std::move(acceleration)),
       rotation(std::move(rotation)),
-      tRotation(rotation.transpose())
+      tRotation(rotation.transpose()),
+      angVel(std::move(angularVelocity))
 {
+    checkDynamic();
 }
 
-Transform::Transform(float x, float y, float z, float roll, float pitch, float yaw)
+Transform::Transform(
+    float x,
+    float y,
+    float z,
+    float vx,
+    float vy,
+    float vz,
+    float ax,
+    float ay,
+    float az,
+    float rx,
+    float ry,
+    float rz,
+    float wx,
+    float wy,
+    float wz)
     : translation({x, y, z}),
-      rotation(fromEulerAngles(roll, pitch, yaw)),
-      tRotation(rotation.transpose())
+      transVel({vx, vy, vz}),
+      transAcc({ax, ay, az}),
+      rotation(Orientation::fromEulerAngles(rx, ry, rz)),
+      tRotation(rotation.transpose()),
+      angVel(AngularVelocity::skewMatFromAngVel(wx, wy, wz))
 {
+    checkDynamic();
 }
 
 Position Transform::apply(const Position& position) const
 {
-    return Position(tRotation * (position.coordinates() - translation));
+    return Position(tRotation * (position.coordinates_ - translation));
 }
 
 Vector Transform::apply(const Vector& vector) const
 {
-    return Vector(tRotation * vector.coordinates());
+    return Vector(tRotation * vector.coordinates_);
+}
+
+DynamicPosition Transform::apply(const DynamicPosition& p) const
+{
+    CMSISMat<3, 1> pf = tRotation * (p.position - translation);
+    CMSISMat<3, 1> vf = tRotation * (p.velocity - transVel + angVel * (translation - p.position));
+    CMSISMat<3, 1> af =
+        tRotation * (p.acceleration - transAcc +
+                     angVel * (2 * (transVel - p.velocity) + angVel * (p.position - translation)));
+    return DynamicPosition(pf, vf, af);
 }
 
 Orientation Transform::apply(const Orientation& orientation) const
 {
-    return Orientation(tRotation * orientation.matrix());
+    return Orientation(tRotation * orientation.matrix_);
+}
+
+DynamicOrientation Transform::apply(const DynamicOrientation& dynamicOrientation) const
+{
+    return DynamicOrientation(
+        tRotation * dynamicOrientation.orientation,
+        tRotation * (dynamicOrientation.angularVelocity - angVel) * rotation);
 }
 
 Transform Transform::getInverse() const
 {
     // negative transposed rotation matrix times original position = new position
-    CMSISMat<3, 1> invTranslation = tRotation * translation;
-    invTranslation = -invTranslation;
-    return Transform(invTranslation, tRotation);
+    CMSISMat<3, 1> invTranslation = -tRotation * translation;
+    if (dynamic)
+    {
+        CMSISMat<3, 1> invVel = tRotation * (angVel * translation - transVel);
+        CMSISMat<3, 1> invAcc =
+            tRotation * (angVel * (2 * transVel - angVel * translation) - transAcc);
+        CMSISMat<3, 3> invAngVel = -tRotation * angVel * rotation;
+        return Transform(invTranslation, tRotation, invVel, invAcc, invAngVel);
+    }
+    else
+    {
+        return Transform(invTranslation, tRotation);
+    }
 }
 
 Transform Transform::compose(const Transform& second) const
+{
+    if (this->dynamic && second.dynamic)
+    {
+        CMSISMat<3, 3> newRot = this->rotation * second.rotation;
+        CMSISMat<3, 1> newPos = this->translation + this->rotation * second.translation;
+        CMSISMat<3, 1> newVel = this->transVel +
+                                this->angVel * this->rotation * second.translation +
+                                this->rotation * second.transVel;
+        CMSISMat<3, 1> newAcc =
+            this->transAcc + this->angVel * this->angVel * this->rotation * second.translation +
+            2 * this->angVel * this->rotation * second.transVel + this->rotation * second.transAcc;
+        CMSISMat<3, 3> newAngVel = this->angVel + this->rotation * second.angVel * this->tRotation;
+        return Transform(newPos, newRot, newVel, newAcc, newAngVel);
+    }
+    else if (this->dynamic)
+    {
+        CMSISMat<3, 3> newRot = this->rotation * second.rotation;
+        CMSISMat<3, 1> newPos = this->translation + this->rotation * second.translation;
+        CMSISMat<3, 1> newVel = this->transVel + this->angVel * this->rotation * second.translation;
+        CMSISMat<3, 1> newAcc =
+            this->transAcc + this->angVel * this->angVel * this->rotation * second.translation;
+        CMSISMat<3, 3> newAngVel = this->angVel;
+        return Transform(newPos, newRot, newVel, newAcc, newAngVel);
+    }
+    else if (second.dynamic)
+    {
+        CMSISMat<3, 3> newRot = this->rotation * second.rotation;
+        CMSISMat<3, 1> newPos = this->translation + this->rotation * second.translation;
+        CMSISMat<3, 1> newVel = this->rotation * second.transVel;
+        CMSISMat<3, 1> newAcc = this->rotation * second.transAcc;
+        CMSISMat<3, 3> newAngVel = this->rotation * second.angVel * this->tRotation;
+        return Transform(newPos, newRot, newVel, newAcc, newAngVel);
+    }
+
+    return composeStatic(second);
+}
+
+Transform Transform::composeStatic(const Transform& second) const
 {
     CMSISMat<3, 3> newRot = this->rotation * second.rotation;
     CMSISMat<3, 1> newPos = this->translation + this->rotation * second.translation;
     return Transform(newPos, newRot);
 }
 
+Transform Transform::projectForward(float dt) const
+{
+    if (!dynamic)
+    {
+        return Transform(
+            this->translation,
+            this->rotation,
+            this->transVel,
+            this->transAcc,
+            this->angVel);
+    }
+
+    CMSISMat<3, 1> newPos =
+        this->translation + dt * this->transVel + 0.5f * dt * dt * this->transAcc;
+    CMSISMat<3, 1> newVel = this->transVel + dt * this->transAcc;
+
+    float angVelMag = sqrt(
+        getRollVelocity() * getRollVelocity() + getPitchVelocity() * getPitchVelocity() +
+        getYawVelocity() * getYawVelocity());
+
+    if (compareFloatClose(angVelMag, 0, 1e-3))
+    {
+        return Transform(newPos, this->rotation, newVel, this->transAcc, this->angVel);
+    }
+
+    float theta = dt * angVelMag;
+    CMSISMat<3, 3> angVelNormalized = angVel / angVelMag;
+    CMSISMat<3, 3> velDt = CMSISMat<3, 3>();
+    velDt.constructIdentityMatrix();
+    velDt = velDt + sin(theta) * angVelNormalized +
+            (1 - cos(theta)) * angVelNormalized * angVelNormalized;
+    CMSISMat<3, 3> newRot = velDt * this->rotation;
+    return Transform(newPos, newRot, newVel, this->transAcc, this->angVel);
+}
+
 float Transform::getRoll() const
 {
     float jz = rotation.data[2 * 3 + 1];
@@ -97,12 +339,16 @@ float Transform::getRoll() const
     return atan2(jz, kz);
 }
 
+float Transform::getRollVelocity() const { return angVel.data[0 * 3 + 2]; }
+
 float Transform::getPitch() const
 {
     float iz = rotation.data[2 * 3 + 0];
     return asinf(-iz);
 }
 
+float Transform::getPitchVelocity() const { return -angVel.data[1 * 3 + 2]; }
+
 float Transform::getYaw() const
 {
     float iy = rotation.data[1 * 3 + 0];
@@ -110,4 +356,6 @@ float Transform::getYaw() const
     return atan2(iy, ix);
 }
 
+float Transform::getYawVelocity() const { return -angVel.data[0 * 3 + 1]; }
+
 }  // namespace tap::algorithms::transforms
diff --git a/ut-robomaster/taproot/src/tap/algorithms/transforms/transform.hpp b/ut-robomaster/taproot/src/tap/algorithms/transforms/transform.hpp
index 6910bb75..237913a3 100644
--- a/ut-robomaster/taproot/src/tap/algorithms/transforms/transform.hpp
+++ b/ut-robomaster/taproot/src/tap/algorithms/transforms/transform.hpp
@@ -3,7 +3,7 @@
 /*****************************************************************************/
 
 /*
- * Copyright (c) 2022-2023 Advanced Robotics at the University of Washington <robomstr@uw.edu>
+ * Copyright (c) 2022-2024 Advanced Robotics at the University of Washington <robomstr@uw.edu>
  *
  * This file is part of Taproot.
  *
@@ -20,11 +20,16 @@
  * You should have received a copy of the GNU General Public License
  * along with Taproot.  If not, see <https://www.gnu.org/licenses/>.
  */
+
 #ifndef TAPROOT_TRANSFORM_HPP_
 #define TAPROOT_TRANSFORM_HPP_
 
+#include "tap/algorithms/cmsis_mat.hpp"
 #include "tap/algorithms/math_user_utils.hpp"
 
+#include "angular_velocity.hpp"
+#include "dynamic_orientation.hpp"
+#include "dynamic_position.hpp"
 #include "orientation.hpp"
 #include "position.hpp"
 #include "vector.hpp"
@@ -32,24 +37,24 @@
 namespace tap::algorithms::transforms
 {
 /**
- Represents a transformation from one coordinate frame to another.
-
-    A Transform from frame A to frame B defines a relationship between the two frames, such that a
-    spatial measurement in frame A can be represented equivalently in frame B by applying a
-    translational and rotational offset. This process is known as *applying* a transform.
-
-    Transforms are specified as a translation and rotation of some "target" frame relative to some
-    "source" frame. The "translation" is the target frame's origin in source frame, and the
-    "rotation" is the target frame's orientation relative to the source frame's orientation.
-
-    Conceptually, translations are applied "before" rotations. This means that the origin of the
-    target frame is entirely defined by the translation in the source frame, and the rotation serves
-    only to change the orientation of the target frame's axes relative to the source frame.
-
-    Utilizes arm's CMSIS matrix operations.
-
-    @param SOURCE represents the source frame of the transformation.
-    @param TARGET represents the target frame of the transformation.
+ * Represents a transformation from one coordinate frame to another.
+ * A Static Transform from frame A to frame B defines a relationship between the two frames, such
+ * that a spatial measurement in frame A can be represented equivalently in frame B by applying a
+ * translational and rotational offset. This process is known as *applying* a transform.
+ *
+ * Static Transforms are specified as a translation and rotation of some "follower" frame relative
+ * to some "base" frame. The "translation" is the follower frame's origin in base frame, and the
+ * "rotation" is the follower frame's orientation relative to the base frame's orientation.
+ *
+ * Conceptually, translations are applied "before" rotations. This means that the origin of the
+ * follower frame is entirely defined by the translation in the base frame, and the rotation serves
+ * only to change the orientation of the follower frame's axes relative to the base frame.
+ *
+ * A Dynamic Transform is an extension of a Static Transform that can store linear velocity, linear
+ * acceleration, and angular velocity. This class handles both, automatically determining whether it
+ * is static or dynamic.
+ *
+ * Utilizes ARM's CMSIS matrix operations.
  */
 class Transform
 {
@@ -70,67 +75,183 @@ class Transform
 
     /**
      * Constructs rotations using XYZ Euler angles,
-     * so rotations are applied in order of A, B, then C.
+     * so rotations are applied in order of rx, ry, then rz.
      * As an example, for an x-forward, z-up coordinate system,
      * this is in the order of roll, pitch, then yaw.
      *
      * @param x: Initial x-component of the translation.
      * @param y: Initial y-component of the translation.
      * @param z: Initial z-component of the translation.
-     * @param A: Initial rotation angle about the x-axis.
-     * @param B: Initial rotation angle about the y-axis.
-     * @param C: Initial rotation angle about the z-axis.
+     * @param rx: Initial rotation angle about the x-axis.
+     * @param ry: Initial rotation angle about the y-axis.
+     * @param rz: Initial rotation angle about the z-axis.
      */
-    Transform(float x, float y, float z, float roll, float pitch, float yaw);
+    Transform(float x, float y, float z, float rx, float ry, float rz);
 
-    // TODO: template specialization for transform between identical frames??
     /**
-     * Constructs an identity transform.
+     * @param translation Initial translation of this transformation.
+     * @param rotation Initial rotation of this transformation.
+     * @param velocity Translational velocity of this transformation.
+     * @param acceleration Translational acceleration of this transformation.
+     * @param angularVelocity Angular velocity pseudovector of this transformation.
+     */
+    Transform(
+        const Position& translation,
+        const Orientation& rotation,
+        const Vector& velocity,
+        const Vector& acceleration,
+        const Vector& angularVelocity);
+
+    /**
+     * @param translation Initial translation of this transformation.
+     * @param rotation Initial rotation of this transformation.
+     * @param velocity Translational velocity of this transformation.
+     * @param acceleration Translational acceleration of this transformation.
+     * @param angularVelocity Angular velocity pseudovector of this transformation.
+     */
+    Transform(
+        Position&& translation,
+        Orientation&& rotation,
+        Vector&& velocity,
+        Vector&& acceleration,
+        Vector&& angularVelocity);
+
+    /**
+     * @param rotation Initial rotation of this transformation.
+     * @param position Initial translation of this transformation.
+     */
+    Transform(const DynamicPosition& dynamicPosition, const DynamicOrientation& dynamicOrientation);
+    Transform(DynamicPosition&& dynamicPosition, DynamicOrientation&& dynamicOrientation);
+
+    /**
+     * @param translation Initial translation of this transformation.
+     * @param rotation Initial rotation of this transformation.
+     * @param velocity Translational velocity of this transformation.
+     * @param acceleration Translational acceleration of this transformation.
+     * @param angularVelocity Angular velocity skew symmetric matrix of this transformation.
+     */
+    Transform(
+        const CMSISMat<3, 1>& translation,
+        const CMSISMat<3, 3>& rotation,
+        const CMSISMat<3, 1>& velocity,
+        const CMSISMat<3, 1>& acceleration,
+        const CMSISMat<3, 3>& angularVelocity);
+
+    /**
+     * @param translation Initial translation of this transformation.
+     * @param rotation Initial rotation of this transformation.
+     * @param velocity Translational velocity of this transformation.
+     * @param acceleration Translational acceleration of this transformation.
+     * @param angularVelocity Angular velocity skew symmetric matrix of this transformation.
+     */
+    Transform(
+        CMSISMat<3, 1>&& translation,
+        CMSISMat<3, 3>&& rotation,
+        CMSISMat<3, 1>&& velocity,
+        CMSISMat<3, 1>&& acceleration,
+        CMSISMat<3, 3>&& angularVelocity);
+
+    /**
+     * Constructs rotations using XYZ Euler angles,
+     * so rotations are applied in order of rx, ry, then rx.
+     *
+     * @param x:  Initial x-component of the translation.
+     * @param y:  Initial y-component of the translation.
+     * @param z:  Initial z-component of the translation.
+     * @param vx: Initial x-component of the translational velocity.
+     * @param vy: Initial y-component of the translational velocity.
+     * @param vz: Initial z-component of the translational velocity.
+     * @param ax: Initial x-component of the translational acceleration.
+     * @param ay: Initial y-component of the translational acceleration.
+     * @param az: Initial z-component of the translational acceleration.
+     * @param rx: Initial rotation angle about the x-axis.
+     * @param ry: Initial rotation angle about the y-axis.
+     * @param rz: Initial rotation angle about the z-axis.
+     * @param wx: Initial angular velocity about the x-axis.
+     * @param wy: Initial angular velocity about the y-axis.
+     * @param wz: Initial angular velocity about the z-axis.
+     */
+    Transform(
+        float x,
+        float y,
+        float z,
+        float vx,
+        float vy,
+        float vz,
+        float ax,
+        float ay,
+        float az,
+        float rx,
+        float ry,
+        float rz,
+        float wx,
+        float wy,
+        float wz);
+
+    /**
+     * @brief Constructs an identity transform.
      */
     static inline Transform identity() { return Transform(0., 0., 0., 0., 0., 0.); }
 
     /**
-     * Apply this transform to a position.
+     * @brief Apply this transform to a position.
      *
-     * @param[in] position Position in source frame.
-     * @return Position in target frame.
+     * @param[in] position Position in base frame.
+     * @return Position in follower frame.
      */
     Position apply(const Position& position) const;
 
     /**
-     * Rotates a vector in the source frame to a vector in the target frame.
+     * @brief Rotates a vector in the base frame to a vector in the follower frame.
      *
      * Intended to be used for things like velocities and accelerations which represent the
      * difference between two positions in space, since both positions get translated the same way,
      * causing the translation to cancel out.
      *
-     * @param vector Vector as read by source frame.
-     * @return Vector in target frame's basis.
+     * @note Only accurate for static transforms!
+     *
+     * @param vector Vector as read by base frame.
+     * @return Vector in follower frame's basis.
      */
     Vector apply(const Vector& vector) const;
 
     /**
-     *
+     * @brief Brings a dynamic position in the base frame to one in the follower frame.
+     */
+    DynamicPosition apply(const DynamicPosition& dynamicPosition) const;
+
+    /**
+     * @brief Brings an orientation in the base frame to one in the follower frame.
      */
     Orientation apply(const Orientation& orientation) const;
 
     /**
-     * Updates the translation of the current transformation matrix.
+     * @brief Brings a dynamic orientation in the base frame to one in the follower frame.
+     */
+    DynamicOrientation apply(const DynamicOrientation& dynamicOrientation) const;
+
+    /**
+     * @brief Updates the translation of the current transformation matrix.
      *
-     * @param newTranslation updated position of target in source frame.
+     * @param newTranslation updated position of follower in base frame.
      */
     inline void updateTranslation(const Position& newTranslation)
     {
         this->translation = newTranslation.coordinates();
     }
 
+    /**
+     * @brief Updates the translation of the current transformation matrix.
+     *
+     * @param newTranslation updated position of follower in base frame.
+     */
     inline void updateTranslation(Position&& newTranslation)
     {
         this->translation = std::move(newTranslation.coordinates());
     }
 
     /**
-     * Updates the translation of the current transformation matrix.
+     * @brief Updates the translation of the current transformation matrix.
      *
      * @param x new translation x-component.
      * @param y new translation y-component.
@@ -142,9 +263,33 @@ class Transform
     }
 
     /**
-     * Updates the rotation of the current transformation matrix.
+     * @brief Updates the translation of the current transformation matrix.
+     *
+     * @param newTranslation updated position of follower in base frame.
+     */
+    inline void updateTranslation(const DynamicPosition& newTranslation)
+    {
+        this->translation = newTranslation.position;
+        this->transVel = newTranslation.velocity;
+        this->transAcc = newTranslation.acceleration;
+    }
+
+    /**
+     * @brief Updates the translation of the current transformation matrix.
      *
-     * @param newRotation updated orienation of target frame in source frame.
+     * @param newTranslation updated position of follower in base frame.
+     */
+    inline void updateTranslation(DynamicPosition&& newTranslation)
+    {
+        this->translation = std::move(newTranslation.position);
+        this->transVel = std::move(newTranslation.velocity);
+        this->transAcc = std::move(newTranslation.acceleration);
+    }
+
+    /**
+     * @brief Updates the rotation of the current transformation matrix.
+     *
+     * @param newRotation updated orientation of follower frame in base frame.
      */
     inline void updateRotation(const Orientation& newRotation)
     {
@@ -152,6 +297,11 @@ class Transform
         this->tRotation = this->rotation.transpose();
     }
 
+    /**
+     * @brief Updates the rotation of the current transformation matrix.
+     *
+     * @param newRotation updated orientation of follower frame in base frame.
+     */
     inline void updateRotation(Orientation&& newRotation)
     {
         this->rotation = std::move(newRotation.matrix());
@@ -159,7 +309,7 @@ class Transform
     }
 
     /**
-     * Updates the rotation of the current transformation matrix.
+     * @brief Updates the rotation of the current transformation matrix.
      * Takes rotation angles in the order of roll->pitch->yaw.
      *
      * @param roll updated rotation angle about the x-axis.
@@ -172,58 +322,303 @@ class Transform
         this->tRotation = this->rotation.transpose();
     }
 
+    /**
+     * @brief Updates the rotation of the current transformation matrix.
+     *
+     * @param newRotation updated orientation of follower frame in base frame.
+     */
+    inline void updateRotation(const DynamicOrientation& newRotation)
+    {
+        this->rotation = newRotation.orientation;
+        this->tRotation = this->rotation.transpose();
+        this->angVel = newRotation.angularVelocity;
+    }
+
+    /**
+     * @brief Updates the rotation of the current transformation matrix.
+     *
+     * @param newRotation updated orientation of follower frame in base frame.
+     */
+    inline void updateRotation(DynamicOrientation&& newRotation)
+    {
+        this->rotation = std::move(newRotation.orientation);
+        this->tRotation = this->rotation.transpose();
+        this->angVel = std::move(newRotation.angularVelocity);
+    }
+
+    /**
+     * @brief Updates the velocity of the current transform.
+     *
+     * @param newVelocity updated velocity of follower in base frame.
+     */
+    inline void updateVelocity(const Vector& newVelocity)
+    {
+        this->transVel = newVelocity.coordinates();
+        checkDynamic();
+    }
+
+    /**
+     * @brief Updates the velocity of the current transform.
+     *
+     * @param newVelocity updated velocity of follower in base frame.
+     */
+    inline void updateVelocity(Vector&& newVelocity)
+    {
+        this->transVel = std::move(newVelocity.coordinates());
+        checkDynamic();
+    }
+
+    /**
+     * @brief Updates the velocity of the current transform.
+     *
+     * @param vx new velocity x-component.
+     * @param vy new velocity y-component.
+     * @param vz new velocity z-component.
+     */
+    inline void updateVelocity(float vx, float vy, float vz)
+    {
+        this->transVel = CMSISMat<3, 1>({vx, vy, vz});
+        checkDynamic();
+    }
+
+    /**
+     * @brief Updates the acceleration of the current transform.
+     *
+     * @param updateAcceleration updated acceleration of follower in base frame.
+     */
+    inline void updateAcceleration(const Vector& newAcceleration)
+    {
+        this->transVel = newAcceleration.coordinates();
+        checkDynamic();
+    }
+
+    /**
+     * @brief Updates the acceleration of the current transform.
+     *
+     * @param updateAcceleration updated acceleration of follower in base frame.
+     */
+    inline void updateAcceleration(Vector&& newAcceleration)
+    {
+        this->transVel = std::move(newAcceleration.coordinates());
+        checkDynamic();
+    }
+
+    /**
+     * @brief Updates the acceleration of the current transform.
+     *
+     * @param ax new acceleration x-component.
+     * @param ay new acceleration y-component.
+     * @param az new acceleration z-component.
+     */
+    inline void updateAcceleration(float ax, float ay, float az)
+    {
+        this->transAcc = CMSISMat<3, 1>({ax, ay, az});
+        checkDynamic();
+    }
+
+    /**
+     * @brief Updates the angular velocity of the current transform.
+     *
+     * @param updateAngularVelocity updated angular velocity of follower in base frame.
+     */
+    inline void updateAngularVelocity(const Vector& newAngularVelocity)
+    {
+        this->angVel = AngularVelocity::skewMatFromAngVel(
+            newAngularVelocity.x(),
+            newAngularVelocity.y(),
+            newAngularVelocity.z());
+        checkDynamic();
+    }
+
+    /**
+     * @brief Updates the angular velocity of the current transform.
+     *
+     * @param updateAngularVelocity updated angular velocity of follower in base frame.
+     */
+    inline void updateAngularVelocity(Position&& newAngularVelocity)
+    {
+        this->angVel = AngularVelocity::skewMatFromAngVel(
+            newAngularVelocity.x(),
+            newAngularVelocity.y(),
+            newAngularVelocity.z());
+        checkDynamic();
+    }
+
+    /**
+     * @brief Updates the angular velocity of the current transform.
+     *
+     * @param ax new angular velocity x-component.
+     * @param ay new angular velocity y-component.
+     * @param az new angular velocity z-component.
+     */
+    inline void updateAngularVelocity(float vr, float vp, float vy)
+    {
+        this->angVel = AngularVelocity::skewMatFromAngVel(vr, vp, vy);
+        checkDynamic();
+    }
+
     /**
      * @return Inverse of this Transform.
+     *
+     * @note This is only instantaneously correct for dynamic transforms; It can no longer be
+     * projected forward in time and behave the same way as the original. This is due to the now
+     * reversed translation-rotation that would be required to truly mimic the motion of the
+     * original. Ex: Consider a dynamic transform with only non-zero translation and angular
+     * velocity. Projecting this forward will cause the follower frame to rotate around its origin.
+     * Intuitively, one would expect the inverted transform to have its follower frame rotate around
+     * the base frame origin. However, this circular translation can only be approximated here with
+     * translational velocity/acceleration. The true inverse would need to be the composition of a
+     * rotation *then* a translation.
      */
     Transform getInverse() const;
 
     /**
-     * Returns the composed transformation of the given transformations.
-     * @return Transformation from frame A to frame C.
+     * @brief Returns the composed transformation of the given transformations.
+     *
+     * @return Transformation from this transform's base frame to `second`'s follower frame.
      */
     Transform compose(const Transform& second) const;
 
+    /**
+     * @brief Returns the composed transformation of the given transformations, ignoring any time
+     * derivatives.
+     *
+     * @return Static transformation from this transform's base frame to `second`'s
+     * follower frame.
+     */
+    Transform composeStatic(const Transform& second) const;
+
+    /**
+     * @brief Projects this transform forward in time according to its translational
+     * velocity/acceleration and angular velocity.
+     *
+     * @param dt Seconds to project forward (can be negative)
+     * @return Projected transform
+     */
+    Transform projectForward(float dt) const;
+
     /* Getters */
     inline Position getTranslation() const { return Position(translation); };
 
+    inline Vector getVelocity() const { return Vector(transVel); };
+
+    inline Vector getAcceleration() const { return Vector(transAcc); };
+
+    inline DynamicPosition getDynamicTranslation() const
+    {
+        return DynamicPosition(translation, transVel, transAcc);
+    };
+
     inline Orientation getRotation() const { return Orientation(rotation); }
 
+    inline Vector getAngularVel() const
+    {
+        return Vector(getRollVelocity(), getPitchVelocity(), getYawVelocity());
+    }
+
+    inline DynamicOrientation getDynamicOrientation() const
+    {
+        return DynamicOrientation(rotation, angVel);
+    };
+
     /**
-     * Get the roll of this transformation
+     * @brief Get the roll of this transformation
      */
     float getRoll() const;
 
     /**
-     * Get the pitch of this transformation
+     * @brief Get the pitch of this transformation
      */
     float getPitch() const;
 
     /**
-     * Get the yaw of this transformation
+     * @brief Get the yaw of this transformation
      */
     float getYaw() const;
 
     /**
-     * Get the x-component of this transform's translation
+     * @brief Get the roll velocity of this transformation
+     */
+    float getRollVelocity() const;
+
+    /**
+     * @brief Get the pitch velocity of this transformation
+     */
+    float getPitchVelocity() const;
+
+    /**
+     * @brief Get the yaw velocity of this transformation
+     */
+    float getYawVelocity() const;
+
+    /**
+     * @brief Get the x-component of this transform's translation
      */
     inline float getX() const { return this->translation.data[0]; }
 
     /**
-     * Get the y-component of this transform's translation
+     * @brief Get the y-component of this transform's translation
      */
     inline float getY() const { return this->translation.data[1]; }
 
     /**
-     * Get the z-component of this transform's translation
+     * @brief Get the z-component of this transform's translation
      */
     inline float getZ() const { return this->translation.data[2]; }
 
+    /**
+     * @brief Get the x-component of this transform's linear velocity
+     */
+    inline float getXVel() const { return this->transVel.data[0]; }
+
+    /**
+     * @brief Get the y-component of this transform's linear velocity
+     */
+    inline float getYVel() const { return this->transVel.data[1]; }
+
+    /**
+     * @brief Get the z-component of this transform's linear velocity
+     */
+    inline float getZVel() const { return this->transVel.data[2]; }
+
+    /**
+     * @brief Get the x-component of this transform's linear acceleration
+     */
+    inline float getXAcc() const { return this->transAcc.data[0]; }
+
+    /**
+     * @brief Get the y-component of this transform's linear acceleration
+     */
+    inline float getYAcc() const { return this->transAcc.data[1]; }
+
+    /**
+     * @brief Get the z-component of this transform's linear acceleration
+     */
+    inline float getZAcc() const { return this->transAcc.data[2]; }
+
+    /**
+     * @brief Whether there are any non-zero derivatives.
+     */
+    inline bool isDynamic() const { return dynamic; }
+
 private:
+    bool dynamic{true};
+
     /**
      * Translation vector.
      */
     CMSISMat<3, 1> translation;
 
+    /**
+     * Translational velocity vector.
+     */
+    CMSISMat<3, 1> transVel;
+
+    /**
+     * Translational acceleration vector.
+     */
+    CMSISMat<3, 1> transAcc;
+
     /**
      * Rotation matrix.
      */
@@ -236,6 +631,29 @@ class Transform
      * The transpose of a rotation is its inverse.
      */
     CMSISMat<3, 3> tRotation;
+
+    /**
+     * Angular velocity skew matrix.
+     */
+    CMSISMat<3, 3> angVel;
+
+    inline void checkDynamic()
+    {
+        dynamic = false;
+
+        dynamic |=
+            !(compareFloatClose(getXVel(), 0, 1e-5) && compareFloatClose(getYVel(), 0, 1e-5) &&
+              compareFloatClose(getZVel(), 0, 1e-5));
+
+        dynamic |=
+            !(compareFloatClose(getXAcc(), 0, 1e-5) && compareFloatClose(getYAcc(), 0, 1e-5) &&
+              compareFloatClose(getZAcc(), 0, 1e-5));
+
+        dynamic |=
+            !(compareFloatClose(getRollVelocity(), 0, 1e-5) &&
+              compareFloatClose(getPitchVelocity(), 0, 1e-5) &&
+              compareFloatClose(getYawVelocity(), 0, 1e-5));
+    }
 };  // class Transform
 }  // namespace tap::algorithms::transforms
 
diff --git a/ut-robomaster/taproot/src/tap/algorithms/transforms/vector.cpp b/ut-robomaster/taproot/src/tap/algorithms/transforms/vector.cpp
index e411e753..6cf19c5f 100644
--- a/ut-robomaster/taproot/src/tap/algorithms/transforms/vector.cpp
+++ b/ut-robomaster/taproot/src/tap/algorithms/transforms/vector.cpp
@@ -31,10 +31,4 @@ inline Vector Vector::operator+(const Position& other) const
 {
     return Vector(this->coordinates_ + other.coordinates());
 }
-
-inline Vector Vector::operator+(const Vector& other) const
-{
-    return Vector(this->coordinates_ + other.coordinates_);
-}
-
 }  // namespace tap::algorithms::transforms
diff --git a/ut-robomaster/taproot/src/tap/algorithms/transforms/vector.hpp b/ut-robomaster/taproot/src/tap/algorithms/transforms/vector.hpp
index d4bb2df9..79720d1e 100644
--- a/ut-robomaster/taproot/src/tap/algorithms/transforms/vector.hpp
+++ b/ut-robomaster/taproot/src/tap/algorithms/transforms/vector.hpp
@@ -40,7 +40,10 @@ class Vector
 
     Vector(const Vector& other) : coordinates_(CMSISMat(other.coordinates_)) {}
 
-    Vector(CMSISMat<3, 1>& coordinates) : coordinates_(CMSISMat(coordinates)) {}
+    /**
+     * Costly copy constructor
+     */
+    Vector(const CMSISMat<3, 1>& coordinates) : coordinates_(CMSISMat(coordinates)) {}
 
     Vector(CMSISMat<3, 1>&& coordinates) : coordinates_(std::move(coordinates)) {}
 
@@ -56,10 +59,18 @@ class Vector
         return *this;
     }
 
-    inline Vector operator+(const Vector& other) const;
-
     inline Vector operator+(const Position& other) const;
 
+    inline Vector operator+(const Vector& other) const
+    {
+        return Vector(this->coordinates_ + other.coordinates_);
+    }
+
+    inline Vector operator-(const Vector& other) const
+    {
+        return Vector(this->coordinates_ - other.coordinates_);
+    }
+
     inline Vector operator*(const float scale) const { return Vector(this->coordinates_ * scale); }
 
     inline static float dot(const Vector& a, const Vector& b)
@@ -75,6 +86,9 @@ class Vector
 
     inline float magnitude() const { return sqrt(dot(*this, *this)); }
 
+    friend class Transform;
+    friend class DynamicPosition;
+
 private:
     CMSISMat<3, 1> coordinates_;
 };  // class Vector
diff --git a/ut-robomaster/taproot/src/tap/algorithms/wrapped_float.hpp b/ut-robomaster/taproot/src/tap/algorithms/wrapped_float.hpp
index 57b27049..aff4eb5b 100644
--- a/ut-robomaster/taproot/src/tap/algorithms/wrapped_float.hpp
+++ b/ut-robomaster/taproot/src/tap/algorithms/wrapped_float.hpp
@@ -53,7 +53,7 @@ class WrappedFloat
     /**
      * @param[in] value: value to initialize with (doesn't have to be wrapped)
      * @param[in] lowerBound: lower wrapping bound, must be less than `upperBound`
-     * @param[in] lowerBound: upper wrapping bound, must be higher than `lowerBound`
+     * @param[in] upperBound: upper wrapping bound, must be higher than `lowerBound`
      */
     WrappedFloat(float value, float lowerBound, float upperBound);
 
diff --git a/ut-robomaster/taproot/src/tap/communication/can/can_rx_handler.cpp b/ut-robomaster/taproot/src/tap/communication/can/can_rx_handler.cpp
index eb01492a..33579f2c 100644
--- a/ut-robomaster/taproot/src/tap/communication/can/can_rx_handler.cpp
+++ b/ut-robomaster/taproot/src/tap/communication/can/can_rx_handler.cpp
@@ -56,12 +56,30 @@ void CanRxHandler::attachReceiveHandler(
     CanRxListener* const canRxListener,
     CanRxListener** messageHandlerStore)
 {
-    uint16_t id = lookupTableIndexForCanId(canRxListener->canIdentifier);
+    uint16_t bin = binIndexForCanId(canRxListener->canIdentifier);
 
-    modm_assert(id < NUM_CAN_IDS, "CAN", "RX listener id out of bounds", 1);
-    modm_assert(messageHandlerStore[id] == nullptr, "CAN", "overloading", 1);
+    if (messageHandlerStore[bin] == nullptr)
+    {
+        messageHandlerStore[bin] = canRxListener;
+    }
+    else
+    {
+        CanRxListener* node = messageHandlerStore[bin];
 
-    messageHandlerStore[id] = canRxListener;
+        do
+        {
+            if (node->canIdentifier == canRxListener->canIdentifier)
+            {
+                RAISE_ERROR(drivers, "overloading can rx listener");
+                return;
+            }
+
+            if (node->next == nullptr) break;
+            node = node->next;
+        } while (true);
+
+        node->next = canRxListener;
+    }
 }
 
 void CanRxHandler::pollCanData()
@@ -85,17 +103,17 @@ void CanRxHandler::processReceivedCanData(
     const modm::can::Message& rxMessage,
     CanRxListener* const* messageHandlerStore)
 {
-    uint16_t id = lookupTableIndexForCanId(rxMessage.getIdentifier());
+    uint16_t bin = binIndexForCanId(rxMessage.getIdentifier());
 
-    if (id >= NUM_CAN_IDS)
+    CanRxListener* listener = messageHandlerStore[bin];
+    while (listener != nullptr && listener->canIdentifier != rxMessage.identifier)
     {
-        RAISE_ERROR(drivers, "Invalid can id received");
-        return;
+        listener = listener->next;
     }
 
-    if (messageHandlerStore[id] != nullptr)
+    if (listener != nullptr)
     {
-        messageHandlerStore[id]->processMessage(rxMessage);
+        listener->processMessage(rxMessage);
     }
 }
 
@@ -115,15 +133,31 @@ void CanRxHandler::removeReceiveHandler(
     const CanRxListener& canRxListener,
     CanRxListener** messageHandlerStore)
 {
-    int id = lookupTableIndexForCanId(canRxListener.canIdentifier);
+    int bin = binIndexForCanId(canRxListener.canIdentifier);
 
-    if (id >= NUM_CAN_IDS)
+    if (messageHandlerStore[bin] == nullptr)
     {
-        RAISE_ERROR(drivers, "index out of bounds");
+        RAISE_ERROR(drivers, "listener not in handler storage");
         return;
     }
 
-    messageHandlerStore[id] = nullptr;
+    if (messageHandlerStore[bin]->canIdentifier == canRxListener.canIdentifier)
+    {
+        messageHandlerStore[bin] = messageHandlerStore[bin]->next;
+    }
+    else
+    {
+        CanRxListener* node = messageHandlerStore[bin];
+        while (node->next != nullptr)
+        {
+            if (node->next->canIdentifier == canRxListener.canIdentifier)
+            {
+                node->next = node->next->next;
+                return;
+            }
+        }
+        RAISE_ERROR(drivers, "listener not in handler storage");
+    }
 }
 
 }  // namespace tap::can
diff --git a/ut-robomaster/taproot/src/tap/communication/can/can_rx_handler.hpp b/ut-robomaster/taproot/src/tap/communication/can/can_rx_handler.hpp
index 8e6683c3..27e8eea8 100644
--- a/ut-robomaster/taproot/src/tap/communication/can/can_rx_handler.hpp
+++ b/ut-robomaster/taproot/src/tap/communication/can/can_rx_handler.hpp
@@ -65,10 +65,9 @@ class CanRxListener;
  * pollCanData function be called at a very high frequency,
  * so call this in a high frequency thread.
  *
- * @note The CAN handler can handle 64 CAN ids between [`0x1E4`, `0x224`). In the middle of this
- *      range, CAN ids [`0x201`, `0x20B`] are used by the `DjiMotor` objects to receive data from
- *      DJI branded motors. If you would like to define your own protocol, it is recommended to
- *      avoid avoid using CAN ids in this range.
+ * @note CAN ids [`0x201`, `0x20B`] are used by the `DjiMotor` objects to receive
+ *      data from DJI branded motors. If you would like to define your own protocol, it
+ *      is recommended to avoid avoid using CAN ids in this range.
  * @note the DjiMotor driver reserves `0x1FF` and `0x200` for commanding motors,
  *      and thus you should not attach listeners for these ids.
  *
@@ -79,27 +78,22 @@ class CanRxListener;
 class CanRxHandler
 {
 public:
-    static constexpr uint16_t MIN_CAN_ID = 0x1E4;
-    static constexpr uint16_t NUM_CAN_IDS = 64;
-    static constexpr uint16_t MAX_CAN_ID = MIN_CAN_ID + NUM_CAN_IDS;
+    /**
+     * The number of "hash" bins to store listeners in. `8` is choosen as most users will only have
+     * DJI Motors on their can bus, reducing any unnecessary overhead storing empty bins. The bus
+     * also become saturated near this limit, so the performance hit of a linked list traversal is
+     * minimal.
+     */
+    static constexpr uint8_t CAN_BINS = 8;
 
     CanRxHandler(Drivers* drivers);
     mockable ~CanRxHandler() = default;
     DISALLOW_COPY_AND_ASSIGN(CanRxHandler)
 
     /**
-     * Given a CAN identifier, returns the "normalized" id between [0, NUM_CAN_IDS), or a
-     * value >= NUM_CAN_IDS if the canId is outside the range specified.
+     * Given a CAN identifier, returns the bin index between [0, NUM_CAN_IDS) for the identifier.
      */
-    static inline uint16_t lookupTableIndexForCanId(uint16_t canId)
-    {
-        if (canId < MIN_CAN_ID)
-        {
-            return NUM_CAN_IDS;
-        }
-
-        return canId - MIN_CAN_ID;
-    }
+    static inline uint16_t binIndexForCanId(uint16_t canId) { return canId % CAN_BINS; }
 
     /**
      * Call this function to add a CanRxListener to the list of CanRxListener's
@@ -110,12 +104,11 @@ class CanRxHandler
      *      store listeners may or not be properly allocated if you do and
      *      undefined behavior will follow.
      * @note if you attempt to add a listener with an identifier identical to
-     *      something already in the `CanRxHandler`, an error is thrown and
-     *      the handler does not add the listener.
+     *      something already in the `CanRxHandler`, an error will be added to the
+     *      `ErrorController` and the handler does not add the listener.
      * @see `CanRxListener`
      *
      * @param[in] listener the listener to be attached ot the handler.
-     * @return `true` if listener successfully added, `false` otherwise.
      */
     mockable void attachReceiveHandler(CanRxListener* const listener);
 
@@ -146,13 +139,13 @@ class CanRxHandler
      * Stores pointers to the `CanRxListeners` for CAN 1, referenced when
      * a new message is received.
      */
-    CanRxListener* messageHandlerStoreCan1[NUM_CAN_IDS];
+    CanRxListener* messageHandlerStoreCan1[CAN_BINS];
 
     /**
      * Stores pointers to the `CanRxListeners` for CAN 2, referenced when
      * a new message is received.
      */
-    CanRxListener* messageHandlerStoreCan2[NUM_CAN_IDS];
+    CanRxListener* messageHandlerStoreCan2[CAN_BINS];
 
 #if defined(PLATFORM_HOSTED) && defined(ENV_UNIT_TESTS)
 public:
diff --git a/ut-robomaster/taproot/src/tap/communication/can/can_rx_listener.hpp b/ut-robomaster/taproot/src/tap/communication/can/can_rx_listener.hpp
index f1c0d618..26b55bf7 100644
--- a/ut-robomaster/taproot/src/tap/communication/can/can_rx_listener.hpp
+++ b/ut-robomaster/taproot/src/tap/communication/can/can_rx_listener.hpp
@@ -39,6 +39,7 @@ namespace tap
 class Drivers;
 namespace can
 {
+class CanRxHandler;
 /**
  * A class that when extended allows you to interface with the `can_rx_handler`.
  *
@@ -88,6 +89,8 @@ namespace can
  */
 class CanRxListener
 {
+    friend class CanRxHandler;
+
 public:
     /**
      * Construct a new CanRxListener, must specify the can identifier
@@ -135,6 +138,14 @@ class CanRxListener
     const CanBus canBus;
 
     Drivers* drivers;
+
+#ifndef ENV_UNIT_TESTS
+private:
+#endif
+    /**
+     * The next CanRxListener in the linked list for lookups.
+     */
+    CanRxListener* next = nullptr;
 };  // class CanRxListener
 
 }  // namespace can
diff --git a/ut-robomaster/taproot/src/tap/communication/sensors/encoder/encoder_interface.hpp b/ut-robomaster/taproot/src/tap/communication/sensors/encoder/encoder_interface.hpp
new file mode 100644
index 00000000..e4cdcb1d
--- /dev/null
+++ b/ut-robomaster/taproot/src/tap/communication/sensors/encoder/encoder_interface.hpp
@@ -0,0 +1,65 @@
+/*****************************************************************************/
+/********** !!! WARNING: CODE GENERATED BY TAPROOT. DO NOT EDIT !!! **********/
+/*****************************************************************************/
+
+/*
+ * Copyright (c) 2024 Advanced Robotics at the University of Washington <robomstr@uw.edu>
+ *
+ * This file is part of Taproot.
+ *
+ * Taproot is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Taproot is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Taproot.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+#ifndef TAPROOT_ENCODER_INTERFACE_HPP_
+#define TAPROOT_ENCODER_INTERFACE_HPP_
+
+#include "tap/algorithms/wrapped_float.hpp"
+
+namespace tap::encoder
+{
+class EncoderInterface
+{
+public:
+    /**
+     * Initialize the encoder. For instance: attaching a can receiver.
+     */
+    virtual void initialize() = 0;
+    /**
+     * Returns true when the encoder is online and reporting valid values.
+     */
+    virtual bool isOnline() const = 0;
+    /**
+     * Zeros the encoder to its current position. All encoder positions will be based from this
+     * position.
+     */
+    virtual void resetEncoderValue() = 0;
+    /**
+     * Gets the current position of the encoder, in a wrapped float on the range [0, M_TWOPI)
+     */
+    virtual tap::algorithms::WrappedFloat getPosition() const = 0;
+    /**
+     * Gets the current velocity reported by the encoder. Returned in a value of radians / second
+     */
+    virtual float getVelocity() const = 0;
+    /**
+     * Aligns this encoder to another encoder so that their positions are equal.
+     * If the two encoders are mechanically linked, they would then continue to report the same
+     * position.
+     */
+    virtual void alignWith(EncoderInterface* other) = 0;
+};
+
+}  // namespace tap::encoder
+
+#endif  // TAPROOT_ENCODER_INTERFACE_HPP_
diff --git a/ut-robomaster/taproot/src/tap/communication/sensors/encoder/multi_encoder.hpp b/ut-robomaster/taproot/src/tap/communication/sensors/encoder/multi_encoder.hpp
new file mode 100644
index 00000000..a5e30cae
--- /dev/null
+++ b/ut-robomaster/taproot/src/tap/communication/sensors/encoder/multi_encoder.hpp
@@ -0,0 +1,203 @@
+/*****************************************************************************/
+/********** !!! WARNING: CODE GENERATED BY TAPROOT. DO NOT EDIT !!! **********/
+/*****************************************************************************/
+
+/*
+ * Copyright (c) 2024 Advanced Robotics at the University of Washington <robomstr@uw.edu>
+ *
+ * This file is part of Taproot.
+ *
+ * Taproot is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Taproot is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Taproot.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+#ifndef TAPROOT_MULTI_ENCODER_HPP_
+#define TAPROOT_MULTI_ENCODER_HPP_
+
+#include <array>
+
+#include "tap/util_macros.hpp"
+
+#include "modm/architecture/interface/assert.hpp"
+
+#include "encoder_interface.hpp"
+
+namespace tap::encoder
+{
+/**
+ * A way to combine multiple encoders into one functional unit. The first encoder in the array
+ * *must* not be null, as it is the primary encoder. All other values can be null. This encoder
+ * uses the primary encoder as an initial source of truth. This allows the other encoders to align
+ * themselves to it, and then have the primary encoder disconnect with the system still working.
+ * This encoder is not valid until the primary encoder comes online, and stays online until either
+ * all synced encoders disconnect, or the primary encoder is offline and no other encoders have
+ * been synced. The positions returned by this encoder are averaged between all synced and online
+ * encoders.
+ */
+template <uint32_t COUNT>
+class MultiEncoder : public EncoderInterface
+{
+public:
+    MultiEncoder(std::array<EncoderInterface*, COUNT> encoders)
+        : encoders(encoders),
+          seenEncoders(0)
+    {
+        modm_assert(this->encoders[0] != nullptr, "MultiEncoder", "nullptr first encoder");
+    }
+
+    void initialize() override
+    {
+        for (EncoderInterface*& encoder : this->encoders)
+        {
+            if (encoder != nullptr)
+            {
+                encoder->initialize();
+            }
+        }
+    }
+
+    bool isOnline() const override
+    {
+        const_cast<MultiEncoder<COUNT>*>(this)->syncEncoders();
+
+        for (uint32_t i = 0; i < COUNT; i++)
+        {
+            if (this->validEncoder(i))
+            {
+                return true;
+            }
+        }
+
+        return false;
+    }
+
+    tap::algorithms::WrappedFloat getPosition() const override
+    {
+        const_cast<MultiEncoder<COUNT>*>(this)->syncEncoders();
+        int onlineEncoders = 0;
+        float position = 0;
+
+        for (uint32_t i = 0; i < COUNT; i++)
+        {
+            if (this->validEncoder(i))
+            {
+                position += this->encoders[i]->getPosition().getUnwrappedValue();
+                onlineEncoders += 1;
+            }
+        }
+
+        return tap::algorithms::WrappedFloat(
+            onlineEncoders == 0 ? 0 : position / onlineEncoders,
+            0,
+            static_cast<float>(M_TWOPI));
+    }
+
+    float getVelocity() const override
+    {
+        const_cast<MultiEncoder<COUNT>*>(this)->syncEncoders();
+        int onlineEncoders = 0;
+        float velocity = 0;
+
+        for (uint32_t i = 0; i < COUNT; i++)
+        {
+            if (this->validEncoder(i))
+            {
+                velocity += this->encoders[i]->getVelocity();
+                onlineEncoders += 1;
+            }
+        }
+
+        return onlineEncoders == 0 ? 0 : velocity / onlineEncoders;
+    };
+
+    void resetEncoderValue() override
+    {
+        this->syncEncoders();
+
+        for (EncoderInterface*& encoder : this->encoders)
+        {
+            if (encoder != nullptr)
+            {
+                encoder->resetEncoderValue();
+            }
+        }
+    }
+
+    void alignWith(EncoderInterface* other) override
+    {
+        this->syncEncoders();
+
+        for (EncoderInterface*& encoder : this->encoders)
+        {
+            if (encoder != nullptr)
+            {
+                encoder->alignWith(other);
+            }
+        }
+    }
+
+    DISALLOW_COPY_AND_ASSIGN(MultiEncoder)
+
+private:
+    std::array<EncoderInterface*, COUNT> encoders;
+    uint32_t seenEncoders;
+
+    void syncEncoders()
+    {
+        // The primary encoder *must* be online before syncing anything to it
+        if (this->encoders[0]->isOnline())
+        {
+            for (uint32_t i = 1; i < COUNT; i++)
+            {
+                bool online = this->encoders[i] != nullptr && this->encoders[i]->isOnline();
+                if (online && !this->seenEncoder(i))
+                {
+                    // Sync the newly discovered encoder to the primary encoder
+                    this->seenEncoders |= 1 << i;
+                    encoders[i]->alignWith(encoders[0]);
+                }
+                else if (validEncoder(i) && !seenEncoder(0))
+                {
+                    // Primary encoder disappeared but came back, resync it.
+                    this->seenEncoders |= 1;
+                    encoders[0]->alignWith(encoders[i]);
+                }
+                else if (!online)
+                {
+                    // This encoder disappeared, remove it
+                    this->seenEncoders &= ~(1 << i);
+                }
+            }
+            this->seenEncoders |= 1;
+        }
+        else
+        {
+            this->seenEncoders &= ~(1);
+        }
+    }
+
+    inline bool validEncoder(uint32_t index) const
+    {
+        return this->seenEncoder(index) && this->encoders[index] != nullptr &&
+               this->encoders[index]->isOnline();
+    }
+
+    inline bool seenEncoder(uint32_t index) const
+    {
+        return ((this->seenEncoders) & (1 << index)) != 0;
+    }
+};
+
+}  // namespace tap::encoder
+
+#endif  // TAPROOT_MULTI_ENCODER_HPP_
diff --git a/ut-robomaster/taproot/src/tap/communication/sensors/encoder/wrapped_encoder.cpp b/ut-robomaster/taproot/src/tap/communication/sensors/encoder/wrapped_encoder.cpp
new file mode 100644
index 00000000..317ff7dc
--- /dev/null
+++ b/ut-robomaster/taproot/src/tap/communication/sensors/encoder/wrapped_encoder.cpp
@@ -0,0 +1,119 @@
+/*****************************************************************************/
+/********** !!! WARNING: CODE GENERATED BY TAPROOT. DO NOT EDIT !!! **********/
+/*****************************************************************************/
+
+/*
+ * Copyright (c) 2025 Advanced Robotics at the University of Washington <robomstr@uw.edu>
+ *
+ * This file is part of Taproot.
+ *
+ * Taproot is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Taproot is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Taproot.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+#include "wrapped_encoder.hpp"
+
+#include "tap/algorithms/math_user_utils.hpp"
+#include "tap/architecture/clock.hpp"
+
+namespace tap
+{
+namespace encoder
+{
+WrappedEncoder::WrappedEncoder(
+    bool isInverted,
+    uint32_t encoderResolution,
+    float gearRatio,
+    uint32_t encoderHomePosition)
+    : encoder(tap::algorithms::WrappedFloat(0, 0, encoderResolution)),
+      position(tap::algorithms::Angle(0)),
+      inverted(isInverted),
+      encoderResolution(encoderResolution),
+      gearRatio(gearRatio),
+      encoderHomePosition(tap::algorithms::WrappedFloat(encoderHomePosition, 0, encoderResolution)),
+      pastPosition(tap::algorithms::Angle(0)),
+      lastUpdateTime(0),
+      deltaTime(0)
+{
+}
+
+void WrappedEncoder::resetEncoderValue()
+{
+    encoderHomePosition = encoder + encoderHomePosition;
+    encoder.setUnwrappedValue(0);
+    pastPosition -= position;
+    position.setUnwrappedValue(0);
+}
+
+tap::algorithms::WrappedFloat WrappedEncoder::getPosition() const
+{
+#if defined(PLATFORM_HOSTED) && defined(ENV_UNIT_TESTS)
+    return tap::algorithms::Angle(
+        this->getEncoder().getUnwrappedValue() * static_cast<float>(M_TWOPI) / encoderResolution *
+        gearRatio);
+#else
+    return position;
+#endif
+}
+
+float WrappedEncoder::getVelocity() const
+{
+    if (deltaTime == 0)
+    {
+        return 0;
+    }
+
+    return (position - pastPosition).getUnwrappedValue() / deltaTime * 1'000'000;
+}
+
+void WrappedEncoder::alignWith(EncoderInterface* other)
+{
+    tap::algorithms::WrappedFloat positionDifference = other->getPosition() - position;
+    float offset = positionDifference.getUnwrappedValue() / static_cast<float>(M_TWOPI) *
+                   encoderResolution * gearRatio;
+    this->encoderHomePosition += offset;
+    this->encoder += offset;
+    this->position = other->getPosition();
+    this->pastPosition += positionDifference;
+}
+
+void WrappedEncoder::updateEncoderValue(uint32_t encoderActual)
+{
+    // invert motor if necessary
+    encoderActual = inverted ? encoderResolution - 1 - encoderActual : encoderActual;
+
+    int32_t encoderRelativeToHome =
+        (int32_t)encoderActual - (int32_t)encoderHomePosition.getWrappedValue();
+
+    uint32_t newEncWrapped = encoderRelativeToHome < 0
+                                 ? (int32_t)encoderResolution + encoderRelativeToHome
+                                 : encoderRelativeToHome;
+
+    if (lastUpdateTime == 0)  // The first time we get a value, want it to always be positive
+    {
+        encoder = tap::algorithms::WrappedFloat(newEncWrapped, 0, encoderResolution);
+    }
+    else
+    {
+        encoder += encoder.minDifference(newEncWrapped);
+    }
+
+    pastPosition = position;
+    deltaTime = tap::arch::clock::getTimeMicroseconds() - lastUpdateTime;
+    lastUpdateTime = tap::arch::clock::getTimeMicroseconds();
+    position.setUnwrappedValue(
+        encoder.getUnwrappedValue() * static_cast<float>(M_TWOPI) / encoderResolution * gearRatio);
+}
+}  // namespace encoder
+
+}  // namespace tap
diff --git a/ut-robomaster/taproot/src/tap/communication/sensors/encoder/wrapped_encoder.hpp b/ut-robomaster/taproot/src/tap/communication/sensors/encoder/wrapped_encoder.hpp
new file mode 100644
index 00000000..83b091ea
--- /dev/null
+++ b/ut-robomaster/taproot/src/tap/communication/sensors/encoder/wrapped_encoder.hpp
@@ -0,0 +1,119 @@
+/*****************************************************************************/
+/********** !!! WARNING: CODE GENERATED BY TAPROOT. DO NOT EDIT !!! **********/
+/*****************************************************************************/
+
+/*
+ * Copyright (c) 2025 Advanced Robotics at the University of Washington <robomstr@uw.edu>
+ *
+ * This file is part of Taproot.
+ *
+ * Taproot is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Taproot is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Taproot.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+#ifndef TAPROOT_WRAPPED_ENCODER_HPP_
+#define TAPROOT_WRAPPED_ENCODER_HPP_
+
+#include "tap/communication/sensors/encoder/encoder_interface.hpp"
+#include "tap/util_macros.hpp"
+
+namespace tap::encoder
+{
+/**
+ * Represents an encoder that reports its values as a wrapped number of ticks.
+ */
+class WrappedEncoder : public EncoderInterface
+{
+public:
+    /**
+     * @param isInverted if `false` the positive rotation direction of the shaft is
+     *      counter-clockwise when looking at the shaft from.
+     *      If `true` then the positive rotation direction will be clockwise.
+     * @param encoderResolution the number of encoder ticks before the value wraps.
+     * @param gearRatio the ratio of input revolutions over output revolutions of this encoder.
+     * @param encoderHomePosition the zero position for the encoder in encoder ticks.
+     */
+    WrappedEncoder(
+        bool isInverted,
+        uint32_t encoderResolution,
+        float gearRatio = 1,
+        uint32_t encoderHomePosition = 0);
+
+    void initialize() override{};
+
+    tap::algorithms::WrappedFloat getPosition() const override;
+
+    /**
+     * Gets the raw encoder position.
+     */
+    mockable tap::algorithms::WrappedFloat getEncoder() const { return encoder; }
+
+    float getVelocity() const override;
+
+    void alignWith(EncoderInterface* other) override;
+
+    void resetEncoderValue() override;
+
+    DISALLOW_COPY_AND_ASSIGN(WrappedEncoder)
+
+#if defined(PLATFORM_HOSTED) && defined(ENV_UNIT_TESTS)
+    bool isOnline() const override { return true; }
+#else
+protected:
+#endif
+
+    /**
+     * Updates the stored encoder value given a newly received encoder value
+     * special logic necessary for keeping track of unwrapped encoder value.
+     */
+    void updateEncoderValue(uint32_t encoderActual);
+
+    /**
+     * The current encoder position.
+     */
+    tap::algorithms::WrappedFloat encoder;
+
+    /**
+     * The encoder position converted into output rotations
+     */
+    tap::algorithms::WrappedFloat position;
+
+    bool inverted;
+
+    const uint32_t encoderResolution;
+
+    const float gearRatio;
+
+private:
+    /**
+     * The actual encoder wrapped value received from CAN messages where this motor
+     * is considered to have an encoder value of 0. encoderHomePosition is 0 by default.
+     */
+    tap::algorithms::WrappedFloat encoderHomePosition;
+
+    /**
+     * The past position for the encoder. Used in velocity calculations.
+     */
+    tap::algorithms::WrappedFloat pastPosition;
+
+    /**
+     * The last update time for the encoder. Used in velocity calculations.
+     */
+    uint64_t lastUpdateTime;
+
+    uint64_t deltaTime;
+};
+
+}  // namespace tap::encoder
+
+#endif  // TAPROOT_WRAPPED_ENCODER_HPP_
diff --git a/ut-robomaster/taproot/src/tap/communication/sensors/imu/abstract_imu.cpp b/ut-robomaster/taproot/src/tap/communication/sensors/imu/abstract_imu.cpp
new file mode 100644
index 00000000..ba33c9fe
--- /dev/null
+++ b/ut-robomaster/taproot/src/tap/communication/sensors/imu/abstract_imu.cpp
@@ -0,0 +1,101 @@
+/*****************************************************************************/
+/********** !!! WARNING: CODE GENERATED BY TAPROOT. DO NOT EDIT !!! **********/
+/*****************************************************************************/
+
+/*
+ * Copyright (c) 2020-2025 Advanced Robotics at the University of Washington <robomstr@uw.edu>
+ *
+ * This file is part of Taproot.
+ *
+ * Taproot is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Taproot is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Taproot.  If not, see <https://www.gnu.org/licenses/>.
+ */
+#include "abstract_imu.hpp"
+
+namespace tap::communication::sensors::imu
+{
+void AbstractIMU::initialize(float sampleFrequency, float mahonyKp, float mahonyKi)
+{
+    mahonyAlgorithm.begin(sampleFrequency, mahonyKp, mahonyKi);
+    imuState = ImuState::IMU_NOT_CALIBRATED;
+    readTimeout.restart(1'000'000 / sampleFrequency);
+}
+
+void AbstractIMU::requestCalibration()
+{
+    if (imuState == ImuState::IMU_NOT_CALIBRATED || imuState == ImuState::IMU_CALIBRATED)
+    {
+        resetOffsets();
+        calibrationSample = 0;
+        imuState = ImuState::IMU_CALIBRATING;
+    }
+}
+
+void AbstractIMU::setMountingTransform(const Transform& transform)
+{
+    mountingTransform = transform;
+}
+
+void AbstractIMU::periodicIMUUpdate()
+{
+    if (imuState == ImuState::IMU_CALIBRATING)
+    {
+        computeOffsets();
+    }
+    else
+    {
+        mahonyAlgorithm.updateIMU(
+            imuData.gyroRadPerSec.x(),
+            imuData.gyroRadPerSec.y(),
+            imuData.gyroRadPerSec.z(),
+            imuData.accG.x(),
+            imuData.accG.y(),
+            imuData.accG.z());
+    }
+}
+
+void AbstractIMU::resetOffsets()
+{
+    imuData.accOffsetRaw = {0, 0, 0};
+    imuData.gyroOffsetRaw = {0, 0, 0};
+}
+
+void AbstractIMU::setAccelOffset(float x, float y, float z)
+{
+    imuData.accOffsetRaw = tap::algorithms::transforms::Vector(x, y, z);
+}
+
+void AbstractIMU::setGyroOffset(float x, float y, float z)
+{
+    imuData.gyroOffsetRaw = tap::algorithms::transforms::Vector(x, y, z);
+}
+
+void AbstractIMU::computeOffsets()
+{
+    calibrationSample++;
+
+    imuData.gyroOffsetRaw = imuData.gyroOffsetRaw + imuData.gyroRaw;
+    imuData.accOffsetRaw = imuData.accOffsetRaw + imuData.accRaw -
+                           tap::algorithms::transforms::Vector(0, 0, getAccelerationSensitivity());
+
+    if (calibrationSample >= offsetSampleCount)
+    {
+        calibrationSample = 0;
+        imuData.gyroOffsetRaw = imuData.gyroOffsetRaw / offsetSampleCount;
+        imuData.accOffsetRaw = imuData.accOffsetRaw / offsetSampleCount;
+        imuState = ImuState::IMU_CALIBRATED;
+        mahonyAlgorithm.reset();
+    }
+}
+
+}  // namespace tap::communication::sensors::imu
diff --git a/ut-robomaster/taproot/src/tap/communication/sensors/imu/abstract_imu.hpp b/ut-robomaster/taproot/src/tap/communication/sensors/imu/abstract_imu.hpp
new file mode 100644
index 00000000..84bb24c4
--- /dev/null
+++ b/ut-robomaster/taproot/src/tap/communication/sensors/imu/abstract_imu.hpp
@@ -0,0 +1,144 @@
+/*****************************************************************************/
+/********** !!! WARNING: CODE GENERATED BY TAPROOT. DO NOT EDIT !!! **********/
+/*****************************************************************************/
+
+/*
+ * Copyright (c) 2020-2025 Advanced Robotics at the University of Washington <robomstr@uw.edu>
+ *
+ * This file is part of Taproot.
+ *
+ * Taproot is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Taproot is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Taproot.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+#ifndef TAPROOT_ABSTRACT_IMU_HPP_
+#define TAPROOT_ABSTRACT_IMU_HPP_
+
+#include "tap/algorithms/MahonyAHRS.h"
+#include "tap/algorithms/transforms/orientation.hpp"
+#include "tap/algorithms/transforms/transform.hpp"
+#include "tap/algorithms/transforms/vector.hpp"
+#include "tap/architecture/periodic_timer.hpp"
+#include "tap/communication/sensors/imu/imu_interface.hpp"
+#include "tap/util_macros.hpp"
+
+namespace tap::communication::sensors::imu
+{
+using tap::algorithms::transforms::Orientation;
+using tap::algorithms::transforms::Transform;
+
+constexpr float GRAVITY_MPS2 = 9.81f;
+/**
+ * A class that represents any directly connected IMU.
+ */
+class AbstractIMU : public ImuInterface
+{
+public:
+    AbstractIMU(const Transform& mountingTransform = Transform::identity())
+        : mountingTransform(mountingTransform)
+    {
+    }
+
+    void setMountingTransform(const Transform& transform);
+
+    virtual ~AbstractIMU() = default;
+
+    virtual void initialize(float sampleFrequency, float mahonyKp, float mahonyKi);
+
+    /**
+     * When this function is called, the bmi088 enters a calibration state during which time,
+     * gyro/accel calibration offsets will be computed and the mahony algorithm reset. When
+     * calibrating, angle, accelerometer, and gyroscope values will return 0. When calibrating
+     * the BMI088 should be level, otherwise the IMU will be calibrated incorrectly.
+     */
+    virtual void requestCalibration();
+
+    /**
+     * Call this function at same rate as intialized sample frequency.
+     * Performs the mahony AHRS algorithm to compute pitch/roll/yaw.
+     */
+    virtual void periodicIMUUpdate();
+
+    /**
+     * Returns the state of the IMU. Can be not connected, connected but not calibrated, or
+     * calibrated. When not connected, IMU data will be garbage. When not calibrated, IMU data is
+     * valid but the computed yaw angle data will drift. When calibrating, the IMU data is invalid.
+     * When calibrated, the IMU data is valid and assuming proper calibration the IMU data should
+     * not drift.
+     *
+     * To be safe, whenever you call the functions below, call this function to ensure
+     * the data you are about to receive is not garbage.
+     */
+    virtual ImuState getImuState() const { return imuState; }
+
+    mockable inline float getAx() const override { return imuData.accG.x(); }
+    mockable inline float getAy() const override { return imuData.accG.y(); }
+    mockable inline float getAz() const override { return imuData.accG.z(); }
+    mockable inline float getAzMinusG() const { return imuData.accG.z() - GRAVITY_MPS2; }
+
+    mockable inline float getGx() const override { return imuData.gyroRadPerSec.x(); }
+    mockable inline float getGy() const override { return imuData.gyroRadPerSec.y(); }
+    mockable inline float getGz() const override { return imuData.gyroRadPerSec.z(); }
+
+    mockable inline float getTemp() const { return imuData.temperature; }
+
+    virtual inline float getYaw() const override { return mahonyAlgorithm.getYaw(); }
+    virtual inline float getPitch() const override { return mahonyAlgorithm.getPitch(); }
+    virtual inline float getRoll() const override { return mahonyAlgorithm.getRoll(); }
+
+    struct ImuData
+    {
+        tap::algorithms::transforms::Vector accRaw = {0, 0, 0};
+        tap::algorithms::transforms::Vector gyroRaw = {0, 0, 0};
+        tap::algorithms::transforms::Vector accOffsetRaw = {0, 0, 0};
+        tap::algorithms::transforms::Vector gyroOffsetRaw = {0, 0, 0};
+        tap::algorithms::transforms::Vector accG = {0, 0, 0};
+        tap::algorithms::transforms::Vector gyroRadPerSec = {0, 0, 0};
+
+        float temperature = 0;
+    };
+
+    void setCalibrationSamples(int sampleCount) { offsetSampleCount = sampleCount; }
+
+protected:
+    void resetOffsets();
+    void computeOffsets();
+    void setAccelOffset(float x, float y, float z);
+    void setGyroOffset(float x, float y, float z);
+
+    inline void applyTransform(ImuData& data)
+    {
+        data.accG = mountingTransform.apply(data.accG);
+        data.gyroRadPerSec = mountingTransform.apply(data.gyroRadPerSec);
+    }
+
+    virtual inline float getAccelerationSensitivity() const = 0;
+
+    tap::algorithms::transforms::Transform mountingTransform;
+
+    Mahony mahonyAlgorithm;
+
+    ImuState imuState = ImuState::IMU_NOT_CONNECTED;
+    int calibrationSample = 0;
+    int offsetSampleCount = 1000;
+
+    ImuData imuData;
+
+    tap::arch::PeriodicMicroTimer readTimeout;
+
+    uint32_t prevIMUDataReceivedTime = 0;
+};
+
+}  // namespace tap::communication::sensors::imu
+
+#endif  // TAPROOT_ABSTRACT_IMU_HPP_
diff --git a/ut-robomaster/taproot/src/tap/communication/sensors/imu/bmi088/bmi088.cpp b/ut-robomaster/taproot/src/tap/communication/sensors/imu/bmi088/bmi088.cpp
index 74f7e1c7..425613f3 100644
--- a/ut-robomaster/taproot/src/tap/communication/sensors/imu/bmi088/bmi088.cpp
+++ b/ut-robomaster/taproot/src/tap/communication/sensors/imu/bmi088/bmi088.cpp
@@ -48,33 +48,9 @@ namespace tap::communication::sensors::imu::bmi088
 #define DELAY_US(us) modm::delay_us(us);
 #endif
 
-Bmi088::Bmi088(tap::Drivers *drivers) : drivers(drivers), imuHeater(drivers) {}
-
-Bmi088::ImuState Bmi088::getImuState() const { return imuState; }
-
-void Bmi088::requestRecalibration()
-{
-    if (imuState == ImuState::IMU_NOT_CALIBRATED || imuState == ImuState::IMU_CALIBRATED)
-    {
-        data.gyroOffsetRaw[ImuData::X] = 0;
-        data.gyroOffsetRaw[ImuData::Y] = 0;
-        data.gyroOffsetRaw[ImuData::Z] = 0;
-        data.accOffsetRaw[ImuData::X] = 0;
-        data.accOffsetRaw[ImuData::Y] = 0;
-        data.accOffsetRaw[ImuData::Z] = 0;
-        data.gyroDegPerSec[ImuData::X] = 0;
-        data.gyroDegPerSec[ImuData::Y] = 0;
-        data.gyroDegPerSec[ImuData::Z] = 0;
-        data.accG[ImuData::X] = 0;
-        data.accG[ImuData::Y] = 0;
-        data.accG[ImuData::Z] = 0;
-        calibrationSample = 0;
-        imuState = ImuState::IMU_CALIBRATING;
-    }
-}
-
 void Bmi088::initialize(float sampleFrequency, float mahonyKp, float mahonyKi)
 {
+    AbstractIMU::initialize(sampleFrequency, mahonyKp, mahonyKi);
 #if !defined(PLATFORM_HOSTED)
     ImuCS1Accel::GpioOutput();
     ImuCS1Gyro::GpioOutput();
@@ -93,8 +69,6 @@ void Bmi088::initialize(float sampleFrequency, float mahonyKp, float mahonyKi)
     initializeGyro();
 
     imuHeater.initialize();
-
-    mahonyAlgorithm.begin(sampleFrequency, mahonyKp, mahonyKi);
 }
 
 void Bmi088::initializeAcc()
@@ -172,89 +146,45 @@ void Bmi088::initializeGyro()
 
 void Bmi088::periodicIMUUpdate()
 {
-    if (imuState == ImuState::IMU_NOT_CONNECTED)
-    {
-        RAISE_ERROR(drivers, "periodicIMUUpdate called w/ imu not connected");
-        return;
-    }
-
-    if (imuState == ImuState::IMU_CALIBRATING)
-    {
-        computeOffsets();
-    }
-    else
-    {
-        mahonyAlgorithm.updateIMU(
-            data.gyroDegPerSec[ImuData::X],
-            data.gyroDegPerSec[ImuData::Y],
-            data.gyroDegPerSec[ImuData::Z],
-            data.accG[ImuData::X],
-            data.accG[ImuData::Y],
-            data.accG[ImuData::Z]);
-    }
-
-    imuHeater.runTemperatureController(data.temperature);
+    AbstractIMU::periodicIMUUpdate();
+    imuHeater.runTemperatureController(imuData.temperature);
 }
 
-void Bmi088::computeOffsets()
+bool Bmi088::read()
 {
-    calibrationSample++;
-
-    data.gyroOffsetRaw[ImuData::X] += data.gyroRaw[ImuData::X];
-    data.gyroOffsetRaw[ImuData::Y] += data.gyroRaw[ImuData::Y];
-    data.gyroOffsetRaw[ImuData::Z] += data.gyroRaw[ImuData::Z];
-    data.accOffsetRaw[ImuData::X] += data.accRaw[ImuData::X];
-    data.accOffsetRaw[ImuData::Y] += data.accRaw[ImuData::Y];
-    data.accOffsetRaw[ImuData::Z] +=
-        data.accRaw[ImuData::Z] - (tap::algorithms::ACCELERATION_GRAVITY / ACC_G_PER_ACC_COUNT);
-
-    if (calibrationSample >= BMI088_OFFSET_SAMPLES)
+    if (!readTimeout.execute())
     {
-        calibrationSample = 0;
-        data.gyroOffsetRaw[ImuData::X] /= BMI088_OFFSET_SAMPLES;
-        data.gyroOffsetRaw[ImuData::Y] /= BMI088_OFFSET_SAMPLES;
-        data.gyroOffsetRaw[ImuData::Z] /= BMI088_OFFSET_SAMPLES;
-        data.accOffsetRaw[ImuData::X] /= BMI088_OFFSET_SAMPLES;
-        data.accOffsetRaw[ImuData::Y] /= BMI088_OFFSET_SAMPLES;
-        data.accOffsetRaw[ImuData::Z] /= BMI088_OFFSET_SAMPLES;
-        imuState = ImuState::IMU_CALIBRATED;
-        mahonyAlgorithm.reset();
+        return false;
     }
-}
 
-void Bmi088::read()
-{
     uint8_t rxBuff[6] = {};
 
     Bmi088Hal::bmi088AccReadMultiReg(Acc::ACC_X_LSB, rxBuff, 6);
 
     prevIMUDataReceivedTime = tap::arch::clock::getTimeMicroseconds();
 
-    data.accRaw[ImuData::X] = bigEndianInt16ToFloat(rxBuff);
-    data.accRaw[ImuData::Y] = bigEndianInt16ToFloat(rxBuff + 2);
-    data.accRaw[ImuData::Z] = bigEndianInt16ToFloat(rxBuff + 4);
+    float rawAccX = bigEndianInt16ToFloat(rxBuff);
+    float rawAccY = bigEndianInt16ToFloat(rxBuff + 2);
+    float rawAccZ = bigEndianInt16ToFloat(rxBuff + 4);
+    imuData.accRaw = tap::algorithms::transforms::Vector(rawAccX, rawAccY, rawAccZ);
 
     Bmi088Hal::bmi088GyroReadMultiReg(Gyro::RATE_X_LSB, rxBuff, 6);
-    data.gyroRaw[ImuData::X] = bigEndianInt16ToFloat(rxBuff);
-    data.gyroRaw[ImuData::Y] = bigEndianInt16ToFloat(rxBuff + 2);
-    data.gyroRaw[ImuData::Z] = bigEndianInt16ToFloat(rxBuff + 4);
+
+    float rawGyroX = bigEndianInt16ToFloat(rxBuff);
+    float rawGyroY = bigEndianInt16ToFloat(rxBuff + 2);
+    float rawGyroZ = bigEndianInt16ToFloat(rxBuff + 4);
+    imuData.gyroRaw = tap::algorithms::transforms::Vector(rawGyroX, rawGyroY, rawGyroZ);
 
     Bmi088Hal::bmi088AccReadMultiReg(Acc::TEMP_MSB, rxBuff, 2);
-    data.temperature = parseTemp(rxBuff[0], rxBuff[1]);
-
-    data.gyroDegPerSec[ImuData::X] =
-        GYRO_DS_PER_GYRO_COUNT * (data.gyroRaw[ImuData::X] - data.gyroOffsetRaw[ImuData::X]);
-    data.gyroDegPerSec[ImuData::Y] =
-        GYRO_DS_PER_GYRO_COUNT * (data.gyroRaw[ImuData::Y] - data.gyroOffsetRaw[ImuData::Y]);
-    data.gyroDegPerSec[ImuData::Z] =
-        GYRO_DS_PER_GYRO_COUNT * (data.gyroRaw[ImuData::Z] - data.gyroOffsetRaw[ImuData::Z]);
-
-    data.accG[ImuData::X] =
-        ACC_G_PER_ACC_COUNT * (data.accRaw[ImuData::X] - data.accOffsetRaw[ImuData::X]);
-    data.accG[ImuData::Y] =
-        ACC_G_PER_ACC_COUNT * (data.accRaw[ImuData::Y] - data.accOffsetRaw[ImuData::Y]);
-    data.accG[ImuData::Z] =
-        ACC_G_PER_ACC_COUNT * (data.accRaw[ImuData::Z] - data.accOffsetRaw[ImuData::Z]);
+    imuData.temperature = parseTemp(rxBuff[0], rxBuff[1]);
+
+    imuData.gyroRadPerSec =
+        (imuData.gyroRaw - imuData.gyroOffsetRaw) * GYRO_RAD_PER_S_PER_GYRO_COUNT;
+    imuData.accG = (imuData.accRaw - imuData.accOffsetRaw) * ACC_G_PER_ACC_COUNT;
+
+    applyTransform(imuData);
+
+    return true;
 }
 
 void Bmi088::setAndCheckAccRegister(Acc::Register reg, Acc::Registers_t value)
diff --git a/ut-robomaster/taproot/src/tap/communication/sensors/imu/bmi088/bmi088.hpp b/ut-robomaster/taproot/src/tap/communication/sensors/imu/bmi088/bmi088.hpp
index 1c546658..5e70867f 100644
--- a/ut-robomaster/taproot/src/tap/communication/sensors/imu/bmi088/bmi088.hpp
+++ b/ut-robomaster/taproot/src/tap/communication/sensors/imu/bmi088/bmi088.hpp
@@ -26,19 +26,17 @@
 
 #include "tap/algorithms/MahonyAHRS.h"
 #include "tap/algorithms/math_user_utils.hpp"
+#include "tap/communication/sensors/imu/abstract_imu.hpp"
 #include "tap/communication/sensors/imu/imu_interface.hpp"
 #include "tap/communication/sensors/imu_heater/imu_heater.hpp"
 #include "tap/util_macros.hpp"
 
-#include "modm/processing/protothread.hpp"
-
 #include "bmi088_data.hpp"
 
 namespace tap
 {
 class Drivers;
 }
-
 namespace tap::communication::sensors::imu::bmi088
 {
 /**
@@ -56,25 +54,29 @@ namespace tap::communication::sensors::imu::bmi088
  *    expect
  *  - roll and pitch to be defined as.
  */
-class Bmi088 final_mockable : public Bmi088Data, public ImuInterface
+class Bmi088 final_mockable : public Bmi088Data, public AbstractIMU
 {
 public:
+    Bmi088(Drivers *drivers) : AbstractIMU(), drivers(drivers), imuHeater(drivers) {}
+    DISALLOW_COPY_AND_ASSIGN(Bmi088)
+    mockable ~Bmi088() = default;
+
     static constexpr Acc::AccRange_t ACC_RANGE = Acc::AccRange::G3;
     static constexpr Gyro::GyroRange_t GYRO_RANGE = Gyro::GyroRange::DPS2000;
     /**
-     * The maximum angular velocity in degrees / second that the gyro can read based on GYRO_RANGE
+     * The maximum angular velocity in radians / second that the gyro can read based on GYRO_RANGE
      * specified above.
      */
-    static constexpr float GYRO_RANGE_MAX_DS = 2000.0f;
+    static constexpr float GYRO_RANGE_MAX_RAD_PER_S = modm::toRadian(2000.0f);
 
     static constexpr float BMI088_TEMP_FACTOR = 0.125f;
     static constexpr float BMI088_TEMP_OFFSET = 23.0f;
 
     /**
-     * Used to convert raw gyro values to units of degrees / second. Ratio has units
-     * (degrees / second) / gyro counts.
+     * Used to convert raw gyro values to units of radians / second. Ratio has units
+     * (radians / second) / gyro counts.
      */
-    static constexpr float GYRO_DS_PER_GYRO_COUNT = GYRO_RANGE_MAX_DS / 32767.0f;
+    static constexpr float GYRO_RAD_PER_S_PER_GYRO_COUNT = GYRO_RANGE_MAX_RAD_PER_S / 32767.0f;
 
     /**
      * Refer to page 27 of the bmi088 datasheet for explination of this equation.
@@ -83,71 +85,26 @@ class Bmi088 final_mockable : public Bmi088Data, public ImuInterface
     static constexpr float ACC_G_PER_ACC_COUNT =
         modm::pow(2, ACC_RANGE.value + 1) * 1.5f * tap::algorithms::ACCELERATION_GRAVITY / 32768.0f;
 
-    /**
-     * The number of samples we take in order to determine the mpu offsets.
-     */
-    float BMI088_OFFSET_SAMPLES = 1000;
-
-    Bmi088(tap::Drivers *drivers);
-
     /**
      * Starts and configures the bmi088. Blocks for < 200 ms.
      */
-    mockable void initialize(float sampleFrequency, float mahonyKp, float mahonyKi);
+    mockable void initialize(float sampleFrequency, float mahonyKp, float mahonyKi) override;
 
     /**
      * Call this function at same rate as intialized sample frequency.
      * Performs the mahony AHRS algorithm to compute pitch/roll/yaw.
      */
-    mockable void periodicIMUUpdate();
+    mockable void periodicIMUUpdate() override;
 
     /**
      * This function reads the IMU data from SPI
      *
      * @note This function blocks for 129 microseconds to read registers from the BMI088.
      */
-    mockable void read();
-
-    /**
-     * Returns the state of the IMU. Can be not connected, connected but not calibrated, or
-     * calibrated. When not connected, IMU data will be garbage. When not calibrated, IMU data is
-     * valid but the computed yaw angle data will drift. When calibrating, the IMU data is invalid.
-     * When calibrated, the IMU data is valid and assuming proper calibration the IMU data should
-     * not drift.
-     *
-     * To be safe, whenever you call the functions below, call this function to ensure
-     * the data you are about to receive is not garbage.
-     */
-    mockable ImuState getImuState() const;
-
-    /**
-     * When this function is called, the bmi088 enters a calibration state during which time,
-     * gyro/accel calibration offsets will be computed and the mahony algorithm reset. When
-     * calibrating, angle, accelerometer, and gyroscope values will return 0. When calibrating
-     * the BMI088 should be level, otherwise the IMU will be calibrated incorrectly.
-     */
-    mockable void requestRecalibration();
+    mockable bool read();
 
     inline const char *getName() const final_mockable { return "bmi088"; }
 
-    mockable inline float getYaw() final_mockable { return mahonyAlgorithm.getYaw(); }
-    mockable inline float getPitch() final_mockable { return mahonyAlgorithm.getPitch(); }
-    mockable inline float getRoll() final_mockable { return mahonyAlgorithm.getRoll(); }
-
-    mockable inline float getGx() final_mockable { return data.gyroDegPerSec[ImuData::X]; }
-    mockable inline float getGy() final_mockable { return data.gyroDegPerSec[ImuData::Y]; }
-    mockable inline float getGz() final_mockable { return data.gyroDegPerSec[ImuData::Z]; }
-
-    mockable inline float getAx() final_mockable { return data.accG[ImuData::X]; }
-    mockable inline float getAy() final_mockable { return data.accG[ImuData::Y]; }
-    mockable inline float getAz() final_mockable { return data.accG[ImuData::Z]; }
-
-    mockable inline float getTemp() final_mockable { return data.temperature; }
-
-    mockable inline uint32_t getPrevIMUDataReceivedTime() const { return prevIMUDataReceivedTime; }
-
-    inline void setOffsetSamples(float samples) { BMI088_OFFSET_SAMPLES = samples; }
-
     inline void setAccOversampling(Acc::AccBandwidth oversampling)
     {
         accOversampling = oversampling;
@@ -162,41 +119,16 @@ class Bmi088 final_mockable : public Bmi088Data, public ImuInterface
     }
 
 private:
+    Drivers *drivers;
+
+    inline float getAccelerationSensitivity() const override { return ACC_G_PER_ACC_COUNT; }
+
     static constexpr uint16_t RAW_TEMPERATURE_TO_APPLY_OFFSET = 1023;
     /// Offset parsed temperature reading by this amount if > RAW_TEMPERATURE_TO_APPLY_OFFSET.
     static constexpr int16_t RAW_TEMPERATURE_OFFSET = -2048;
 
-    struct ImuData
-    {
-        enum Axis
-        {
-            X = 0,
-            Y = 1,
-            Z = 2,
-        };
-
-        float accRaw[3] = {};
-        float gyroRaw[3] = {};
-        float accOffsetRaw[3] = {};
-        float gyroOffsetRaw[3] = {};
-        float accG[3] = {};
-        float gyroDegPerSec[3] = {};
-
-        float temperature;
-    } data;
-
-    tap::Drivers *drivers;
-
-    ImuState imuState = ImuState::IMU_NOT_CONNECTED;
-
-    Mahony mahonyAlgorithm;
-
     imu_heater::ImuHeater imuHeater;
 
-    int calibrationSample = 0;
-
-    uint32_t prevIMUDataReceivedTime = 0;
-
     Acc::AccBandwidth accOversampling = Acc::AccBandwidth::NORMAL;
     Acc::AccOutputRate accOutputRate = Acc::AccOutputRate::Hz800;
 
@@ -205,8 +137,6 @@ class Bmi088 final_mockable : public Bmi088Data, public ImuInterface
     Gyro::GyroBandwidth gyroOutputRate = Gyro::GyroBandwidth::ODR1000_BANDWIDTH116;
     void initializeGyro();
 
-    void computeOffsets();
-
     void setAndCheckAccRegister(Acc::Register reg, Acc::Registers_t value);
 
     void setAndCheckGyroRegister(Gyro::Register reg, Gyro::Registers_t value);
diff --git a/ut-robomaster/taproot/src/tap/communication/sensors/imu/imu_interface.hpp b/ut-robomaster/taproot/src/tap/communication/sensors/imu/imu_interface.hpp
index fa5541be..905c104c 100644
--- a/ut-robomaster/taproot/src/tap/communication/sensors/imu/imu_interface.hpp
+++ b/ut-robomaster/taproot/src/tap/communication/sensors/imu/imu_interface.hpp
@@ -56,57 +56,52 @@ class ImuInterface
      * Returns the linear acceleration in the x direction, in
      * \f$\frac{\mbox{m}}{\mbox{second}^2}\f$.
      */
-    virtual inline float getAx() = 0;
+    virtual inline float getAx() const = 0;
 
     /**
      * Returns the linear acceleration in the y direction, in
      * \f$\frac{\mbox{m}}{\mbox{second}^2}\f$.
      */
-    virtual inline float getAy() = 0;
+    virtual inline float getAy() const = 0;
 
     /**
      * Returns the linear acceleration in the z direction, in
      * \f$\frac{\mbox{m}}{\mbox{second}^2}\f$.
      */
-    virtual inline float getAz() = 0;
+    virtual inline float getAz() const = 0;
 
     /**
      * Returns the gyroscope reading (rotational speed) in the x direction, in
-     * \f$\frac{\mbox{degrees}}{\mbox{second}}\f$.
+     * \f$\frac{\mbox{radians}}{\mbox{second}}\f$.
      */
-    virtual inline float getGx() = 0;
+    virtual inline float getGx() const = 0;
 
     /**
      * Returns the gyroscope reading (rotational speed) in the y direction, in
-     * \f$\frac{\mbox{degrees}}{\mbox{second}}\f$.
+     * \f$\frac{\mbox{radians}}{\mbox{second}}\f$.
      */
-    virtual inline float getGy() = 0;
+    virtual inline float getGy() const = 0;
 
     /**
      * Returns the gyroscope reading (rotational speed) in the z direction, in
-     * \f$\frac{\mbox{degrees}}{\mbox{second}}\f$.
+     * \f$\frac{\mbox{radians}}{\mbox{second}}\f$.
      */
-    virtual inline float getGz() = 0;
+    virtual inline float getGz() const = 0;
 
     /**
-     * Returns the temperature of the imu in degrees C.
+     * Returns yaw angle. in radians.
      */
-    virtual inline float getTemp() = 0;
+    virtual inline float getYaw() const = 0;
 
     /**
-     * Returns yaw angle. in degrees.
+     * Returns pitch angle in radians.
      */
-    virtual inline float getYaw() = 0;
+    virtual inline float getPitch() const = 0;
 
     /**
-     * Returns pitch angle in degrees.
+     * Returns roll angle in radians.
      */
-    virtual inline float getPitch() = 0;
-
-    /**
-     * Returns roll angle in degrees.
-     */
-    virtual inline float getRoll() = 0;
+    virtual inline float getRoll() const = 0;
 };
 }  // namespace tap::communication::sensors::imu
 
diff --git a/ut-robomaster/taproot/src/tap/communication/sensors/imu/imu_terminal_serial_handler.hpp b/ut-robomaster/taproot/src/tap/communication/sensors/imu/imu_terminal_serial_handler.hpp
index d6e6fd81..9064905e 100644
--- a/ut-robomaster/taproot/src/tap/communication/sensors/imu/imu_terminal_serial_handler.hpp
+++ b/ut-robomaster/taproot/src/tap/communication/sensors/imu/imu_terminal_serial_handler.hpp
@@ -29,7 +29,7 @@
 
 #include "modm/architecture/interface/register.hpp"
 
-#include "imu_interface.hpp"
+#include "abstract_imu.hpp"
 
 namespace tap
 {
@@ -46,7 +46,7 @@ namespace tap::communication::sensors::imu
 class ImuTerminalSerialHandler : public communication::serial::TerminalSerialCallbackInterface
 {
 public:
-    ImuTerminalSerialHandler(Drivers* drivers, ImuInterface* imu)
+    ImuTerminalSerialHandler(Drivers* drivers, AbstractIMU* imu)
         : drivers(drivers),
           imu(imu),
           subjectsBeingInspected(0)
@@ -75,7 +75,7 @@ class ImuTerminalSerialHandler : public communication::serial::TerminalSerialCal
 
     Drivers* drivers;
 
-    ImuInterface* imu;
+    AbstractIMU* imu;
 
     /**
      * Each element in the enum corresponds to a sensor that a user may query.
diff --git a/ut-robomaster/taproot/src/tap/algorithms/linear_interpolation_predictor_contiguous.cpp b/ut-robomaster/taproot/src/tap/communication/sensors/voltage/voltage_sensor_interface.hpp
similarity index 54%
rename from ut-robomaster/taproot/src/tap/algorithms/linear_interpolation_predictor_contiguous.cpp
rename to ut-robomaster/taproot/src/tap/communication/sensors/voltage/voltage_sensor_interface.hpp
index a0b20378..59a7b86e 100644
--- a/ut-robomaster/taproot/src/tap/algorithms/linear_interpolation_predictor_contiguous.cpp
+++ b/ut-robomaster/taproot/src/tap/communication/sensors/voltage/voltage_sensor_interface.hpp
@@ -21,35 +21,24 @@
  * along with Taproot.  If not, see <https://www.gnu.org/licenses/>.
  */
 
-#include "linear_interpolation_predictor_contiguous.hpp"
+#ifndef TAPROOT_VOLTAGE_SENSOR_INTERFACE_HPP_
+#define TAPROOT_VOLTAGE_SENSOR_INTERFACE_HPP_
 
-namespace tap::algorithms
-{
-LinearInterpolationPredictorContiguous::LinearInterpolationPredictorContiguous(
-    float lowerBound,
-    float upperBound)
-    : lastUpdateCallTime(0),
-      previousValue(0.0f, lowerBound, upperBound),
-      slope(0.0f)
-{
-}
+#include "tap/communication/sensors/sensor_interface.hpp"
 
-void LinearInterpolationPredictorContiguous::update(float newValue, uint32_t currTime)
+namespace tap::communication::sensors::voltage
 {
-    if (currTime <= lastUpdateCallTime)
-    {
-        slope = 0;
-        return;
-    }
-    slope = (previousValue.difference(newValue)) / (currTime - lastUpdateCallTime);
-    previousValue.setValue(newValue);
-    lastUpdateCallTime = currTime;
-}
-
-void LinearInterpolationPredictorContiguous::reset(float initialValue, uint32_t initialTime)
+/**
+ * Interface for a generic voltage sensor.
+ */
+class VoltageSensorInterface : public tap::communication::sensors::SensorInterface
 {
-    previousValue.setValue(initialValue);
-    lastUpdateCallTime = initialTime;
-    slope = 0.0f;
-}
-}  // namespace tap::algorithms
+public:
+    /**
+     * @return The voltage read by the voltage sensor, in millivolts.
+     */
+    virtual float getVoltageMv() const = 0;
+};
+}  // namespace tap::communication::sensors::voltage
+
+#endif  // TAPROOT_CURRENT_SENSOR_INTERFACE_HPP_
diff --git a/ut-robomaster/taproot/src/tap/communication/serial/ref_serial.cpp b/ut-robomaster/taproot/src/tap/communication/serial/ref_serial.cpp
index 3665b021..58efcd7d 100644
--- a/ut-robomaster/taproot/src/tap/communication/serial/ref_serial.cpp
+++ b/ut-robomaster/taproot/src/tap/communication/serial/ref_serial.cpp
@@ -77,11 +77,6 @@ void RefSerial::messageReceiveCallback(const ReceivedSerialMessage& completeMess
             decodeToSiteEventData(completeMessage);
             break;
         }
-        case REF_MESSAGE_TYPE_PROJECTILE_SUPPPLIER_ACTION:
-        {
-            decodeToProjectileSupplierAction(completeMessage);
-            break;
-        }
         case REF_MESSAGE_TYPE_WARNING_DATA:
         {
             decodeToWarningData(completeMessage);
@@ -113,11 +108,6 @@ void RefSerial::messageReceiveCallback(const ReceivedSerialMessage& completeMess
             decodeToRobotBuffs(completeMessage);
             break;
         }
-        case REF_MESSAGE_TYPE_AERIAL_ENERGY_STATUS:
-        {
-            decodeToAerialEnergyStatus(completeMessage);
-            break;
-        }
         case REF_MESSAGE_TYPE_RECEIVE_DAMAGE:
         {
             decodeToDamageStatus(completeMessage);
@@ -220,7 +210,6 @@ bool RefSerial::decodeToAllRobotHP(const ReceivedSerialMessage& message)
     convertFromLittleEndian(&robotData.allRobotHp.red.engineer2, message.data + 2);
     convertFromLittleEndian(&robotData.allRobotHp.red.standard3, message.data + 4);
     convertFromLittleEndian(&robotData.allRobotHp.red.standard4, message.data + 6);
-    convertFromLittleEndian(&robotData.allRobotHp.red.standard5, message.data + 8);
     convertFromLittleEndian(&robotData.allRobotHp.red.sentry7, message.data + 10);
     convertFromLittleEndian(&robotData.allRobotHp.red.outpost, message.data + 12);
     convertFromLittleEndian(&robotData.allRobotHp.red.base, message.data + 14);
@@ -229,7 +218,6 @@ bool RefSerial::decodeToAllRobotHP(const ReceivedSerialMessage& message)
     convertFromLittleEndian(&robotData.allRobotHp.blue.engineer2, message.data + 18);
     convertFromLittleEndian(&robotData.allRobotHp.blue.standard3, message.data + 20);
     convertFromLittleEndian(&robotData.allRobotHp.blue.standard4, message.data + 22);
-    convertFromLittleEndian(&robotData.allRobotHp.blue.standard5, message.data + 24);
     convertFromLittleEndian(&robotData.allRobotHp.blue.sentry7, message.data + 26);
     convertFromLittleEndian(&robotData.allRobotHp.blue.outpost, message.data + 28);
     convertFromLittleEndian(&robotData.allRobotHp.blue.base, message.data + 30);
@@ -248,9 +236,8 @@ bool RefSerial::decodeToSiteEventData(const ReceivedSerialMessage& message)
     convertFromLittleEndian(&data, message.data);
 
     gameData.eventData.siteData.value = data;
-    gameData.eventData.virtualShieldRemainingPercent = static_cast<uint8_t>((data >> 12) & 0x3F);
-    gameData.eventData.timeSinceLastDartHit = static_cast<uint8_t>((data >> 19) & 0xFF);
-    gameData.eventData.lastDartHit = static_cast<Rx::SiteDartHit>((data >> 28) & 0x03);
+    gameData.eventData.timeSinceLastDartHit = static_cast<uint8_t>((data >> 9) & 0xFF);
+    gameData.eventData.lastDartHit = static_cast<Rx::SiteDartHit>((data >> 18) & 0x07);
 
     return true;
 }
@@ -325,9 +312,6 @@ bool RefSerial::decodeToPowerAndHeat(const ReceivedSerialMessage& message)
     {
         return false;
     }
-    convertFromLittleEndian(&robotData.chassis.volt, message.data);
-    convertFromLittleEndian(&robotData.chassis.current, message.data + 2);
-    convertFromLittleEndian(&robotData.chassis.power, message.data + 4);
     convertFromLittleEndian(&robotData.chassis.powerBuffer, message.data + 8);
     convertFromLittleEndian(&robotData.turret.heat17ID1, message.data + 10);
     convertFromLittleEndian(&robotData.turret.heat17ID2, message.data + 12);
@@ -349,7 +333,7 @@ bool RefSerial::decodeToRobotPosition(const ReceivedSerialMessage& message)
 
 bool RefSerial::decodeToRobotBuffs(const ReceivedSerialMessage& message)
 {
-    if (message.header.dataLength != 6)
+    if (message.header.dataLength != 7)
     {
         return false;
     }
@@ -359,6 +343,8 @@ bool RefSerial::decodeToRobotBuffs(const ReceivedSerialMessage& message)
     robotData.robotBuffStatus.vulnerabilityBuff = message.data[3];
 
     convertFromLittleEndian(&robotData.robotBuffStatus.attackBuff, message.data + 4);
+    robotData.robotEnergyRemaining = static_cast<Rx::RobotEnergyLevel>(message.data[6]);
+
     return true;
 }
 
@@ -446,8 +432,6 @@ bool RefSerial::decodeToGroundPositions(const ReceivedSerialMessage& message)
     convertFromLittleEndian(&gameData.positions.standard3.y, message.data + 20);
     convertFromLittleEndian(&gameData.positions.standard4.x, message.data + 24);
     convertFromLittleEndian(&gameData.positions.standard4.y, message.data + 28);
-    convertFromLittleEndian(&gameData.positions.standard5.x, message.data + 32);
-    convertFromLittleEndian(&gameData.positions.standard5.y, message.data + 36);
     return true;
 }
 
@@ -462,7 +446,6 @@ bool RefSerial::decodeToRadarProgress(const ReceivedSerialMessage& message)
     gameData.radarProgress.engineer = message.data[2];
     gameData.radarProgress.standard3 = message.data[3];
     gameData.radarProgress.standard4 = message.data[4];
-    gameData.radarProgress.standard5 = message.data[5];
     gameData.radarProgress.sentry = message.data[6];
 
     return true;
diff --git a/ut-robomaster/taproot/src/tap/communication/serial/ref_serial.hpp b/ut-robomaster/taproot/src/tap/communication/serial/ref_serial.hpp
index 988534d1..11c330f9 100644
--- a/ut-robomaster/taproot/src/tap/communication/serial/ref_serial.hpp
+++ b/ut-robomaster/taproot/src/tap/communication/serial/ref_serial.hpp
@@ -81,8 +81,8 @@ class RefSerial : public DJISerial, public RefSerialData
      * parser yet. They are values that are used in message headers to indicate the type of message
      * we have received.
      *
-     * Current Ref Serial Version: 1.6.1
-     * Updated April 2024.
+     * Current Ref Serial Version: 1.7.0
+     * Updated March 2025.
      */
     enum MessageType
     {
@@ -91,7 +91,6 @@ class RefSerial : public DJISerial, public RefSerialData
         REF_MESSAGE_TYPE_ALL_ROBOT_HP = 0x3,
 
         REF_MESSAGE_TYPE_SITE_EVENT_DATA = 0x101,
-        REF_MESSAGE_TYPE_PROJECTILE_SUPPPLIER_ACTION = 0x102,
         REF_MESSAGE_TYPE_WARNING_DATA = 0x104,
         REF_MESSAGE_TYPE_DART_INFO = 0x105,
 
@@ -99,7 +98,6 @@ class RefSerial : public DJISerial, public RefSerialData
         REF_MESSAGE_TYPE_POWER_AND_HEAT = 0x202,
         REF_MESSAGE_TYPE_ROBOT_POSITION = 0x203,
         REF_MESSAGE_TYPE_ROBOT_BUFF_STATUS = 0x204,
-        REF_MESSAGE_TYPE_AERIAL_ENERGY_STATUS = 0x205,
         REF_MESSAGE_TYPE_RECEIVE_DAMAGE = 0x206,
         REF_MESSAGE_TYPE_PROJECTILE_LAUNCH = 0x207,
         REF_MESSAGE_TYPE_BULLETS_REMAIN = 0x208,
@@ -111,9 +109,14 @@ class RefSerial : public DJISerial, public RefSerialData
         REF_MESSAGE_TYPE_RADAR_INFO = 0x20E,
 
         REF_MESSAGE_TYPE_CUSTOM_DATA = 0x301,
-        // REF_MESSAGE_TYPE_CUSTOM_CONTROLLER = 0x302,
-        // REF_MESSAGE_TYPE_SMALL_MAP = 0x303,
-        // REF_MESSAGE_TYPE_VTM_INPUT_DATA = 0x304
+        // REF_MESSAGE_TYPE_CUSTOM_CONTROLLER_DATA_RECEIVE = 0x302,
+        // REF_MESSAGE_TYPE_SMALL_MAP_INTERACTION = 0x303,
+        // REF_MESSAGE_TYPE_VTM_INPUT_DATA = 0x304,
+        // REF_MESSAGE_TYPE_RADAR_MINIMAP = 0x305,
+        // REF_MESSAGE_TYPE_CUSTOM_CONTROLLER_DATA_SEND = 0x306,
+        // REF_MESSAGE_TYPE_SENTRY_SMALL_MAP = 0x307,
+        // REF_MESSAGE_TYPE_ROBOT_SMALL_MAP = 0x308,
+        // REF_MESSAGE_TYPE_ROBOT_CUSTOM_CONTROLLER_FEEDBACK = 0x309
     };
 
     /**
diff --git a/ut-robomaster/taproot/src/tap/communication/serial/ref_serial_data.hpp b/ut-robomaster/taproot/src/tap/communication/serial/ref_serial_data.hpp
index 2d8c401a..8bae42f0 100644
--- a/ut-robomaster/taproot/src/tap/communication/serial/ref_serial_data.hpp
+++ b/ut-robomaster/taproot/src/tap/communication/serial/ref_serial_data.hpp
@@ -117,10 +117,11 @@ class RefSerialData
 
         enum class SiteDartHit : uint8_t
         {
-            NONE = 0,        ///< No hit target.
-            OUTPOST = 1,     ///< Outpost hit.
-            BASE_FIXED = 2,  ///< Fixed target hit.
-            BASE_RANDOM = 3  ///< Random target hit.
+            NONE = 0,               ///< No hit target.
+            OUTPOST = 1,            ///< Outpost hit.
+            BASE_FIXED = 2,         ///< Fixed target hit.
+            BASE_RANDOM_FIXED = 3,  ///< Fixed target hit after random movement.
+            BASE_RANDOM_MOVING = 4  ///< Random moving target hit.
         };
 
         enum class SupplierOutletStatus : uint8_t
@@ -158,25 +159,20 @@ class RefSerialData
 
         enum class SiteData : uint32_t
         {
-            RESTORATION_FRONT_OCCUPIED = modm::Bit0,
-            RESTORATION_INSIDE_OCCUPIED = modm::Bit1,
+            RESUPPLY_OUTSIDE_EXCHANGE_OCCUPIED = modm::Bit0,
+            RESUPPLY_INSIDE_EXCHANGE_OCCUPIED = modm::Bit1,
             SUPPLIER_OCCUPIED = modm::Bit2,
 
-            POWER_RUNE_OCCUPIED = modm::Bit3,
-            SMALL_POWER_RUNE_ACTIVATED = modm::Bit4,
-            LARGER_POWER_RUNE_ACTIVIATED = modm::Bit5,
+            SMALL_POWER_RUNE_ACTIVATED = modm::Bit3,
+            LARGER_POWER_RUNE_ACTIVIATED = modm::Bit4,
 
-            RING_OCCUPIED_TEAM = modm::Bit6,
-            RING_OCCUPIED_OPPONENT = modm::Bit7,
+            CENTRAL_ELEVATED_GROUND_OCCUPIED_TEAM = modm::Bit5,
+            CENTRAL_ELEVATED_GROUND_OCCUPIED_OPPONENT = modm::Bit6,
 
-            TRAPEZOID_R3_OCCUPIED_TEAM = modm::Bit8,
-            TRAPEZOID_R3_OCCUPIED_OPPONENT = modm::Bit9,
+            TRAPEZOID_OCCUPIED_TEAM = modm::Bit7,
 
-            TRAPEZOID_R4_OCCUPIED_TEAM = modm::Bit10,
-            TRAPEZOID_R4_OCCUPIED_OPPONENT = modm::Bit11,
-
-            CENTRAL_BUFF_OCCUPIED_TEAM = modm::Bit30,
-            CENTRAL_BUFF_OCCUPIED_OPPONENT = modm::Bit31
+            CENTRAL_BUFF_OCCUPIED_TEAM = modm::Bit21,
+            CENTRAL_BUFF_OCCUPIED_OPPONENT = modm::Bit22
         };
         MODM_FLAGS32(SiteData);
 
@@ -188,29 +184,43 @@ class RefSerialData
         };
         MODM_FLAGS8(RobotPower);
 
+        enum class RobotEnergyLevel : uint8_t
+        {
+            ABOVE_50_PERCENT = 0x32,
+            ABOVE_30_PERCENT = 0b11110,
+            ABOVE_15_PERCENT = 0b11100,
+            ABOVE_5_PERCENT = 0b11000,
+            ABOVE_1_PERCENT = 0b10000,
+            BELOW_1_PERCENT = 0b00000,
+        };
+
         /// Activation status flags for the RFID module (for RMUC only).
         enum class RFIDActivationStatus : uint32_t
         {
             BASE_BUFF = modm::Bit0,
-            ELEVATED_RING_OWN = modm::Bit1,
-            ELEVATED_RING_OPPONENT = modm::Bit2,
-            TRAPEZOID_R3_OWN = modm::Bit3,
-            TRAPEZOID_R3_OPPONENT = modm::Bit4,
-            TRAPEZOID_R4_OWN = modm::Bit5,
-            TRAPEZOID_R4_OPPONENT = modm::Bit6,
-            POWER_RUNE_ACTIVATION = modm::Bit7,
-            LAUNCH_RAMP_FRONT_OWN = modm::Bit8,
-            LAUNCH_RAMP_BACK_OWN = modm::Bit9,
-            LAUNCH_RAMP_FRONT_OPPONENT = modm::Bit10,
-            LAUNCH_RAMP_BACK_OPPONENT = modm::Bit11,
-            OUTPOST_BUFF = modm::Bit12,
-            RESTORATION_ZONE = modm::Bit13,
-            SENTRY_PATROL_OWN = modm::Bit14,
-            SENTRY_PATROL_OPPONENT = modm::Bit15,
-            LARGE_ISLAND_OWN = modm::Bit16,
-            LARGE_ISLAND_OPPONENT = modm::Bit17,
-            EXCHANGE_ZONE = modm::Bit18,
-            CENTRAL_BUFF = modm::Bit19
+            CENTRAL_ELEVATED_GROUND_OWN = modm::Bit1,
+            CENTRAL_ELEVATED_GROUND_OPPONENT = modm::Bit2,
+            TRAPEZOID_OWN = modm::Bit3,
+            TRAPEZOID_OPPONENT = modm::Bit4,
+            LAUNCH_RAMP_FRONT_OWN = modm::Bit5,
+            LAUNCH_RAMP_BACK_OWN = modm::Bit6,
+            LAUNCH_RAMP_FRONT_OPPONENT = modm::Bit7,
+            LAUNCH_RAMP_BACK_OPPONENT = modm::Bit8,
+            TERRAIN_CROSSING_OFF_CENTRAL_OWN = modm::Bit9,
+            TERRAIN_CROSSING_ON_CENTRAL_OWN = modm::Bit10,
+            TERRAIN_CROSSING_OFF_CENTRAL_OPPONENT = modm::Bit11,
+            TERRAIN_CROSSING_ON_CENTRAL_OPPONENT = modm::Bit12,
+            TERRAIN_CROSSING_OFF_ROAD_OWN = modm::Bit13,
+            TERRAIN_CROSSING_ON_ROAD_OWN = modm::Bit14,
+            TERRAIN_CROSSING_OFF_ROAD_OPPONENT = modm::Bit15,
+            TERRAIN_CROSSING_ON_ROAD_OPPONENT = modm::Bit16,
+            FORTRESS_BUFF_OWN = modm::Bit17,
+            OUTPOST_BUFF_OWN = modm::Bit18,
+            RESUPPLY_ZONE_OUTSIDE_EXCHANGE = modm::Bit19,
+            RESUPPLY_ZONE_INSIDE_EXCHANGE = modm::Bit20,
+            LARGE_RESOURCE_ISLAND_OWN = modm::Bit21,
+            LARGE_RESOURCE_ISLAND_OPPONENT = modm::Bit22,
+            CENTRAL_BUFF = modm::Bit23
         };
         MODM_FLAGS32(RFIDActivationStatus);
 
@@ -240,7 +250,7 @@ class RefSerialData
         /**
          * The Maximum launch speed for a 17mm barrel in m/s.
          */
-        static constexpr int MAX_LAUNCH_SPEED_17MM = 30;
+        static constexpr int MAX_LAUNCH_SPEED_17MM = 25;
 
         /**
          * The Maximum launch speed for a 42mm barrel in m/s.
@@ -258,7 +268,6 @@ class RefSerialData
                 uint16_t engineer2;
                 uint16_t standard3;
                 uint16_t standard4;
-                uint16_t standard5;
                 uint16_t sentry7;
                 uint16_t outpost;
                 uint16_t base;
@@ -273,9 +282,7 @@ class RefSerialData
          */
         struct EventData
         {
-            SiteData_t siteData;  ///< Information about occupied zones.
-            uint8_t
-                virtualShieldRemainingPercent;  ///< Remain percent on own base's virtual shield.
+            SiteData_t siteData;           ///< Information about occupied zones.
             uint8_t timeSinceLastDartHit;  ///< Time since the last dart hit own outpost or base.
             SiteDartHit lastDartHit;       ///< The target hit by the last dart.
         };
@@ -301,9 +308,6 @@ class RefSerialData
 
         struct ChassisData
         {
-            uint16_t volt;                   ///< Output voltage to the chassis (in mV).
-            uint16_t current;                ///< Output current to the chassis (in mA).
-            float power;                     ///< Output power to the chassis (in W).
             uint16_t powerBuffer;            ///< Chassis power buffer (in J).
             RobotPosition position;          ///< x, y coordinate of the chassis (in m).
             uint16_t powerConsumptionLimit;  ///< The current chassis power limit (in W).
@@ -415,7 +419,6 @@ class RefSerialData
             RobotPosition engineer;
             RobotPosition standard3;
             RobotPosition standard4;
-            RobotPosition standard5;
         };
 
         /**
@@ -427,7 +430,6 @@ class RefSerialData
             uint8_t engineer;
             uint8_t standard3;
             uint8_t standard4;
-            uint8_t standard5;
             uint8_t sentry;
         };
 
@@ -490,6 +492,7 @@ class RefSerialData
                                                     ///< received.
             RefereeWarningData refereeWarningData;  ///< Referee warning information, updated when
                                                     ///< a robot receives a penalty
+            RobotEnergyLevel robotEnergyRemaining;  ///< The current energy level of the robot.
         };
     };
 
diff --git a/ut-robomaster/taproot/src/tap/communication/tcp-server/json_messages.cpp b/ut-robomaster/taproot/src/tap/communication/tcp-server/json_messages.cpp
index abefc6ce..aeb4d635 100644
--- a/ut-robomaster/taproot/src/tap/communication/tcp-server/json_messages.cpp
+++ b/ut-robomaster/taproot/src/tap/communication/tcp-server/json_messages.cpp
@@ -54,9 +54,11 @@ string makeMotorMessage(const tap::motor::DjiMotor& motor)
         "\"canBus\":" +
         std::to_string(static_cast<int>(motor.getCanBus()) + 1) + "," +
         "\"motorID\":" + std::to_string(motor.getMotorIdentifier()) + "," +
-        "\"shaftRPM\":" + std::to_string(motor.getShaftRPM()) + "," +
-        "\"torque\":" + std::to_string(motor.getTorque()) + "," +
-        "\"encoderValue\":" + std::to_string(motor.getEncoderUnwrapped()) + "}";
+        "\"shaftRPM\":" + std::to_string(motor.getInternalEncoder().getShaftRPM()) + "," +
+        "\"torque\":" + std::to_string(motor.getTorque()) + "," + "\"encoderValue\":" +
+        std::to_string(
+            static_cast<uint64_t>(motor.getInternalEncoder().getEncoder().getWrappedValue())) +
+        "}";
     return jsonMessage;
 }
 
diff --git a/ut-robomaster/taproot/src/tap/communication/tcp-server/json_messages.hpp b/ut-robomaster/taproot/src/tap/communication/tcp-server/json_messages.hpp
index d180a095..e32b5895 100644
--- a/ut-robomaster/taproot/src/tap/communication/tcp-server/json_messages.hpp
+++ b/ut-robomaster/taproot/src/tap/communication/tcp-server/json_messages.hpp
@@ -28,10 +28,12 @@
 
 #include <string>
 
-#include "tap/motor/dji_motor.hpp"
-
 namespace tap
 {
+namespace motor
+{
+class DjiMotor;
+}
 namespace communication
 {
 /**
diff --git a/ut-robomaster/taproot/src/tap/communication/tcp-server/tcp_server.cpp b/ut-robomaster/taproot/src/tap/communication/tcp-server/tcp_server.cpp
index c8903c48..726525ef 100644
--- a/ut-robomaster/taproot/src/tap/communication/tcp-server/tcp_server.cpp
+++ b/ut-robomaster/taproot/src/tap/communication/tcp-server/tcp_server.cpp
@@ -39,6 +39,8 @@
 #include <iostream>
 #include <stdexcept>
 
+#include "tap/util_macros.hpp"
+
 #include "json_messages.hpp"
 using std::cerr;
 /**
diff --git a/ut-robomaster/taproot/src/tap/control/chassis/chassis_subsystem_interface.hpp b/ut-robomaster/taproot/src/tap/control/chassis/chassis_subsystem_interface.hpp
index 14657052..15987f95 100644
--- a/ut-robomaster/taproot/src/tap/control/chassis/chassis_subsystem_interface.hpp
+++ b/ut-robomaster/taproot/src/tap/control/chassis/chassis_subsystem_interface.hpp
@@ -25,7 +25,6 @@
 #define TAPROOT_CHASSIS_SUBSYSTEM_INTERFACE_HPP_
 
 #include "tap/algorithms/math_user_utils.hpp"
-#include "tap/motor/dji_motor.hpp"
 
 #include "../subsystem.hpp"
 #include "modm/math/matrix.hpp"
diff --git a/ut-robomaster/taproot/src/tap/control/chassis/power_limiter.cpp b/ut-robomaster/taproot/src/tap/control/chassis/power_limiter.cpp
index 203b6c82..d31272d7 100644
--- a/ut-robomaster/taproot/src/tap/control/chassis/power_limiter.cpp
+++ b/ut-robomaster/taproot/src/tap/control/chassis/power_limiter.cpp
@@ -35,11 +35,13 @@ namespace tap::control::chassis
 PowerLimiter::PowerLimiter(
     const tap::Drivers *drivers,
     tap::communication::sensors::current::CurrentSensorInterface *currentSensor,
+    tap::communication::sensors::voltage::VoltageSensorInterface *voltageSensor,
     float startingEnergyBuffer,
     float energyBufferLimitThreshold,
     float energyBufferCritThreshold)
     : drivers(drivers),
       currentSensor(currentSensor),
+      voltageSensor(voltageSensor),
       startingEnergyBuffer(startingEnergyBuffer),
       energyBufferLimitThreshold(energyBufferLimitThreshold),
       energyBufferCritThreshold(energyBufferCritThreshold),
@@ -79,8 +81,9 @@ void PowerLimiter::updatePowerAndEnergyBuffer()
 {
     const auto &robotData = drivers->refSerial.getRobotData();
     const auto &chassisData = robotData.chassis;
-    const float current = currentSensor->getCurrentMa();
-    const float newChassisPower = chassisData.volt * current / 1'000'000.0f;
+    const float current = currentSensor->getCurrentMa() / 1000.0f;
+    const float voltage = voltageSensor->getVoltageMv() / 1000.0f;
+    const float newChassisPower = voltage * current;
 
     // Manually compute energy buffer using consumedPower read from current sensor.
     // See rules manual for reasoning behind the energy buffer calculation.
diff --git a/ut-robomaster/taproot/src/tap/control/chassis/power_limiter.hpp b/ut-robomaster/taproot/src/tap/control/chassis/power_limiter.hpp
index 4966dcaf..f3cd6a0f 100644
--- a/ut-robomaster/taproot/src/tap/control/chassis/power_limiter.hpp
+++ b/ut-robomaster/taproot/src/tap/control/chassis/power_limiter.hpp
@@ -26,6 +26,7 @@
 
 #include "tap/communication/gpio/analog.hpp"
 #include "tap/communication/sensors/current/current_sensor_interface.hpp"
+#include "tap/communication/sensors/voltage/voltage_sensor_interface.hpp"
 
 namespace tap
 {
@@ -80,6 +81,7 @@ class PowerLimiter
     PowerLimiter(
         const tap::Drivers *drivers,
         tap::communication::sensors::current::CurrentSensorInterface *currentSensor,
+        tap::communication::sensors::voltage::VoltageSensorInterface *voltageSensor,
         float startingEnergyBuffer,
         float energyBufferLimitThreshold,
         float energyBufferCritThreshold);
@@ -102,6 +104,7 @@ class PowerLimiter
 private:
     const tap::Drivers *drivers;
     tap::communication::sensors::current::CurrentSensorInterface *currentSensor;
+    tap::communication::sensors::voltage::VoltageSensorInterface *voltageSensor;
     const float startingEnergyBuffer;
     const float energyBufferLimitThreshold;
     const float energyBufferCritThreshold;
diff --git a/ut-robomaster/taproot/src/tap/control/setpoint/commands/calibrate_command.hpp b/ut-robomaster/taproot/src/tap/control/setpoint/commands/calibrate_command.hpp
index fba45d7d..f7b00889 100644
--- a/ut-robomaster/taproot/src/tap/control/setpoint/commands/calibrate_command.hpp
+++ b/ut-robomaster/taproot/src/tap/control/setpoint/commands/calibrate_command.hpp
@@ -25,7 +25,6 @@
 #define TAPROOT_CALIBRATE_COMMAND_HPP_
 
 #include "tap/control/command.hpp"
-#include "tap/drivers.hpp"
 
 namespace tap
 {
diff --git a/ut-robomaster/taproot/src/tap/control/setpoint/commands/move_command.hpp b/ut-robomaster/taproot/src/tap/control/setpoint/commands/move_command.hpp
index 97af1c09..924a749a 100644
--- a/ut-robomaster/taproot/src/tap/control/setpoint/commands/move_command.hpp
+++ b/ut-robomaster/taproot/src/tap/control/setpoint/commands/move_command.hpp
@@ -29,7 +29,6 @@
 #include "tap/architecture/timeout.hpp"
 #include "tap/control/command.hpp"
 #include "tap/control/setpoint/interfaces/setpoint_subsystem.hpp"
-#include "tap/drivers.hpp"
 
 #include "modm/math/filter/pid.hpp"
 
diff --git a/ut-robomaster/taproot/src/tap/control/setpoint/commands/unjam_command.hpp b/ut-robomaster/taproot/src/tap/control/setpoint/commands/unjam_command.hpp
index e3a6d973..9071f9a0 100644
--- a/ut-robomaster/taproot/src/tap/control/setpoint/commands/unjam_command.hpp
+++ b/ut-robomaster/taproot/src/tap/control/setpoint/commands/unjam_command.hpp
@@ -29,8 +29,6 @@
 #include "tap/algorithms/ramp.hpp"
 #include "tap/architecture/timeout.hpp"
 #include "tap/control/command.hpp"
-#include "tap/drivers.hpp"
-#include "tap/motor/dji_motor.hpp"
 
 namespace tap
 {
diff --git a/ut-robomaster/taproot/src/tap/motor/dji_motor.cpp b/ut-robomaster/taproot/src/tap/motor/dji_motor.cpp
index cb424896..5b1adb59 100644
--- a/ut-robomaster/taproot/src/tap/motor/dji_motor.cpp
+++ b/ut-robomaster/taproot/src/tap/motor/dji_motor.cpp
@@ -47,21 +47,26 @@ DjiMotor::DjiMotor(
     tap::can::CanBus motorCanBus,
     bool isInverted,
     const char* name,
-    uint16_t encoderWrapped,
-    int64_t encoderRevolutions)
+    bool currentControl,
+    float gearRatio,
+    uint32_t encoderHomePosition,
+    tap::encoder::EncoderInterface* externalEncoder)
     : CanRxListener(drivers, static_cast<uint32_t>(desMotorIdentifier), motorCanBus),
       motorName(name),
       drivers(drivers),
       motorIdentifier(desMotorIdentifier),
       motorCanBus(motorCanBus),
       desiredOutput(0),
-      shaftRPM(0),
       temperature(0),
       torque(0),
       motorInverted(isInverted),
-      encoderWrapped(encoderWrapped),
-      encoderRevolutions(encoderRevolutions),
-      encoderHomePosition(0)
+      currentControl(currentControl),
+      internalEncoder(isInverted, gearRatio, encoderHomePosition),
+      encoder(
+          {externalEncoder != nullptr ? externalEncoder
+                                      : const_cast<Encoder*>(&this->getInternalEncoder()),
+           externalEncoder != nullptr ? const_cast<Encoder*>(&this->getInternalEncoder())
+                                      : nullptr})
 {
     motorDisconnectTimeout.stop();
 }
@@ -70,6 +75,7 @@ void DjiMotor::initialize()
 {
     drivers->djiMotorTxHandler.addMotorToManager(this);
     attachSelfToRxHandler();
+    this->encoder.initialize();
 }
 
 void DjiMotor::processMessage(const modm::can::Message& message)
@@ -78,10 +84,6 @@ void DjiMotor::processMessage(const modm::can::Message& message)
     {
         return;
     }
-    uint16_t encoderActual =
-        static_cast<uint16_t>(message.data[0] << 8 | message.data[1]);        // encoder value
-    shaftRPM = static_cast<int16_t>(message.data[2] << 8 | message.data[3]);  // rpm
-    shaftRPM = motorInverted ? -shaftRPM : shaftRPM;
     torque = static_cast<int16_t>(message.data[4] << 8 | message.data[5]);  // torque
     torque = motorInverted ? -torque : torque;
     temperature = static_cast<int8_t>(message.data[6]);  // temperature
@@ -89,14 +91,7 @@ void DjiMotor::processMessage(const modm::can::Message& message)
     // restart disconnect timer, since you just received a message from the motor
     motorDisconnectTimeout.restart(MOTOR_DISCONNECT_TIME);
 
-    // invert motor if necessary
-    encoderActual = motorInverted ? ENC_RESOLUTION - 1 - encoderActual : encoderActual;
-
-    int32_t encoderRelativeToHome = (int32_t)encoderActual - (int32_t)encoderHomePosition;
-
-    updateEncoderValue(
-        encoderRelativeToHome < 0 ? (int32_t)ENC_RESOLUTION + encoderRelativeToHome
-                                  : encoderRelativeToHome);
+    this->internalEncoder.processMessage(message);
 }
 
 void DjiMotor::setDesiredOutput(int32_t desiredOutput)
@@ -136,52 +131,14 @@ int8_t DjiMotor::getTemperature() const { return temperature; }
 
 int16_t DjiMotor::getTorque() const { return torque; }
 
-int16_t DjiMotor::getShaftRPM() const { return shaftRPM; }
-
 bool DjiMotor::isMotorInverted() const { return motorInverted; }
 
 tap::can::CanBus DjiMotor::getCanBus() const { return motorCanBus; }
 
 const char* DjiMotor::getName() const { return motorName; }
 
-int64_t DjiMotor::getEncoderUnwrapped() const
-{
-    return static_cast<int64_t>(encoderWrapped) +
-           static_cast<int64_t>(ENC_RESOLUTION) * encoderRevolutions;
-}
-
-uint16_t DjiMotor::getEncoderWrapped() const { return encoderWrapped; }
+bool DjiMotor::isInCurrentControl() const { return currentControl; }
 
-void DjiMotor::resetEncoderValue()
-{
-    encoderRevolutions = 0;
-    encoderHomePosition = (encoderWrapped + encoderHomePosition) % ENC_RESOLUTION;
-    encoderWrapped = 0;
-}
-
-float DjiMotor::getPositionUnwrapped() const
-{
-    return getEncoderUnwrapped() * M_TWOPI / ENC_RESOLUTION;
-}
-
-float DjiMotor::getPositionWrapped() const
-{
-    return getEncoderWrapped() * M_TWOPI / ENC_RESOLUTION;
-}
-
-void DjiMotor::updateEncoderValue(uint16_t newEncWrapped)
-{
-    int16_t enc_dif = newEncWrapped - encoderWrapped;
-    if (enc_dif < -ENC_RESOLUTION / 2)
-    {
-        encoderRevolutions++;
-    }
-    else if (enc_dif > ENC_RESOLUTION / 2)
-    {
-        encoderRevolutions--;
-    }
-    encoderWrapped = newEncWrapped;
-}
 }  // namespace motor
 
 }  // namespace tap
diff --git a/ut-robomaster/taproot/src/tap/motor/dji_motor.hpp b/ut-robomaster/taproot/src/tap/motor/dji_motor.hpp
index 3b970524..6d35324c 100644
--- a/ut-robomaster/taproot/src/tap/motor/dji_motor.hpp
+++ b/ut-robomaster/taproot/src/tap/motor/dji_motor.hpp
@@ -28,30 +28,27 @@
 
 #include "tap/architecture/timeout.hpp"
 #include "tap/communication/can/can_rx_listener.hpp"
+#include "tap/communication/sensors/encoder/multi_encoder.hpp"
 
 #include "modm/math/geometry/angle.hpp"
 
+#include "dji_motor_encoder.hpp"
+#include "dji_motor_ids.hpp"
 #include "motor_interface.hpp"
 
+#if defined(PLATFORM_HOSTED) && defined(ENV_UNIT_TESTS)
+#include <gmock/gmock.h>
+
+#include "tap/mock/dji_motor_encoder_mock.hpp"
+#endif
+
 namespace tap::motor
 {
-/**
- * CAN IDs for the feedback messages sent by DJI motor controllers. Motor `i` in the set
- * {1, 2,...,8} sends feedback data with in a CAN message with ID 0x200 + `i`.
- * for declaring a new motor, must be one of these motor
- * identifiers
- */
-enum MotorId : uint32_t
-{
-    MOTOR1 = 0X201,
-    MOTOR2 = 0x202,
-    MOTOR3 = 0x203,
-    MOTOR4 = 0x204,
-    MOTOR5 = 0x205,
-    MOTOR6 = 0x206,
-    MOTOR7 = 0x207,
-    MOTOR8 = 0x208,
-};
+#if defined(PLATFORM_HOSTED) && defined(ENV_UNIT_TESTS)
+using Encoder = tap::mock::DjiMotorEncoderMock;
+#else
+using Encoder = DjiMotorEncoder;
+#endif
 
 /**
  * A class designed to interface with DJI brand motors and motor controllers over CAN.
@@ -62,11 +59,7 @@ enum MotorId : uint32_t
  *      == false`) is counter clockwise when looking at the shaft from the side opposite
  *      the motor. This is specified in the C620 user manual (page 18).
  *
- * DJI motor encoders store a consistent encoding for a given angle across power-cycles.
- * This means the encoder angle reported by the motor can have meaning if the encoding
- * for an angle is unique as it is for the GM6020s. However for geared motors like the
- * M3508 where a full encoder revolution does not correspond 1:1 to a shaft revolution,
- * it is impossible to know the orientation of the shaft given just the encoder value.
+ * @see DjiMotorEncoder
  *
  * Extends the CanRxListener class to attach a message handler for feedback data from the
  * motor to the CAN Rx dispatch handler.
@@ -77,9 +70,6 @@ enum MotorId : uint32_t
 class DjiMotor : public can::CanRxListener, public MotorInterface
 {
 public:
-    // 0 - 8191 for dji motors
-    static constexpr uint16_t ENC_RESOLUTION = 8192;
-
     // Maximum values for following motors
     // Controller for the M2006, in mA output
     static constexpr uint16_t MAX_OUTPUT_C610 = 10000;
@@ -93,14 +83,6 @@ class DjiMotor : public can::CanRxListener, public MotorInterface
     // Output is in mV
     static constexpr uint16_t MAX_OUTPUT_GM3510 = 29000;
 
-    // Internal gear ratio of the following motors
-    static constexpr float GEAR_RATIO_M3508 = 3591.0f / 187.0f;
-    static constexpr float GEAR_RATIO_M3510_L1 = 3.7f / 1.0f;
-    static constexpr float GEAR_RATIO_M3510_L2 = 5.2f / 1.0f;
-    static constexpr float GEAR_RATIO_M3510_L3 = 19.0f / 1.0f;
-    static constexpr float GEAR_RATIO_M3510_L4 = 27.0f / 1.0f;
-    static constexpr float GEAR_RATIO_M2006 = 36.0f / 1.0f;
-
     /**
      * @param drivers a pointer to the drivers struct
      * @param desMotorIdentifier the ID of this motor controller
@@ -109,10 +91,9 @@ class DjiMotor : public can::CanRxListener, public MotorInterface
      *      counter-clockwise when looking at the shaft from the side opposite the motor.
      *      If `true` then the positive rotation direction will be clockwise.
      * @param name a name to associate with the motor for use in the motor menu
-     * @param encoderWrapped the starting encoderValue to store for this motor.
-     *      Will be overwritten by the first reported encoder value from the motor
-     * @param encoderRevolutions the starting number of encoder revolutions to store.
-     *      See comment for DjiMotor::encoderRevolutions for more details.
+     * @param gearRatio the ratio of input revolutions to output revolutions of this encoder.
+     * @param encoderHomePosition the zero position for the encoder in encoder ticks.
+     * @param externalEncoder a pointer to an external encoder to average with the internal encoder.
      */
     DjiMotor(
         Drivers* drivers,
@@ -120,26 +101,24 @@ class DjiMotor : public can::CanRxListener, public MotorInterface
         tap::can::CanBus motorCanBus,
         bool isInverted,
         const char* name,
-        uint16_t encoderWrapped = ENC_RESOLUTION / 2,
-        int64_t encoderRevolutions = 0);
+        bool currentControl = false,
+        float gearRatio = 1,
+        uint32_t encoderHomePosition = 0,
+        tap::encoder::EncoderInterface* externalEncoder = nullptr);
 
     mockable ~DjiMotor();
 
     void initialize() override;
 
-    float getPositionUnwrapped() const override;
-
-    float getPositionWrapped() const override;
-
-    int64_t getEncoderUnwrapped() const override;
-
-    uint16_t getEncoderWrapped() const override;
+    tap::encoder::EncoderInterface* getEncoder() const override
+    {
+        return const_cast<tap::encoder::MultiEncoder<2>*>(&this->encoder);
+    }
 
     /**
-     * Resets this motor's current encoder home position to the current encoder position reported by
-     * CAN messages, and resets this motor's encoder revolutions to 0.
+     * Returns the builtin encoder associated with the motor.
      */
-    void resetEncoderValue() override;
+    mockable const Encoder& getInternalEncoder() const { return this->internalEncoder; }
 
     DISALLOW_COPY_AND_ASSIGN(DjiMotor)
 
@@ -190,37 +169,13 @@ class DjiMotor : public can::CanRxListener, public MotorInterface
 
     int16_t getTorque() const override;
 
-    /// For interpreting the sign of return value see class comment
-    int16_t getShaftRPM() const override;
-
     mockable bool isMotorInverted() const;
 
     mockable tap::can::CanBus getCanBus() const;
 
     mockable const char* getName() const;
 
-    template <typename T>
-    static void assertEncoderType()
-    {
-        constexpr bool good_type =
-            std::is_same<typename std::decay<T>::type, std::int64_t>::value ||
-            std::is_same<typename std::decay<T>::type, std::uint16_t>::value;
-        static_assert(good_type, "x is not of the correct type");
-    }
-
-    template <typename T>
-    static T degreesToEncoder(float angle)
-    {
-        assertEncoderType<T>();
-        return static_cast<T>((ENC_RESOLUTION * angle) / 360);
-    }
-
-    template <typename T>
-    static float encoderToDegrees(T encoder)
-    {
-        assertEncoderType<T>();
-        return (360.0f * static_cast<float>(encoder)) / ENC_RESOLUTION;
-    }
+    mockable bool isInCurrentControl() const;
 
 private:
     // wait time before the motor is considered disconnected, in milliseconds
@@ -228,12 +183,6 @@ class DjiMotor : public can::CanRxListener, public MotorInterface
 
     const char* motorName;
 
-    /**
-     * Updates the stored encoder value given a newly received encoder value
-     * special logic necessary for keeping track of unwrapped encoder value.
-     */
-    void updateEncoderValue(uint16_t newEncWrapped);
-
     Drivers* drivers;
 
     uint32_t motorIdentifier;
@@ -242,8 +191,6 @@ class DjiMotor : public can::CanRxListener, public MotorInterface
 
     int16_t desiredOutput;
 
-    int16_t shaftRPM;
-
     int8_t temperature;
 
     int16_t torque;
@@ -255,26 +202,15 @@ class DjiMotor : public can::CanRxListener, public MotorInterface
      */
     bool motorInverted;
 
-    /**
-     * The raw encoder value reported by the motor controller relative to
-     * encoderHomePosition. It wraps around from {0..8191}, hence "Wrapped"
-     */
-    uint16_t encoderWrapped;
+    bool currentControl;
 
-    /**
-     * Absolute unwrapped encoder position =
-     *      encoderRevolutions * ENCODER_RESOLUTION + encoderWrapped
-     * This lets us keep track of some sense of absolute position even while
-     * raw encoderValue continuosly loops within {0..8191}. Origin value is
-     * arbitrary.
-     */
-    int64_t encoderRevolutions;
+#if defined(PLATFORM_HOSTED) && defined(ENV_UNIT_TESTS)
+    testing::NiceMock<Encoder> internalEncoder;
+#else
+    Encoder internalEncoder;
+#endif
 
-    /**
-     * The actual encoder wrapped value received from CAN messages where this motor
-     * is considered to have an encoder value of 0. encoderHomePosition is 0 by default.
-     */
-    uint16_t encoderHomePosition;
+    tap::encoder::MultiEncoder<2> encoder;
 
     tap::arch::MilliTimeout motorDisconnectTimeout;
 };
diff --git a/ut-robomaster/taproot/src/tap/motor/dji_motor_encoder.cpp b/ut-robomaster/taproot/src/tap/motor/dji_motor_encoder.cpp
new file mode 100644
index 00000000..40243930
--- /dev/null
+++ b/ut-robomaster/taproot/src/tap/motor/dji_motor_encoder.cpp
@@ -0,0 +1,69 @@
+/*****************************************************************************/
+/********** !!! WARNING: CODE GENERATED BY TAPROOT. DO NOT EDIT !!! **********/
+/*****************************************************************************/
+
+/*
+ * Copyright (c) 2024 Advanced Robotics at the University of Washington <robomstr@uw.edu>
+ *
+ * This file is part of Taproot.
+ *
+ * Taproot is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Taproot is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Taproot.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+#include "dji_motor_encoder.hpp"
+
+#include "tap/algorithms/math_user_utils.hpp"
+
+namespace tap
+{
+namespace motor
+{
+DjiMotorEncoder::DjiMotorEncoder(bool isInverted, float gearRatio, uint32_t encoderHomePosition)
+    : WrappedEncoder(isInverted, ENC_RESOLUTION, gearRatio, encoderHomePosition),
+      shaftRPM(0)
+{
+    encoderDisconnectTimeout.stop();
+}
+
+void DjiMotorEncoder::processMessage(const modm::can::Message& message)
+{
+    encoderDisconnectTimeout.restart(MOTOR_DISCONNECT_TIME);
+    shaftRPM = static_cast<int16_t>(message.data[2] << 8 | message.data[3]);  // rpm
+    shaftRPM = inverted ? -shaftRPM : shaftRPM;
+
+    uint16_t encoderActual =
+        static_cast<uint16_t>(message.data[0] << 8 | message.data[1]);  // encoder value
+
+    updateEncoderValue(encoderActual);
+}
+
+bool DjiMotorEncoder::isOnline() const
+{
+    /*
+     * motor online if the disconnect timout has not expired (if it received message but
+     * somehow got disconnected) and the timeout hasn't been stopped (initially, the timeout
+     * is stopped)
+     */
+    return !encoderDisconnectTimeout.isExpired() && !encoderDisconnectTimeout.isStopped();
+}
+
+float DjiMotorEncoder::getVelocity() const
+{
+    return this->getShaftRPM() * static_cast<float>(M_TWOPI) / 60.f * this->gearRatio;
+}
+
+int16_t DjiMotorEncoder::getShaftRPM() const { return shaftRPM; }
+}  // namespace motor
+
+}  // namespace tap
diff --git a/ut-robomaster/taproot/src/tap/motor/dji_motor_encoder.hpp b/ut-robomaster/taproot/src/tap/motor/dji_motor_encoder.hpp
new file mode 100644
index 00000000..4995dcb4
--- /dev/null
+++ b/ut-robomaster/taproot/src/tap/motor/dji_motor_encoder.hpp
@@ -0,0 +1,112 @@
+/*****************************************************************************/
+/********** !!! WARNING: CODE GENERATED BY TAPROOT. DO NOT EDIT !!! **********/
+/*****************************************************************************/
+
+/*
+ * Copyright (c) 2024 Advanced Robotics at the University of Washington <robomstr@uw.edu>
+ *
+ * This file is part of Taproot.
+ *
+ * Taproot is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Taproot is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Taproot.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+#ifndef TAPROOT_DJI_MOTOR_ENCODER_HPP_
+#define TAPROOT_DJI_MOTOR_ENCODER_HPP_
+
+#include "tap/architecture/timeout.hpp"
+#include "tap/communication/sensors/encoder/wrapped_encoder.hpp"
+#include "tap/util_macros.hpp"
+
+#include "modm/architecture/interface/can_message.hpp"
+#include "modm/math/geometry/angle.hpp"
+
+namespace tap::motor
+{
+/**
+ * A class designed to interface with the encoder for DJI brand motors and motor controllers over
+ * CAN. This includes the C610 and C620 motor controllers and the GM6020 motor (that has a built-in
+ * motor controller).
+ *
+ * @note: the default positive rotation direction (i.e.: when `this->isMotorInverted()
+ *      == false`) is counter clockwise when looking at the shaft from the side opposite
+ *      the motor. This is specified in the C620 user manual (page 18).
+ *
+ * DJI motor encoders store a consistent encoding for a given angle across power-cycles.
+ * This means the encoder angle reported by the motor can have meaning if the encoding
+ * for an angle is unique as it is for the GM6020s. However for geared motors like the
+ * M3508 where a full encoder revolution does not correspond 1:1 to a shaft revolution,
+ * it is impossible to know the orientation of the shaft given just the encoder value.
+ *
+ * Combining them with some form of absolute encoder on the output shaft would give you knowledge of
+ * the orientation of the output shaft.
+ */
+class DjiMotorEncoder : public tap::encoder::WrappedEncoder
+{
+public:
+    // 0 - 8191 for dji motors
+    static constexpr uint16_t ENC_RESOLUTION = 8192;
+
+    // Internal gear ratio of the following motors
+    static constexpr float GEAR_RATIO_M3508 = 3591.0f / 187.0f;
+    static constexpr float GEAR_RATIO_GM3510_L1 = 3.7f / 1.0f;
+    static constexpr float GEAR_RATIO_GM3510_L2 = 5.2f / 1.0f;
+    static constexpr float GEAR_RATIO_GM3510_L3 = 19.0f / 1.0f;
+    static constexpr float GEAR_RATIO_GM3510_L4 = 27.0f / 1.0f;
+    static constexpr float GEAR_RATIO_M2006 = 36.0f / 1.0f;
+    static constexpr float GEAR_RATIO_GM6020 = 1.0f / 1.0f;
+
+    /**
+     * @param isInverted if `false` the positive rotation direction of the shaft is
+     *      counter-clockwise when looking at the shaft from.
+     *      If `true` then the positive rotation direction will be clockwise.
+     * @param encoderResolution the number of encoder ticks before the value wraps.
+     * @param gearRatio the ratio of input revolutions to output revolutions of this encoder.
+     * @param encoderHomePosition the zero position for the encoder in encoder ticks.
+     */
+    DjiMotorEncoder(bool isInverted, float gearRatio = 1, uint32_t encoderHomePosition = 0);
+
+    void initialize() override{};
+
+    bool isOnline() const override;
+
+    float getVelocity() const override;
+
+    /**
+     * The current RPM reported by the motor controller.
+     */
+    mockable int16_t getShaftRPM() const;
+
+    DISALLOW_COPY_AND_ASSIGN(DjiMotorEncoder)
+
+    /**
+     * Overrides virtual method in the can class, called every time a message with the
+     * CAN message id this class is attached to is received by the can receive handler.
+     * Parses the data in the message and updates this class's fields accordingly.
+     *
+     * @param[in] message the message to be processed.
+     */
+    mockable void processMessage(const modm::can::Message& message);
+
+private:
+    // wait time before the motor is considered disconnected, in milliseconds
+    static const uint32_t MOTOR_DISCONNECT_TIME = 100;
+
+    tap::arch::MilliTimeout encoderDisconnectTimeout;
+
+    int16_t shaftRPM;
+};
+
+}  // namespace tap::motor
+
+#endif  // TAPROOT_DJI_MOTOR_ENCODER_HPP_
diff --git a/ut-robomaster/taproot/src/tap/motor/dji_motor_ids.hpp b/ut-robomaster/taproot/src/tap/motor/dji_motor_ids.hpp
new file mode 100644
index 00000000..01848237
--- /dev/null
+++ b/ut-robomaster/taproot/src/tap/motor/dji_motor_ids.hpp
@@ -0,0 +1,50 @@
+/*****************************************************************************/
+/********** !!! WARNING: CODE GENERATED BY TAPROOT. DO NOT EDIT !!! **********/
+/*****************************************************************************/
+
+/*
+ * Copyright (c) 2024 Advanced Robotics at the University of Washington <robomstr@uw.edu>
+ *
+ * This file is part of Taproot.
+ *
+ * Taproot is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Taproot is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Taproot.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+#ifndef TAPROOT_DJI_MOTOR_IDS_HPP_
+#define TAPROOT_DJI_MOTOR_IDS_HPP_
+
+#include <cstdint>
+
+namespace tap::motor
+{
+/**
+ * CAN IDs for the feedback messages sent by DJI motor controllers. Motor `i` in the set
+ * {1, 2,...,8} sends feedback data with in a CAN message with ID 0x200 + `i`.
+ * for declaring a new motor, must be one of these motor
+ * identifiers
+ */
+enum MotorId : uint32_t
+{
+    MOTOR1 = 0X201,
+    MOTOR2 = 0x202,
+    MOTOR3 = 0x203,
+    MOTOR4 = 0x204,
+    MOTOR5 = 0x205,
+    MOTOR6 = 0x206,
+    MOTOR7 = 0x207,
+    MOTOR8 = 0x208,
+};
+}  // namespace tap::motor
+
+#endif  // TAPROOT_DJI_MOTOR_IDS_HPP_
diff --git a/ut-robomaster/taproot/src/tap/motor/dji_motor_terminal_serial_handler.cpp b/ut-robomaster/taproot/src/tap/motor/dji_motor_terminal_serial_handler.cpp
index 85d6a1d8..32518082 100644
--- a/ut-robomaster/taproot/src/tap/motor/dji_motor_terminal_serial_handler.cpp
+++ b/ut-robomaster/taproot/src/tap/motor/dji_motor_terminal_serial_handler.cpp
@@ -26,6 +26,7 @@
 #include "tap/algorithms/strtok.hpp"
 #include "tap/drivers.hpp"
 
+#include "dji_motor.hpp"
 #include "dji_motor_tx_handler.hpp"
 
 namespace tap
@@ -190,7 +191,8 @@ void DjiMotorTerminalSerialHandler::getMotorInfoToString(
     {
         outputStream << (DJI_MOTOR_TO_NORMALIZED_ID(motor->getMotorIdentifier()) + 1) << ". "
                      << motor->getName() << ": online: " << (motor->isMotorOnline() ? "yes" : "no")
-                     << ", enc: " << motor->getEncoderWrapped() << ", rpm: " << motor->getShaftRPM()
+                     << ", enc: " << motor->getInternalEncoder().getEncoder().getWrappedValue()
+                     << ", rpm: " << motor->getInternalEncoder().getShaftRPM()
                      << ", out des: " << motor->getOutputDesired() << modm::endl;
     }
 }
diff --git a/ut-robomaster/taproot/src/tap/motor/dji_motor_terminal_serial_handler.hpp b/ut-robomaster/taproot/src/tap/motor/dji_motor_terminal_serial_handler.hpp
index cd887156..7b95afde 100644
--- a/ut-robomaster/taproot/src/tap/motor/dji_motor_terminal_serial_handler.hpp
+++ b/ut-robomaster/taproot/src/tap/motor/dji_motor_terminal_serial_handler.hpp
@@ -27,13 +27,14 @@
 #include "tap/communication/serial/terminal_serial.hpp"
 #include "tap/util_macros.hpp"
 
-#include "dji_motor.hpp"
+#include "dji_motor_ids.hpp"
 
 namespace tap
 {
 class Drivers;
 namespace motor
 {
+class DjiMotor;
 class DjiMotorTxHandler;
 class DjiMotorTerminalSerialHandler : public communication::serial::TerminalSerialCallbackInterface
 {
diff --git a/ut-robomaster/taproot/src/tap/motor/dji_motor_tx_handler.cpp b/ut-robomaster/taproot/src/tap/motor/dji_motor_tx_handler.cpp
index 9de73908..a511fe64 100644
--- a/ut-robomaster/taproot/src/tap/motor/dji_motor_tx_handler.cpp
+++ b/ut-robomaster/taproot/src/tap/motor/dji_motor_tx_handler.cpp
@@ -32,6 +32,8 @@
 #include "modm/architecture/interface/assert.h"
 #include "modm/architecture/interface/can_message.hpp"
 
+#include "dji_motor.hpp"
+
 namespace tap::motor
 {
 void DjiMotorTxHandler::addMotorToManager(DjiMotor** canMotorStore, DjiMotor* const motor)
@@ -72,6 +74,11 @@ void DjiMotorTxHandler::encodeAndSendCanData()
         CAN_DJI_MESSAGE_SEND_LENGTH,
         0,
         false);
+    modm::can::Message can1Message6020Current(
+        CAN_DJI_6020_CURRENT_IDENTIFIER,
+        CAN_DJI_MESSAGE_SEND_LENGTH,
+        0,
+        false);
     modm::can::Message can2MessageLow(
         CAN_DJI_LOW_IDENTIFIER,
         CAN_DJI_MESSAGE_SEND_LENGTH,
@@ -82,25 +89,36 @@ void DjiMotorTxHandler::encodeAndSendCanData()
         CAN_DJI_MESSAGE_SEND_LENGTH,
         0,
         false);
+    modm::can::Message can2Message6020Current(
+        CAN_DJI_6020_CURRENT_IDENTIFIER,
+        CAN_DJI_MESSAGE_SEND_LENGTH,
+        0,
+        false);
 
     bool can1ValidMotorMessageLow = false;
     bool can1ValidMotorMessageHigh = false;
+    bool can1ValidMotorMessage6020Current = false;
     bool can2ValidMotorMessageLow = false;
     bool can2ValidMotorMessageHigh = false;
+    bool can2ValidMotorMessage6020Current = false;
 
     serializeMotorStoreSendData(
         can1MotorStore,
         &can1MessageLow,
         &can1MessageHigh,
+        &can1Message6020Current,
         &can1ValidMotorMessageLow,
-        &can1ValidMotorMessageHigh);
+        &can1ValidMotorMessageHigh,
+        &can1ValidMotorMessage6020Current);
 
     serializeMotorStoreSendData(
         can2MotorStore,
         &can2MessageLow,
         &can2MessageHigh,
+        &can2Message6020Current,
         &can2ValidMotorMessageLow,
-        &can2ValidMotorMessageHigh);
+        &can2ValidMotorMessageHigh,
+        &can2ValidMotorMessage6020Current);
 
     bool messageSuccess = true;
 
@@ -113,8 +131,11 @@ void DjiMotorTxHandler::encodeAndSendCanData()
         if (can1ValidMotorMessageHigh)
         {
             messageSuccess &= drivers->can.sendMessage(can::CanBus::CAN_BUS1, can1MessageHigh);
-            can1MessageHigh.setIdentifier(0x1FE);
-            messageSuccess &= drivers->can.sendMessage(can::CanBus::CAN_BUS1, can1MessageHigh);
+        }
+        if (can1ValidMotorMessage6020Current)
+        {
+            messageSuccess &=
+                drivers->can.sendMessage(can::CanBus::CAN_BUS1, can1Message6020Current);
         }
     }
     if (drivers->can.isReadyToSend(can::CanBus::CAN_BUS2))
@@ -127,6 +148,11 @@ void DjiMotorTxHandler::encodeAndSendCanData()
         {
             messageSuccess &= drivers->can.sendMessage(can::CanBus::CAN_BUS2, can2MessageHigh);
         }
+        if (can2ValidMotorMessage6020Current)
+        {
+            messageSuccess &=
+                drivers->can.sendMessage(can::CanBus::CAN_BUS2, can2Message6020Current);
+        }
     }
 
     if (!messageSuccess)
@@ -139,8 +165,10 @@ void DjiMotorTxHandler::serializeMotorStoreSendData(
     DjiMotor** canMotorStore,
     modm::can::Message* messageLow,
     modm::can::Message* messageHigh,
+    modm::can::Message* message6020Current,
     bool* validMotorMessageLow,
-    bool* validMotorMessageHigh)
+    bool* validMotorMessageHigh,
+    bool* validMotorMessage6020Current)
 {
     for (int i = 0; i < DJI_MOTORS_PER_CAN; i++)
     {
@@ -153,6 +181,11 @@ void DjiMotorTxHandler::serializeMotorStoreSendData(
                 motor->serializeCanSendData(messageLow);
                 *validMotorMessageLow = true;
             }
+            else if (motor->isInCurrentControl())
+            {
+                motor->serializeCanSendData(message6020Current);
+                *validMotorMessage6020Current = true;
+            }
             else
             {
                 motor->serializeCanSendData(messageHigh);
diff --git a/ut-robomaster/taproot/src/tap/motor/dji_motor_tx_handler.hpp b/ut-robomaster/taproot/src/tap/motor/dji_motor_tx_handler.hpp
index 2807a55d..7ff4b2fa 100644
--- a/ut-robomaster/taproot/src/tap/motor/dji_motor_tx_handler.hpp
+++ b/ut-robomaster/taproot/src/tap/motor/dji_motor_tx_handler.hpp
@@ -28,7 +28,9 @@
 
 #include "tap/util_macros.hpp"
 
-#include "dji_motor.hpp"
+#include "modm/architecture/interface/can_message.hpp"
+
+#include "dji_motor_ids.hpp"
 
 namespace tap
 {
@@ -37,6 +39,8 @@ class Drivers;
 
 namespace tap::motor
 {
+class DjiMotor;
+
 /**
  * Converts the dji MotorId to a uint32_t.
  * @param[in] id Some CAN MotorId
@@ -77,6 +81,8 @@ class DjiMotorTxHandler
     static constexpr uint32_t CAN_DJI_LOW_IDENTIFIER = 0X200;
     /** CAN message identifier for "high" segment (high 4 CAN motor IDs) of control message. */
     static constexpr uint32_t CAN_DJI_HIGH_IDENTIFIER = 0X1FF;
+    /** CAN message identifier for 6020s in current mode of control message. */
+    static constexpr uint32_t CAN_DJI_6020_CURRENT_IDENTIFIER = 0x1FE;
 
     DjiMotorTxHandler(Drivers* drivers) : drivers(drivers) {}
     mockable ~DjiMotorTxHandler() = default;
@@ -116,8 +122,10 @@ class DjiMotorTxHandler
         DjiMotor** canMotorStore,
         modm::can::Message* messageLow,
         modm::can::Message* messageHigh,
+        modm::can::Message* message6020Current,
         bool* validMotorMessageLow,
-        bool* validMotorMessageHigh);
+        bool* validMotorMessageHigh,
+        bool* validMotorMessage6020Current);
 
     void removeFromMotorManager(const DjiMotor& motor, DjiMotor** motorStore);
 };
diff --git a/ut-robomaster/taproot/src/tap/motor/double_dji_motor.cpp b/ut-robomaster/taproot/src/tap/motor/double_dji_motor.cpp
index 3b0112fd..aa8a196c 100644
--- a/ut-robomaster/taproot/src/tap/motor/double_dji_motor.cpp
+++ b/ut-robomaster/taproot/src/tap/motor/double_dji_motor.cpp
@@ -23,6 +23,10 @@
 
 #include "double_dji_motor.hpp"
 
+#define CAST_ENC(x)                              \
+    const_cast<tap::encoder::EncoderInterface*>( \
+        static_cast<const tap::encoder::EncoderInterface*>(&x))
+
 namespace tap::motor
 {
 DoubleDjiMotor::DoubleDjiMotor(
@@ -35,24 +39,32 @@ DoubleDjiMotor::DoubleDjiMotor(
     bool isInvertedTwo,
     const char* nameOne,
     const char* nameTwo,
-    uint16_t encWrapped,
-    int64_t encRevolutions)
+    bool currentControl,
+    float gearRatio,
+    uint32_t encoderHomePositionOne,
+    tap::encoder::EncoderInterface* externalEncoder)
     : motorOne(
           drivers,
           desMotorIdentifierOne,
           motorCanBusOne,
           isInvertedOne,
           nameOne,
-          encWrapped,
-          encRevolutions),
+          currentControl,
+          gearRatio,
+          encoderHomePositionOne),
       motorTwo(
           drivers,
           desMotorIdentifierTwo,
           motorCanBusTwo,
           isInvertedTwo,
           nameTwo,
-          encWrapped,
-          encRevolutions)
+          currentControl,
+          gearRatio),
+      encoder(
+          {externalEncoder != nullptr ? externalEncoder : CAST_ENC(motorOne.getInternalEncoder()),
+           externalEncoder != nullptr ? CAST_ENC(motorOne.getInternalEncoder())
+                                      : CAST_ENC(motorTwo.getInternalEncoder()),
+           externalEncoder != nullptr ? CAST_ENC(motorTwo.getInternalEncoder()) : nullptr})
 {
 }
 
@@ -60,22 +72,11 @@ void DoubleDjiMotor::initialize()
 {
     motorOne.initialize();
     motorTwo.initialize();
+    // This is weird because the initialize is called twice for the internal encoders. This is
+    // fine because the internal encoders have no initialize logic.
+    encoder.initialize();
 }
 
-int64_t DoubleDjiMotor::getEncoderUnwrapped() const { return motorOne.getEncoderUnwrapped(); }
-
-uint16_t DoubleDjiMotor::getEncoderWrapped() const { return motorOne.getEncoderWrapped(); }
-
-void DoubleDjiMotor::resetEncoderValue()
-{
-    motorOne.resetEncoderValue();
-    motorTwo.resetEncoderValue();
-}
-
-float DoubleDjiMotor::getPositionUnwrapped() const { return motorOne.getPositionUnwrapped(); }
-
-float DoubleDjiMotor::getPositionWrapped() const { return motorOne.getPositionWrapped(); }
-
 void DoubleDjiMotor::setDesiredOutput(int32_t desiredOutput)
 {
     motorOne.setDesiredOutput(desiredOutput);
@@ -101,17 +102,13 @@ int8_t DoubleDjiMotor::getTemperature() const
 {
     return std::max(motorOne.getTemperature(), motorTwo.getTemperature());
 }
+
 int16_t DoubleDjiMotor::getTorque() const
 {
-    return (static_cast<int32_t>(motorOne.getTorque()) +
-            static_cast<int32_t>(motorTwo.getTorque())) /
-           2;
-}
+    int32_t m1Torque = motorOne.getTorque();
+    int32_t m2Torque = motorTwo.getTorque();
+    int num_online = motorOne.isMotorOnline() + motorTwo.isMotorOnline();
 
-int16_t DoubleDjiMotor::getShaftRPM() const
-{
-    return (static_cast<int32_t>(motorOne.getShaftRPM()) +
-            static_cast<int32_t>(motorTwo.getShaftRPM())) /
-           2;
+    return num_online == 0 ? 0 : (m1Torque + m2Torque) / num_online;
 }
 }  // namespace tap::motor
diff --git a/ut-robomaster/taproot/src/tap/motor/double_dji_motor.hpp b/ut-robomaster/taproot/src/tap/motor/double_dji_motor.hpp
index c684fb26..c28d1719 100644
--- a/ut-robomaster/taproot/src/tap/motor/double_dji_motor.hpp
+++ b/ut-robomaster/taproot/src/tap/motor/double_dji_motor.hpp
@@ -54,21 +54,21 @@ class DoubleDjiMotor : public MotorInterface
         bool isInvertedTwo,
         const char* nameOne,
         const char* nameTwo,
-        uint16_t encWrapped = DjiMotor::ENC_RESOLUTION / 2,
-        int64_t encRevolutions = 0);
+        bool currentControl = false,
+        float gearRatio = 1,
+        uint32_t encoderHomePositionOne = 0,
+        tap::encoder::EncoderInterface* externalEncoder = nullptr);
 
     void initialize() override;
-    float getPositionUnwrapped() const override;
-    float getPositionWrapped() const override;
-    int64_t getEncoderUnwrapped() const override;
-    uint16_t getEncoderWrapped() const override;
-    void resetEncoderValue() override;
+    tap::encoder::EncoderInterface* getEncoder() const override
+    {
+        return const_cast<tap::encoder::MultiEncoder<3>*>(&this->encoder);
+    }
     void setDesiredOutput(int32_t desiredOutput) override;
     bool isMotorOnline() const override;
     int16_t getOutputDesired() const override;
     int8_t getTemperature() const override;
     int16_t getTorque() const override;
-    int16_t getShaftRPM() const override;
 
 protected:
 #if defined(PLATFORM_HOSTED) && defined(ENV_UNIT_TESTS)
@@ -81,6 +81,7 @@ class DoubleDjiMotor : public MotorInterface
     DjiMotor motorOne;
     DjiMotor motorTwo;
 #endif
+    tap::encoder::MultiEncoder<3> encoder;
 };
 }  // namespace tap::motor
 
diff --git a/ut-robomaster/taproot/src/tap/motor/motor_interface.hpp b/ut-robomaster/taproot/src/tap/motor/motor_interface.hpp
index fba27b78..48901143 100644
--- a/ut-robomaster/taproot/src/tap/motor/motor_interface.hpp
+++ b/ut-robomaster/taproot/src/tap/motor/motor_interface.hpp
@@ -26,23 +26,34 @@
 
 #include <cstdint>
 
+#include "tap/communication/sensors/encoder/encoder_interface.hpp"
+
 namespace tap::motor
 {
 class MotorInterface
 {
 public:
     virtual void initialize() = 0;
-    virtual int64_t getEncoderUnwrapped() const = 0;
-    virtual uint16_t getEncoderWrapped() const = 0;
-    virtual void resetEncoderValue() = 0;
-    virtual float getPositionUnwrapped() const = 0;
-    virtual float getPositionWrapped() const = 0;
+    virtual tap::encoder::EncoderInterface* getEncoder() const = 0;
     virtual void setDesiredOutput(int32_t desiredOutput) = 0;
     virtual bool isMotorOnline() const = 0;
     virtual int16_t getOutputDesired() const = 0;
     virtual int8_t getTemperature() const = 0;
     virtual int16_t getTorque() const = 0;
-    virtual int16_t getShaftRPM() const = 0;
+
+    [[deprecated]] virtual void resetEncoderValue() { this->getEncoder()->resetEncoderValue(); };
+    [[deprecated]] virtual float getPositionUnwrapped() const
+    {
+        return this->getEncoder()->getPosition().getUnwrappedValue();
+    };
+    [[deprecated]] virtual float getPositionWrapped() const
+    {
+        return this->getEncoder()->getPosition().getWrappedValue();
+    };
+    [[deprecated]] virtual int16_t getShaftRPM() const
+    {
+        return this->getEncoder()->getVelocity() / static_cast<float>(M_TWOPI) * 60.f;
+    };
 };
 
 }  // namespace tap::motor
diff --git a/ut-robomaster/taproot/src/tap/motor/motorsim/can_serializer.hpp b/ut-robomaster/taproot/src/tap/motor/motorsim/can_serializer.hpp
index 53a565d1..72c8ff36 100644
--- a/ut-robomaster/taproot/src/tap/motor/motorsim/can_serializer.hpp
+++ b/ut-robomaster/taproot/src/tap/motor/motorsim/can_serializer.hpp
@@ -29,7 +29,7 @@
 #include <array>
 #include <cstdint>
 
-#include "tap/motor/dji_motor.hpp"
+#include "tap/motor/dji_motor_ids.hpp"
 
 namespace modm::can
 {
diff --git a/ut-robomaster/taproot/test/tap/mock/abstract_imu_mock.cpp b/ut-robomaster/taproot/test/tap/mock/abstract_imu_mock.cpp
new file mode 100644
index 00000000..40e99f24
--- /dev/null
+++ b/ut-robomaster/taproot/test/tap/mock/abstract_imu_mock.cpp
@@ -0,0 +1,30 @@
+/*****************************************************************************/
+/********** !!! WARNING: CODE GENERATED BY TAPROOT. DO NOT EDIT !!! **********/
+/*****************************************************************************/
+
+/*
+ * Copyright (c) 2025 Advanced Robotics at the University of Washington <robomstr@uw.edu>
+ *
+ * This file is part of Taproot.
+ *
+ * Taproot is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Taproot is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Taproot.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+#include "abstract_imu_mock.hpp"
+
+namespace tap::mock
+{
+AbstractImuMock::AbstractImuMock() {}
+AbstractImuMock::~AbstractImuMock() {}
+}  // namespace tap::mock
diff --git a/ut-robomaster/taproot/test/tap/mock/abstract_imu_mock.hpp b/ut-robomaster/taproot/test/tap/mock/abstract_imu_mock.hpp
new file mode 100644
index 00000000..40f4ceda
--- /dev/null
+++ b/ut-robomaster/taproot/test/tap/mock/abstract_imu_mock.hpp
@@ -0,0 +1,54 @@
+/*****************************************************************************/
+/********** !!! WARNING: CODE GENERATED BY TAPROOT. DO NOT EDIT !!! **********/
+/*****************************************************************************/
+
+/*
+ * Copyright (c) 2025 Advanced Robotics at the University of Washington <robomstr@uw.edu>
+ *
+ * This file is part of Taproot.
+ *
+ * Taproot is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Taproot is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Taproot.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+#ifndef TAPROOT_ABSTRACT_IMU_MOCK_HPP_
+#define TAPROOT_ABSTRACT_IMU_MOCK_HPP_
+
+#include <gmock/gmock.h>
+
+#include "tap/communication/sensors/imu/abstract_imu.hpp"
+
+namespace tap::mock
+{
+class AbstractImuMock : public tap::communication::sensors::imu::AbstractIMU
+{
+public:
+    AbstractImuMock();
+    virtual ~AbstractImuMock();
+
+    MOCK_METHOD(const char *, getName, (), (const override));
+    MOCK_METHOD(float, getTemp, (), (const override));
+    MOCK_METHOD(float, getAx, (), (const override));
+    MOCK_METHOD(float, getAy, (), (const override));
+    MOCK_METHOD(float, getAz, (), (const override));
+    MOCK_METHOD(float, getGx, (), (const override));
+    MOCK_METHOD(float, getGy, (), (const override));
+    MOCK_METHOD(float, getGz, (), (const override));
+    MOCK_METHOD(float, getYaw, (), (const override));
+    MOCK_METHOD(float, getPitch, (), (const override));
+    MOCK_METHOD(float, getRoll, (), (const override));
+    MOCK_METHOD(float, getAccelerationSensitivity, (), (const override));
+};
+}  // namespace tap::mock
+
+#endif  // TAPROOT_ABSTRACT_IMU_MOCK_HPP_
diff --git a/ut-robomaster/taproot/test/tap/mock/bmi088_mock.hpp b/ut-robomaster/taproot/test/tap/mock/bmi088_mock.hpp
index 157a9bdc..a6ab2817 100644
--- a/ut-robomaster/taproot/test/tap/mock/bmi088_mock.hpp
+++ b/ut-robomaster/taproot/test/tap/mock/bmi088_mock.hpp
@@ -38,19 +38,18 @@ class Bmi088Mock : public tap::communication::sensors::imu::bmi088::Bmi088
 
     MOCK_METHOD(void, initialize, (float, float, float), (override));
     MOCK_METHOD(void, periodicIMUUpdate, (), (override));
-    MOCK_METHOD(void, read, (), (override));
-    MOCK_METHOD(ImuState, getImuState, (), (const override));
-    MOCK_METHOD(void, requestRecalibration, (), (override));
-    MOCK_METHOD(float, getYaw, (), (override));
-    MOCK_METHOD(float, getPitch, (), (override));
-    MOCK_METHOD(float, getRoll, (), (override));
-    MOCK_METHOD(float, getGx, (), (override));
-    MOCK_METHOD(float, getGy, (), (override));
-    MOCK_METHOD(float, getGz, (), (override));
-    MOCK_METHOD(float, getAx, (), (override));
-    MOCK_METHOD(float, getAy, (), (override));
-    MOCK_METHOD(float, getAz, (), (override));
-    MOCK_METHOD(float, getTemp, (), (override));
+    MOCK_METHOD(bool, read, (), (override));
+    MOCK_METHOD(ImuState, getImuState, (), (const final override));
+    MOCK_METHOD(float, getYaw, (), (const override));
+    MOCK_METHOD(float, getPitch, (), (const override));
+    MOCK_METHOD(float, getRoll, (), (const override));
+    MOCK_METHOD(float, getGx, (), (const override));
+    MOCK_METHOD(float, getGy, (), (const override));
+    MOCK_METHOD(float, getGz, (), (const override));
+    MOCK_METHOD(float, getAx, (), (const override));
+    MOCK_METHOD(float, getAy, (), (const override));
+    MOCK_METHOD(float, getAz, (), (const override));
+    MOCK_METHOD(float, getTemp, (), (const override));
     MOCK_METHOD(uint32_t, getPrevIMUDataReceivedTime, (), (const override));
 };
 }  // namespace tap::mock
diff --git a/ut-robomaster/taproot/test/tap/mock/dji_motor_encoder_mock.cpp b/ut-robomaster/taproot/test/tap/mock/dji_motor_encoder_mock.cpp
new file mode 100644
index 00000000..a2a88529
--- /dev/null
+++ b/ut-robomaster/taproot/test/tap/mock/dji_motor_encoder_mock.cpp
@@ -0,0 +1,50 @@
+/*****************************************************************************/
+/********** !!! WARNING: CODE GENERATED BY TAPROOT. DO NOT EDIT !!! **********/
+/*****************************************************************************/
+
+/*
+ * Copyright (c) 2024 Advanced Robotics at the University of Washington <robomstr@uw.edu>
+ *
+ * This file is part of Taproot.
+ *
+ * Taproot is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Taproot is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Taproot.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+#include "dji_motor_encoder_mock.hpp"
+
+namespace tap::mock
+{
+DjiMotorEncoderMock::DjiMotorEncoderMock(
+    bool isInverted,
+    float gearRatio,
+    uint32_t encoderHomePosition)
+    : DjiMotorEncoder(isInverted, gearRatio, encoderHomePosition)
+{
+    ON_CALL(*this, isOnline).WillByDefault(testing::Return(true));
+    ON_CALL(*this, getVelocity).WillByDefault(testing::Invoke([&]() {
+        return this->DjiMotorEncoder::getVelocity();
+    }));
+    ON_CALL(*this, getPosition).WillByDefault(testing::Invoke([&]() {
+        return this->DjiMotorEncoder::getPosition();
+    }));
+    ON_CALL(*this, getEncoder)
+        .WillByDefault(testing::Return(tap::algorithms::WrappedFloat(0, 0, ENC_RESOLUTION)));
+}
+
+DjiMotorEncoderMock::~DjiMotorEncoderMock()
+{
+    ON_CALL(*this, getVelocity).WillByDefault(testing::Return(0));
+    ON_CALL(*this, getPosition).WillByDefault(testing::Return(tap::algorithms::Angle(0)));
+}
+}  // namespace tap::mock
diff --git a/ut-robomaster/taproot/test/tap/mock/dji_motor_encoder_mock.hpp b/ut-robomaster/taproot/test/tap/mock/dji_motor_encoder_mock.hpp
new file mode 100644
index 00000000..e9ab923c
--- /dev/null
+++ b/ut-robomaster/taproot/test/tap/mock/dji_motor_encoder_mock.hpp
@@ -0,0 +1,60 @@
+/*****************************************************************************/
+/********** !!! WARNING: CODE GENERATED BY TAPROOT. DO NOT EDIT !!! **********/
+/*****************************************************************************/
+
+/*
+ * Copyright (c) 2024 Advanced Robotics at the University of Washington <robomstr@uw.edu>
+ *
+ * This file is part of Taproot.
+ *
+ * Taproot is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Taproot is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Taproot.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+#ifndef TAPROOT_DJI_MOTOR_ENCODER_MOCK_HPP_
+#define TAPROOT_DJI_MOTOR_ENCODER_MOCK_HPP_
+
+#include <gmock/gmock.h>
+
+#include "tap/motor/dji_motor_encoder.hpp"
+
+#include "modm/architecture/interface/can_message.hpp"
+
+namespace tap::mock
+{
+class DjiMotorEncoderMock : public tap::motor::DjiMotorEncoder
+{
+public:
+    DjiMotorEncoderMock(bool isInverted, float gearRatio = 1, uint32_t encoderHomePosition = 0);
+    virtual ~DjiMotorEncoderMock();
+
+    MOCK_METHOD(void, initialize, (), (override));
+
+    MOCK_METHOD(bool, isOnline, (), (const override));
+
+    MOCK_METHOD(tap::algorithms::WrappedFloat, getPosition, (), (const override));
+
+    MOCK_METHOD(float, getVelocity, (), (const override));
+
+    MOCK_METHOD(tap::algorithms::WrappedFloat, getEncoder, (), (const));
+
+    MOCK_METHOD(int16_t, getShaftRPM, (), (const override));
+
+    MOCK_METHOD(void, resetEncoderValue, (), (override));
+
+    MOCK_METHOD(void, processMessage, (const modm::can::Message& message), (override));
+};
+
+}  // namespace tap::mock
+
+#endif  // TAPROOT_DJI_MOTOR_ENCODER_MOCK_HPP_
diff --git a/ut-robomaster/taproot/test/tap/mock/dji_motor_mock.cpp b/ut-robomaster/taproot/test/tap/mock/dji_motor_mock.cpp
index 7daac1e4..6815f9b0 100644
--- a/ut-robomaster/taproot/test/tap/mock/dji_motor_mock.cpp
+++ b/ut-robomaster/taproot/test/tap/mock/dji_motor_mock.cpp
@@ -31,16 +31,20 @@ DjiMotorMock::DjiMotorMock(
     tap::can::CanBus motorCanBus,
     bool isInverted,
     const char *name,
-    uint16_t encWrapped,
-    int64_t encRevolutions)
+    bool currentControl,
+    float gearRatio,
+    uint32_t encoderHomePosition,
+    tap::encoder::EncoderInterface *externalEncoder)
     : DjiMotor(
           drivers,
           desMotorIdentifier,
           motorCanBus,
           isInverted,
           name,
-          encWrapped,
-          encRevolutions)
+          currentControl,
+          gearRatio,
+          encoderHomePosition,
+          externalEncoder)
 {
 }
 DjiMotorMock::~DjiMotorMock() {}
diff --git a/ut-robomaster/taproot/test/tap/mock/dji_motor_mock.hpp b/ut-robomaster/taproot/test/tap/mock/dji_motor_mock.hpp
index 50abfba2..9d4fb57c 100644
--- a/ut-robomaster/taproot/test/tap/mock/dji_motor_mock.hpp
+++ b/ut-robomaster/taproot/test/tap/mock/dji_motor_mock.hpp
@@ -31,6 +31,8 @@
 
 #include "modm/architecture/interface/can_message.hpp"
 
+#include "dji_motor_encoder_mock.hpp"
+
 namespace tap
 {
 namespace mock
@@ -44,26 +46,24 @@ class DjiMotorMock : public tap::motor::DjiMotor
         tap::can::CanBus motorCanBus,
         bool isInverted,
         const char* name,
-        uint16_t encWrapped = ENC_RESOLUTION / 2,
-        int64_t encRevolutions = 0);
+        bool currentControl = false,
+        float gearRatio = 1,
+        uint32_t encoderHomePosition = 0,
+        tap::encoder::EncoderInterface* externalEncoder = nullptr);
     virtual ~DjiMotorMock();
 
     MOCK_METHOD(void, initialize, (), (override));
-    MOCK_METHOD(float, getPositionUnwrapped, (), (const override));
-    MOCK_METHOD(float, getPositionWrapped, (), (const override));
-    MOCK_METHOD(int64_t, getEncoderUnwrapped, (), (const override));
-    MOCK_METHOD(uint16_t, getEncoderWrapped, (), (const override));
     MOCK_METHOD(void, processMessage, (const modm::can::Message& message), (override));
     MOCK_METHOD(void, setDesiredOutput, (int32_t desiredOutput), (override));
     MOCK_METHOD(void, resetEncoderValue, (), (override));
     MOCK_METHOD(bool, isMotorOnline, (), (const override));
+    MOCK_METHOD(bool, isInCurrentControl, (), (const override));
     MOCK_METHOD(void, serializeCanSendData, (modm::can::Message * txMessage), (const override));
 
     MOCK_METHOD(int16_t, getOutputDesired, (), (const override));
     MOCK_METHOD(uint32_t, getMotorIdentifier, (), (const override));
     MOCK_METHOD(int8_t, getTemperature, (), (const override));
     MOCK_METHOD(int16_t, getTorque, (), (const override));
-    MOCK_METHOD(int16_t, getShaftRPM, (), (const override));
     MOCK_METHOD(bool, isMotorInverted, (), (const override));
     MOCK_METHOD(tap::can::CanBus, getCanBus, (), (const override));
     MOCK_METHOD(const char*, getName, (), (const override));
diff --git a/ut-robomaster/taproot/test/tap/mock/dji_motor_tx_handler_mock.cpp b/ut-robomaster/taproot/test/tap/mock/dji_motor_tx_handler_mock.cpp
index aa8eaba5..cea7fe7f 100644
--- a/ut-robomaster/taproot/test/tap/mock/dji_motor_tx_handler_mock.cpp
+++ b/ut-robomaster/taproot/test/tap/mock/dji_motor_tx_handler_mock.cpp
@@ -23,6 +23,8 @@
 
 #include "dji_motor_tx_handler_mock.hpp"
 
+#include "tap/motor/dji_motor.hpp"
+
 namespace tap::mock
 {
 DjiMotorTxHandlerMock::DjiMotorTxHandlerMock(tap::Drivers *drivers)
diff --git a/ut-robomaster/taproot/test/tap/mock/encoder_interface_mock.cpp b/ut-robomaster/taproot/test/tap/mock/encoder_interface_mock.cpp
new file mode 100644
index 00000000..89be60ff
--- /dev/null
+++ b/ut-robomaster/taproot/test/tap/mock/encoder_interface_mock.cpp
@@ -0,0 +1,30 @@
+/*****************************************************************************/
+/********** !!! WARNING: CODE GENERATED BY TAPROOT. DO NOT EDIT !!! **********/
+/*****************************************************************************/
+
+/*
+ * Copyright (c) 2024 Advanced Robotics at the University of Washington <robomstr@uw.edu>
+ *
+ * This file is part of Taproot.
+ *
+ * Taproot is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Taproot is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Taproot.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+#include "encoder_interface_mock.hpp"
+
+namespace tap::mock
+{
+EncoderInterfaceMock::EncoderInterfaceMock() : EncoderInterface() {}
+EncoderInterfaceMock::~EncoderInterfaceMock() {}
+}  // namespace tap::mock
diff --git a/ut-robomaster/taproot/test/tap/mock/encoder_interface_mock.hpp b/ut-robomaster/taproot/test/tap/mock/encoder_interface_mock.hpp
new file mode 100644
index 00000000..e8d709a8
--- /dev/null
+++ b/ut-robomaster/taproot/test/tap/mock/encoder_interface_mock.hpp
@@ -0,0 +1,54 @@
+/*****************************************************************************/
+/********** !!! WARNING: CODE GENERATED BY TAPROOT. DO NOT EDIT !!! **********/
+/*****************************************************************************/
+
+/*
+ * Copyright (c) 2024 Advanced Robotics at the University of Washington <robomstr@uw.edu>
+ *
+ * This file is part of Taproot.
+ *
+ * Taproot is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Taproot is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Taproot.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+#ifndef TAPROOT_ENCODER_INTERFACE_MOCK_HPP_
+#define TAPROOT_ENCODER_INTERFACE_MOCK_HPP_
+
+#include <gmock/gmock.h>
+
+#include "tap/communication/sensors/encoder/encoder_interface.hpp"
+
+namespace tap::mock
+{
+class EncoderInterfaceMock : public tap::encoder::EncoderInterface
+{
+public:
+    EncoderInterfaceMock();
+    virtual ~EncoderInterfaceMock();
+
+    MOCK_METHOD(void, initialize, (), (override));
+
+    MOCK_METHOD(bool, isOnline, (), (const override));
+
+    MOCK_METHOD(tap::algorithms::WrappedFloat, getPosition, (), (const override));
+
+    MOCK_METHOD(float, getVelocity, (), (const override));
+
+    MOCK_METHOD(void, resetEncoderValue, (), (override));
+
+    MOCK_METHOD(void, alignWith, (EncoderInterface*), (override));
+};
+
+}  // namespace tap::mock
+
+#endif  // TAPROOT_ENCODER_INTERFACE_MOCK_HPP_
diff --git a/ut-robomaster/taproot/test/tap/mock/imu_interface_mock.hpp b/ut-robomaster/taproot/test/tap/mock/imu_interface_mock.hpp
index 30c7a2c8..e98a56da 100644
--- a/ut-robomaster/taproot/test/tap/mock/imu_interface_mock.hpp
+++ b/ut-robomaster/taproot/test/tap/mock/imu_interface_mock.hpp
@@ -37,16 +37,15 @@ class ImuInterfaceMock : public tap::communication::sensors::imu::ImuInterface
     virtual ~ImuInterfaceMock();
 
     MOCK_METHOD(const char *, getName, (), (const override));
-    MOCK_METHOD(float, getAx, (), (override));
-    MOCK_METHOD(float, getAy, (), (override));
-    MOCK_METHOD(float, getAz, (), (override));
-    MOCK_METHOD(float, getGx, (), (override));
-    MOCK_METHOD(float, getGy, (), (override));
-    MOCK_METHOD(float, getGz, (), (override));
-    MOCK_METHOD(float, getTemp, (), (override));
-    MOCK_METHOD(float, getYaw, (), (override));
-    MOCK_METHOD(float, getPitch, (), (override));
-    MOCK_METHOD(float, getRoll, (), (override));
+    MOCK_METHOD(float, getAx, (), (const override));
+    MOCK_METHOD(float, getAy, (), (const override));
+    MOCK_METHOD(float, getAz, (), (const override));
+    MOCK_METHOD(float, getGx, (), (const override));
+    MOCK_METHOD(float, getGy, (), (const override));
+    MOCK_METHOD(float, getGz, (), (const override));
+    MOCK_METHOD(float, getYaw, (), (const override));
+    MOCK_METHOD(float, getPitch, (), (const override));
+    MOCK_METHOD(float, getRoll, (), (const override));
 };
 }  // namespace tap::mock
 
diff --git a/ut-robomaster/taproot/test/tap/mock/imu_terminal_serial_handler_mock.cpp b/ut-robomaster/taproot/test/tap/mock/imu_terminal_serial_handler_mock.cpp
index 5f5154bc..b62f9a05 100644
--- a/ut-robomaster/taproot/test/tap/mock/imu_terminal_serial_handler_mock.cpp
+++ b/ut-robomaster/taproot/test/tap/mock/imu_terminal_serial_handler_mock.cpp
@@ -27,7 +27,7 @@ namespace tap::mock
 {
 ImuTerminalSerialHandlerMock::ImuTerminalSerialHandlerMock(
     tap::Drivers* drivers,
-    communication::sensors::imu::ImuInterface* imu)
+    communication::sensors::imu::AbstractIMU* imu)
     : communication::sensors::imu::ImuTerminalSerialHandler(drivers, imu)
 {
 }
diff --git a/ut-robomaster/taproot/test/tap/mock/imu_terminal_serial_handler_mock.hpp b/ut-robomaster/taproot/test/tap/mock/imu_terminal_serial_handler_mock.hpp
index e9927c78..6f18693d 100644
--- a/ut-robomaster/taproot/test/tap/mock/imu_terminal_serial_handler_mock.hpp
+++ b/ut-robomaster/taproot/test/tap/mock/imu_terminal_serial_handler_mock.hpp
@@ -35,7 +35,7 @@ class ImuTerminalSerialHandlerMock : public communication::sensors::imu::ImuTerm
 public:
     ImuTerminalSerialHandlerMock(
         tap::Drivers* drivers,
-        communication::sensors::imu::ImuInterface* imu);
+        communication::sensors::imu::AbstractIMU* imu);
     virtual ~ImuTerminalSerialHandlerMock();
 
     MOCK_METHOD(void, init, (), (override));
diff --git a/ut-robomaster/taproot/test/tap/mock/motor_interface_mock.cpp b/ut-robomaster/taproot/test/tap/mock/motor_interface_mock.cpp
index 3db31015..0b4c4e4c 100644
--- a/ut-robomaster/taproot/test/tap/mock/motor_interface_mock.cpp
+++ b/ut-robomaster/taproot/test/tap/mock/motor_interface_mock.cpp
@@ -25,7 +25,7 @@
 
 namespace tap::mock
 {
-MotorInterfaceMock::MotorInterfaceMock() : tap::motor::MotorInterface() {}
+MotorInterfaceMock::MotorInterfaceMock() : tap::motor::MotorInterface(), encoder() {}
 
 MotorInterfaceMock::~MotorInterfaceMock() {}
 }  // namespace tap::mock
diff --git a/ut-robomaster/taproot/test/tap/mock/motor_interface_mock.hpp b/ut-robomaster/taproot/test/tap/mock/motor_interface_mock.hpp
index 4586473c..7b27a1dc 100644
--- a/ut-robomaster/taproot/test/tap/mock/motor_interface_mock.hpp
+++ b/ut-robomaster/taproot/test/tap/mock/motor_interface_mock.hpp
@@ -30,6 +30,8 @@
 
 #include "tap/motor/motor_interface.hpp"
 
+#include "encoder_interface_mock.hpp"
+
 namespace tap::mock
 {
 class MotorInterfaceMock : public tap::motor::MotorInterface
@@ -39,17 +41,19 @@ class MotorInterfaceMock : public tap::motor::MotorInterface
     virtual ~MotorInterfaceMock();
 
     MOCK_METHOD(void, initialize, (), (override));
-    MOCK_METHOD(int64_t, getEncoderUnwrapped, (), (const override));
-    MOCK_METHOD(uint16_t, getEncoderWrapped, (), (const override));
-    MOCK_METHOD(void, resetEncoderValue, (), (override));
-    MOCK_METHOD(float, getPositionUnwrapped, (), (const override));
-    MOCK_METHOD(float, getPositionWrapped, (), (const override));
     MOCK_METHOD(void, setDesiredOutput, (int32_t), (override));
     MOCK_METHOD(bool, isMotorOnline, (), (const override));
     MOCK_METHOD(int16_t, getOutputDesired, (), (const override));
     MOCK_METHOD(int8_t, getTemperature, (), (const override));
     MOCK_METHOD(int16_t, getTorque, (), (const override));
-    MOCK_METHOD(int16_t, getShaftRPM, (), (const override));
+
+    EncoderInterfaceMock* getEncoder() const override
+    {
+        return const_cast<testing::NiceMock<tap::mock::EncoderInterfaceMock>*>(&encoder);
+    }
+
+private:
+    testing::NiceMock<tap::mock::EncoderInterfaceMock> encoder;
 };
 
 }  // namespace tap::mock
diff --git a/ut-robomaster/taproot/test/tap/mock/odometry_2d_interface_mock.hpp b/ut-robomaster/taproot/test/tap/mock/odometry_2d_interface_mock.hpp
index 7fa3f649..a13f98ab 100644
--- a/ut-robomaster/taproot/test/tap/mock/odometry_2d_interface_mock.hpp
+++ b/ut-robomaster/taproot/test/tap/mock/odometry_2d_interface_mock.hpp
@@ -37,6 +37,11 @@ class Odometry2DInterfaceMock : public algorithms::odometry::Odometry2DInterface
     MOCK_METHOD(modm::Vector2f, getCurrentVelocity2D, (), (const override));
     MOCK_METHOD(uint32_t, getLastComputedOdometryTime, (), (const override));
     MOCK_METHOD(float, getYaw, (), (const override));
+    MOCK_METHOD(
+        void,
+        overrideOdometryPosition,
+        (const float positionX, const float positionY),
+        (override));
 };
 }  // namespace tap::mock