1 files changed, 9 insertions, 3 deletions
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
index 67a532e78..d334c9001 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
@@ -490,9 +490,15 @@ uint32_t MillisecLoop(uint32_t tick)
 #ifdef HUNDRED_MICROSECONDS_DANCER_READ
    SaveLogData();
 #else
-    Dancer_Data[FEEDER_DANCER] = Read_Dancer_Position(FEEDER_DANCER);
-    Dancer_Data[POOLER_DANCER] = Read_Dancer_Position(POOLER_DANCER);
-    Dancer_Data[WINDER_DANCER] = Read_Dancer_Position(WINDER_DANCER);
+
+    #ifdef test_RTFU_dancer
+       test_dancer_responce_RTFU();
+    #else
+        Dancer_Data[FEEDER_DANCER] = Read_Dancer_Position(FEEDER_DANCER);
+        Dancer_Data[POOLER_DANCER] = Read_Dancer_Position(POOLER_DANCER);
+        Dancer_Data[WINDER_DANCER] = Read_Dancer_Position(WINDER_DANCER);
+    #endif
+
 #endif
     return OK;
 }