diff --git a/.gitignore b/.gitignore
index 106877fa..f29e35c0 100644
--- a/.gitignore
+++ b/.gitignore
@@ -53,3 +53,5 @@ ODrive\.creator\.user
 ODrive\.files
 
 ODrive\.includes
+
+Firmware/Tests/bin/
diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp
index 0e6f64dc..53b9925d 100644
--- a/Firmware/MotorControl/axis.cpp
+++ b/Firmware/MotorControl/axis.cpp
@@ -551,9 +551,12 @@ void Axis::run_state_machine_loop() {
         }
 
         // If the state failed, go to idle, else advance task chain
-        if (!status)
+        if (!status) {
+            std::fill(task_chain_.begin(), task_chain_.end(), AXIS_STATE_UNDEFINED);
             current_state_ = AXIS_STATE_IDLE;
-        else
-            memmove(task_chain_, task_chain_ + 1, sizeof(task_chain_) - sizeof(task_chain_[0]));
+        } else {
+            std::rotate(task_chain_.begin(), task_chain_.begin() + 1, task_chain_.end());
+            task_chain_.back() = AXIS_STATE_UNDEFINED;
+        }
     }
 }
diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp
index 9f99f33a..c5a87a67 100644
--- a/Firmware/MotorControl/axis.hpp
+++ b/Firmware/MotorControl/axis.hpp
@@ -253,8 +253,8 @@ public:
     uint16_t dir_pin_;
 
     State_t requested_state_ = AXIS_STATE_STARTUP_SEQUENCE;
-    State_t task_chain_[10] = { AXIS_STATE_UNDEFINED };
-    State_t& current_state_ = task_chain_[0];
+    std::array<State_t, 10> task_chain_ = { AXIS_STATE_UNDEFINED };
+    State_t& current_state_ = task_chain_.front();
     uint32_t loop_counter_ = 0;
     LockinState_t lockin_state_ = LOCKIN_STATE_INACTIVE;
     Homing_t homing_;
diff --git a/Firmware/Tests/test_can.cpp b/Firmware/Tests/test_can.cpp
index 29f96a79..ef2ffecf 100644
--- a/Firmware/Tests/test_can.cpp
+++ b/Firmware/Tests/test_can.cpp
@@ -1,5 +1,4 @@
 
-#define DOCTEST_IMPLEMENT
 #include <doctest.h>
 #include <algorithm>
 #include <cstring>
diff --git a/Firmware/Tests/test_rotate.cpp b/Firmware/Tests/test_rotate.cpp
new file mode 100644
index 00000000..b77376cc
--- /dev/null
+++ b/Firmware/Tests/test_rotate.cpp
@@ -0,0 +1,32 @@
+#include <doctest.h>
+
+#include <algorithm>
+#include <array>
+#include <iostream>
+
+TEST_CASE("Rotate Axis State"){
+    std::array<int, 10> testArr = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9};
+    const int& currentVal = testArr.front();
+    CHECK(currentVal == testArr[0]);
+
+    std::rotate(testArr.begin(), testArr.begin() + 1, testArr.end());
+    CHECK(currentVal == testArr[0]);
+    CHECK(currentVal == 1);
+
+    CHECK(testArr.back() == 0);
+
+    std::rotate(testArr.begin(), testArr.begin() + 1, testArr.end());
+    CHECK(currentVal == testArr[0]);
+    CHECK(currentVal == 2);
+    CHECK(testArr.back() == 1);
+
+
+}
+
+TEST_CASE("Fill Test"){
+    std::array<int, 10> testArr = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9};
+    std::fill(testArr.begin(), testArr.end(), 6);
+    for(const auto& val : testArr){
+        CHECK(val == 6);
+    }
+}
\ No newline at end of file
diff --git a/Firmware/Tests/test_runner.cpp b/Firmware/Tests/test_runner.cpp
index e2314477..e111405f 100644
--- a/Firmware/Tests/test_runner.cpp
+++ b/Firmware/Tests/test_runner.cpp
@@ -16,10 +16,6 @@
 using std::cout;
 using std::endl;
 
-
-
-
-
 TEST_SUITE("delta_enc") {
     // Modulo (as opposed to remainder), per https://stackoverflow.com/a/19288271
     int mod(int dividend, int divisor) {
@@ -131,7 +127,7 @@ TEST_SUITE("vel_ramp") {
         return v & 1;
     }
 
-    TEST_CASE("Blah") {
+    TEST_CASE("Equivalence") {
         float vel_setpoint = 0.0f;
         float vel_ramp_rate = 8000;
         float input_vel = 0.0f;
diff --git a/Firmware/Tests/test_timer.cpp b/Firmware/Tests/test_timer.cpp
index 4b3d81ac..89487f93 100644
--- a/Firmware/Tests/test_timer.cpp
+++ b/Firmware/Tests/test_timer.cpp
@@ -1,4 +1,3 @@
-#define DOCTEST_IMPLEMENT
 #include <doctest.h>
 #include "MotorControl/timer.hpp"
 #include <stdint.h>
diff --git a/Firmware/Tupfile.lua b/Firmware/Tupfile.lua
index 8b66326a..4577bde1 100644
--- a/Firmware/Tupfile.lua
+++ b/Firmware/Tupfile.lua
@@ -191,6 +191,7 @@ build{
 
 if tup.getconfig('DOCTEST') == 'true' then
     TEST_INCLUDES = '-I. -I./MotorControl -I./fibre/cpp/include -I./Drivers/DRV8301 -I./doctest'
-    tup.frule{inputs='Tests/*.cpp', command='g++ -O3 -std=gnu++17 '..TEST_INCLUDES..' %f -o %o', outputs='Tests/test_runner.exe'}
+    tup.foreach_rule('Tests/*.cpp', 'g++ -O3 -std=c++17 '..TEST_INCLUDES..' -c %f -o %o', 'Tests/bin/%B.o')
+    tup.frule{inputs='Tests/bin/*.o', command='g++ %f -o %o', outputs='Tests/test_runner.exe'}
     tup.frule{inputs='Tests/test_runner.exe', command='%f'}
-end
\ No newline at end of file
+end