diff --git a/App/Inc/sta/config.hpp b/App/Inc/sta/config.hpp
index 8fba61b..f46a2d7 100644
--- a/App/Inc/sta/config.hpp
+++ b/App/Inc/sta/config.hpp
@@ -30,6 +30,7 @@
 
 // Settings for TACOS
 #define STA_TACOS_MANAGER_PRIORITY osPriorityNormal
+#define STA_TACOS_STATEMACHINE_PRIORITY osPriorityNormal
 #define STA_TACOS_NUM_STATES 4
 #define STA_TACOS_INITIAL_STATE 0
 
diff --git a/App/Src/startup.cpp b/App/Src/startup.cpp
index a6265f9..3467975 100644
--- a/App/Src/startup.cpp
+++ b/App/Src/startup.cpp
@@ -28,9 +28,10 @@ namespace sta
 		{
 			STA_DEBUG_PRINTLN("Starting manager task!");
 
-			Manager::instance()->registerThread(std::make_unique<demo::DummyTask>("A"), {0, 1});
-			Manager::instance()->registerThread(std::make_unique<demo::DummyTask>("B"), {0, 2});
-			Manager::instance()->registerThread(std::make_unique<demo::DummyTask>("C"), {0, 3});
+			Manager::instance()->registerThread(std::make_shared<demo::DummyTask>("A"), {0, 1});
+			Manager::instance()->registerThread(std::make_shared<demo::DummyTask>("B"), {0, 2});
+			Manager::instance()->registerThread(std::make_shared<demo::DummyTask>("C"), {0, 1});
+			Manager::instance()->registerThread(std::make_shared<demo::DummyTask>("D"), {0, 2});
 		}
 	} // namespace tacos
 } // namespace sta
diff --git a/Core/Src/freertos.c b/Core/Src/freertos.c
index 2cc7d3e..64b2b36 100644
--- a/Core/Src/freertos.c
+++ b/Core/Src/freertos.c
@@ -52,7 +52,7 @@ typedef StaticSemaphore_t osStaticMutexDef_t;
 osThreadId_t defaultTaskHandle;
 const osThreadAttr_t defaultTask_attributes = {
   .name = "defaultTask",
-  .stack_size = 128 * 4,
+  .stack_size = 256 * 4,
   .priority = (osPriority_t) osPriorityNormal,
 };
 /* Definitions for uartMutex */
diff --git a/TACOS-Demo.ioc b/TACOS-Demo.ioc
index 78aba71..1d7d028 100644
--- a/TACOS-Demo.ioc
+++ b/TACOS-Demo.ioc
@@ -5,7 +5,7 @@ CAD.provider=
 FREERTOS.FootprintOK=true
 FREERTOS.IPParameters=Tasks01,configUSE_NEWLIB_REENTRANT,configRECORD_STACK_HIGH_ADDRESS,configCHECK_FOR_STACK_OVERFLOW,Mutexes01,FootprintOK,configTIMER_TASK_PRIORITY
 FREERTOS.Mutexes01=uartMutex,Static,uartMutex_cb
-FREERTOS.Tasks01=defaultTask,24,128,StartDefaultTask,Default,NULL,Dynamic,NULL,NULL
+FREERTOS.Tasks01=defaultTask,24,256,StartDefaultTask,Default,NULL,Dynamic,NULL,NULL
 FREERTOS.configCHECK_FOR_STACK_OVERFLOW=1
 FREERTOS.configRECORD_STACK_HIGH_ADDRESS=1
 FREERTOS.configTIMER_TASK_PRIORITY=48
diff --git a/Tacos/include/sta/tacos/statemachine.hpp b/Tacos/include/sta/tacos/statemachine.hpp
index 749ab8b..443d4b4 100644
--- a/Tacos/include/sta/tacos/statemachine.hpp
+++ b/Tacos/include/sta/tacos/statemachine.hpp
@@ -10,11 +10,6 @@
 
 #include <sta/config.hpp>
 
-#include <sta/tacos/thread.hpp>
-#include <sta/rtos/timer.hpp>
-
-#include <sta/debug/assert.hpp>
-
 
 #ifndef STA_TACOS_NUM_STATES
 #	error "Number of states wasn't defined in config.hpp"
@@ -26,6 +21,13 @@
 #endif
 
 
+#include <sta/tacos/thread.hpp>
+#include <sta/rtos/timer.hpp>
+#include <sta/debug/assert.hpp>
+
+#include <functional>
+
+
 namespace sta
 {
 	namespace tacos
@@ -52,6 +54,8 @@ namespace sta
 
 			uint16_t getCurrentState() const;
 
+			void setStateTransitionFunction(uint16_t (*function)(uint16_t));
+
 		private:
 			static Statemachine * _instance;
 
@@ -81,6 +85,7 @@ namespace sta
 			uint16_t currentState_;
 			RtosTimer lockoutTimer_;
 			RtosTimer failsafeTimer_;
+			std::function<uint16_t(uint16_t)> transitionFunc_;
 		};
 	}  // namespace tacos
 }  // namespace sta
diff --git a/Tacos/src/statemachine.cpp b/Tacos/src/statemachine.cpp
index 244ccb1..9d607a4 100644
--- a/Tacos/src/statemachine.cpp
+++ b/Tacos/src/statemachine.cpp
@@ -15,7 +15,8 @@ namespace sta
 	namespace tacos
 	{
 		Statemachine::Statemachine()
-			: currentState_{STA_TACOS_INITIAL_STATE},
+			: TacosThread{"Statemachine", STA_TACOS_STATEMACHINE_PRIORITY},
+			  currentState_{STA_TACOS_INITIAL_STATE},
 			  lockoutTimer_{[](void *){}, nullptr},
 			  failsafeTimer_{[](void *){}, nullptr}
 		{
@@ -24,18 +25,30 @@ namespace sta
 
 		void Statemachine::init()
 		{
-			STA_DEBUG_PRINTLN("INITIALIZING STATEMACHINE");
-
-			lockoutTimer_.setCallback([](void *) { STA_DEBUG_PRINTLN("[LOCKOUT TIMEOUT]"); }, nullptr);
+			lockoutTimer_.setCallback([](void *) {  }, nullptr);
 			lockoutTimer_.start(5000);
 
-			failsafeTimer_.setCallback([](void * arg) { STA_DEBUG_PRINTLN("[FAILSAFE TIMEOUT]"); }, nullptr);
+			STA_DEBUG_PRINTLN("INITIALIZING STATEMACHINE");
+
+			failsafeTimer_.setCallback([](void * arg) {  }, nullptr);
 			failsafeTimer_.start(10000);
 		}
 
 		void Statemachine::func()
 		{
+			/*
+			uint16_t next = transitionFunc_(currentState_);
 
+			STA_ASSERT(next < STA_TACOS_NUM_STATES);
+
+			if (next != currentState_)
+			{
+				currentState_ = next;
+
+				// TODO: Emit state transition event.
+			}*/
+			STA_DEBUG_PRINTLN(lockoutTimer_.isRunning() ? "[RUNNING]" : "[STOPPED]");
+			osDelay(1000);
 		}
 
 		uint16_t Statemachine::getCurrentState() const
@@ -43,6 +56,13 @@ namespace sta
 			return currentState_;
 		}
 
+		void Statemachine::setStateTransitionFunction(uint16_t (*function)(uint16_t))
+		{
+			STA_ASSERT(function != nullptr);
+
+			transitionFunc_ = function;
+		}
+
 		Statemachine* Statemachine::_instance = nullptr;
 
 	}  // namespace tacos