Solved a bug: incremented the current servo index after each iteration.

src/motion_manager.c

@@ -295,7 +295,7 @@ void manager_init(uint16_t period_us)
       // set the action current angles
       manager_current_angles[i]=manager_servos[i].current_angle<<9;
       manager_num_servos++;
-      current=0;
+      current++;
     }
     else
     {