diff --git a/test/pid_stepwisecontroller_unittest.cpp b/test/pid_stepwisecontroller_unittest.cpp
new file mode 100644
index 0000000..45d1b81
--- /dev/null
+++ b/test/pid_stepwisecontroller_unittest.cpp
@@ -0,0 +1,83 @@
+#include "pid/controller.hpp"
+#include "pid/ec/stepwise.hpp"
+#include "pid/stepwisecontroller.hpp"
+#include "test/zone_mock.hpp"
+
+#include <string>
+#include <vector>
+
+#include <gmock/gmock.h>
+#include <gtest/gtest.h>
+
+using ::testing::Return;
+using ::testing::StrEq;
+
+TEST(StepwiseControllerTest, HysteresisTestPositive)
+{
+    // Verifies positive hysteresis works as expected
+
+    ZoneMock z;
+
+    std::vector<std::string> inputs = {"test"};
+    ec::StepwiseInfo initial;
+    initial.negativeHysteresis = 3.0;
+    initial.positiveHysteresis = 2.0;
+    initial.reading[0] = 20.0;
+    initial.reading[1] = 30.0;
+    initial.reading[2] = std::numeric_limits<float>::quiet_NaN();
+    initial.output[0] = 40.0;
+    initial.output[1] = 60.0;
+
+    std::unique_ptr<Controller> p =
+        StepwiseController::CreateStepwiseController(&z, "foo", inputs,
+                                                     initial);
+
+    EXPECT_CALL(z, getCachedValue(StrEq("test")))
+        .Times(3)
+        .WillOnce(Return(29.0))  // return 40
+        .WillOnce(Return(31.0))  // return 40
+        .WillOnce(Return(32.0)); // return 60
+
+    EXPECT_CALL(z, addRPMSetPoint(40.0)).Times(2);
+    EXPECT_CALL(z, addRPMSetPoint(60.0)).Times(1);
+
+    for (int ii = 0; ii < 3; ii++)
+    {
+        p->process();
+    }
+}
+
+TEST(StepwiseControllerTest, HysteresisTestNegative)
+{
+    // Verifies negative hysteresis works as expected
+
+    ZoneMock z;
+
+    std::vector<std::string> inputs = {"test"};
+    ec::StepwiseInfo initial;
+    initial.negativeHysteresis = 3.0;
+    initial.positiveHysteresis = 2.0;
+    initial.reading[0] = 20.0;
+    initial.reading[1] = 30.0;
+    initial.reading[2] = std::numeric_limits<float>::quiet_NaN();
+    initial.output[0] = 40.0;
+    initial.output[1] = 60.0;
+
+    std::unique_ptr<Controller> p =
+        StepwiseController::CreateStepwiseController(&z, "foo", inputs,
+                                                     initial);
+
+    EXPECT_CALL(z, getCachedValue(StrEq("test")))
+        .Times(3)
+        .WillOnce(Return(30.0))  // return 60
+        .WillOnce(Return(27.0))  // return 60
+        .WillOnce(Return(26.0)); // return 40
+
+    EXPECT_CALL(z, addRPMSetPoint(40.0)).Times(1);
+    EXPECT_CALL(z, addRPMSetPoint(60.0)).Times(2);
+
+    for (int ii = 0; ii < 3; ii++)
+    {
+        p->process();
+    }
+}