[TaskScheduler] Fixed pipeline

src/shared/scheduler/TaskScheduler.cpp

@@ -73,7 +73,7 @@ uint8_t TaskScheduler::addTask(function_t function, uint32_t period,
     uint8_t id = 1;
 
     // Find a suitable id for the new task
-    for (; tasks[id] != nullptr && id < 255; ++id)
+    for (; id < 255 && tasks[id] != nullptr; ++id)
         ;
 
     return addTask(function, period, id, policy, startTick);

src/tests/actuators/test-servo.cpp

@@ -80,7 +80,7 @@ int main()
 
     // Start a periodic task to move the first three servos
     TaskScheduler scheduler;
-    uint8_t id = scheduler.addTask(&moveServo, 2 * 1000);
+    scheduler.addTask(&moveServo, 2 * 1000);
     scheduler.start();
 
     // Control the fourth servo manually