fix the control. in this version Motors PID wors for the first time

2 files changed, 3 insertions, 0 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
index 4e16a5599..d9571f3ff 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
@@ -211,6 +211,7 @@ uint32_t MillisecLoop(uint32_t tick)
             }
         }
     }
+    Dancer_Data[FEEDER_DANCER] = Read_Dancer_Position(FEEDER_DANCER);
 
 #ifndef EVALUATION_BOARD
     /* this cannot be done within one millisecond, and not needed
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/control.c b/Software/Embedded_SW/Embedded/Modules/Control/control.c
index 6bf238507..28b4afb8d 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/control.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/control.c
@@ -52,6 +52,7 @@
 
 #include "drivers/adc_sampling/adc.h"
 #include "control.h"
+#include "MillisecTask.h"
 /******************** Definitions  ********************************************/
 #define INVALID_MSG_ID    0xFFFF
 #define MAX_TANGO_CONTROL_DEVICES 200
@@ -152,6 +153,7 @@ void ControlStart(void)
     if (ControlRestart == false)
     {
         ControlRestart = true;
+        MillisecStart();
         ROM_TimerLoadSet(Control_timerBase, TIMER_A,120000+(ControlPhaseDelay*120)/*one millisecond*/);
         TimerEnable(Control_timerBase, TIMER_A);