diff --git a/src/shared/scheduler/TaskScheduler.cpp b/src/shared/scheduler/TaskScheduler.cpp
index 3b4ae20d36b87fe523365abb429fcf780bf18fa0..a071374d1aa8ba12b98881314fd6a76c1d3c339a 100644
--- a/src/shared/scheduler/TaskScheduler.cpp
+++ b/src/shared/scheduler/TaskScheduler.cpp
@@ -35,15 +35,23 @@ namespace Boardcore
 TaskScheduler::TaskScheduler()
     : ActiveObject(STACK_MIN_FOR_SKYWARD, miosix::PRIORITY_MAX - 1)
 {
-    stopFlag = false;
-    running  = false;
+    stopFlag       = false;
+    running        = false;
+    possibleFreeID = 1;
 }
 
-uint8_t TaskScheduler::addTask(function_t function, uint32_t period, uint8_t id,
+uint8_t TaskScheduler::addTask(function_t function, uint32_t period,
                                Policy policy, int64_t startTick)
 {
+
     Lock<FastMutex> lock(mutex);
+    uint8_t id = possibleFreeID;
+
+    // Find a suitable id for the new task
+    for (; id < 255 && tasks[id] != nullptr; ++id)
+        ;
 
+    possibleFreeID = id + 1;
     // Check if in the corresponding id there's already a task
     if (tasks[id] != nullptr)
     {
@@ -67,18 +75,6 @@ uint8_t TaskScheduler::addTask(function_t function, uint32_t period, uint8_t id,
     return id;
 }
 
-uint8_t TaskScheduler::addTask(function_t function, uint32_t period,
-                               Policy policy, int64_t startTick)
-{
-    uint8_t id = 1;
-
-    // Find a suitable id for the new task
-    for (; id < 255 && tasks[id] != nullptr; ++id)
-        ;
-
-    return addTask(function, period, id, policy, startTick);
-}
-
 bool TaskScheduler::removeTask(uint8_t id)
 {
     Lock<FastMutex> lock(mutex);
@@ -97,6 +93,8 @@ bool TaskScheduler::removeTask(uint8_t id)
     // We do not deallocate the task here because of future deallocation when
     // popped from queue
     tasks[id] = nullptr;
+    if (id < possibleFreeID)
+        possibleFreeID = id;
 
     return true;
 }
@@ -251,6 +249,8 @@ void TaskScheduler::enqueue(Event& event, int64_t startTick)
             // If the task is one shot we won't push it to the agenda and we'll
             // remove it from the tasks map.
             tasks[event.task->id] = nullptr;
+            if (event.task->id < possibleFreeID)
+                possibleFreeID = event.task->id;
             delete event.task;
 
             return;
diff --git a/src/shared/scheduler/TaskScheduler.h b/src/shared/scheduler/TaskScheduler.h
index 119605437a6bd5c2672ebce6a599a8e5a1780a60..daa18abdd1f52b0d6644b7f6287b5f792e5c5ad1 100644
--- a/src/shared/scheduler/TaskScheduler.h
+++ b/src/shared/scheduler/TaskScheduler.h
@@ -162,27 +162,6 @@ private:
 
     void run() override;
 
-    /**
-     * @brief Add a task function to the scheduler.
-     *
-     * Note that each task has it's own unique ID, even one shot tasks!
-     * Therefore, if a task already exists with the same id, the function will
-     * fail and return false.
-     *
-     * For one shot tasks, the period is used as a delay. If 0 the task will be
-     * executed immediately, otherwise after the given period.
-     *
-     * @param function Function to be called periodically.
-     * @param period Inter call period.
-     * @param id Task identification number.
-     * @param policy Task policy, default is SKIP.
-     * @param startTick First activation time, useful for synchronizing tasks.
-     * @return true if the task was added successfully.
-     */
-    uint8_t addTask(function_t function, uint32_t period, uint8_t id,
-                    Policy policy     = Policy::SKIP,
-                    int64_t startTick = miosix::getTick());
-
     /**
      * @brief Update task statistics (Intended for when the task is executed).
      *
@@ -223,6 +202,7 @@ private:
     std::array<Task*, 256> tasks{};     ///< Holds all tasks to be scheduled.
     miosix::ConditionVariable condvar;  ///< Used when agenda is empty.
     std::priority_queue<Event> agenda;  ///< Ordered list of functions.
+    uint8_t possibleFreeID;
 
     PrintLogger logger = Logging::getLogger("taskscheduler");
 };
diff --git a/src/tests/catch/test-sensormanager-catch.cpp b/src/tests/catch/test-sensormanager-catch.cpp
index 1bead9c7c321e4110ac29a6a4c249340a98d9d8d..9ae7ca2c111d41eae695fcb99f758245284eac8f 100644
--- a/src/tests/catch/test-sensormanager-catch.cpp
+++ b/src/tests/catch/test-sensormanager-catch.cpp
@@ -37,7 +37,7 @@
 
 using namespace Boardcore;
 
-static const uint8_t FIRST_TASK_ID = 7;  // used to test IDs assignment to tasks
+static const uint8_t FIRST_TASK_ID = 1;  // used to test IDs assignment to tasks
 
 class FailingSensorCatch : public Sensor<TestData>
 {
@@ -54,9 +54,9 @@ public:
     SensorManagerFixture()
     {
         scheduler = new TaskScheduler();
+        // inserst a test function in the scheduler
         scheduler->addTask([]() { std::cout << "Task Callback!" << std::endl; },
-                           2000,  // inserst a test function in the scheduler
-                           FIRST_TASK_ID);
+                           2000);
 
         sensorManager = new SensorManager({{&s1, s1_info},
                                            {&s2, s2_info},