mERGE

1 files changed, 32 insertions, 1 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
index c242401fd..e0abf6c2b 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
@@ -24,6 +24,7 @@
 #include "control.h"
 
 #include "drivers/Motors/Motor.h"
+#include "drivers/Danser_SSI/SSI_Comm.h"
 #include "drivers/Heater/TemperatureSensor.h"
 
 /******************** Definitions  ********************************************/
@@ -49,6 +50,7 @@ uint32_t  MotorSpeed_Data[MOTOR_SPARE1_1] = {0};
 uint32_t  MotorStatus_Data[MOTOR_SPARE1_1] = {0};
 uint32_t  MotorPosition_Data[MOTOR_SPARE1_1] = {0};
 bool      MotorBusy_Data[MOTOR_SPARE1_1] = {true};
+uint32_t  Dancer_Data[NUM_OF_DANCERS] = {0};
 
 /******************** GLOBAL PARAMETERS  ********************************************/
 Mailbox_Handle          MillisecMsgQ = NULL;
@@ -58,7 +60,7 @@ static GateMutex_Handle gateMillisecDB;
 uint32_t                MillisecDatalog[MAX_TANGO_CONTROL_DEVICES];
 uint32_t        Millisec_timerBase = TIMER1_BASE;        //Timer handle
 /******************** Functions  ********************************************/
-
+uint32_t Control_Delta_Position_Pass(uint32_t Current_Read,uint32_t Previous_Read);
 //**********************************************************************
 /******************** CODE  ********************************************/
 //**********************************************************************
@@ -154,6 +156,9 @@ uint32_t MillisecLoop(uint32_t tick)
     MotorSpeed_Data[MOTOR_RDRIVING] = MotorGetSpeedFromFPGA(MOTOR_RDRIVING);
     MotorStatus_Data[MOTOR_RDRIVING] = MotorGetStatusFromFPGA(MOTOR_RDRIVING);
     //gather Dancer data from FPGA
+    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);
     //gather data from FPGA
     if (Ten_msTick)
     {
@@ -240,3 +245,29 @@ uint32_t getADCData(int DeviceId)
     return ADC_Data[DeviceId];
 }
 
+/********************************************************************
+*
+*    Name        : GTIME_Delta_Time_Pass
+*
+*    Parameters  : start_time.
+*
+*    Return      : time pass from start time
+*
+*    Description :
+*
+*********************************************************************/
+
+uint32_t Control_Delta_Position_Pass(uint32_t Current_Read,uint32_t Previous_Read)
+{
+    uint32_t Time_Pass;
+  #define   MAX_COUNTER 0x3FFF  //14 bits
+
+
+  if (Current_Read < Previous_Read)
+    Time_Pass = (MAX_COUNTER - Previous_Read) + Current_Read + 1;
+  else
+    Time_Pass = Current_Read - Previous_Read;
+
+  return (Time_Pass);
+}
+