Merge branch 'master' of https://twinetfs.visualstudio.com/Tango/_git/Tango

1 files changed, 33 insertions, 17 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
index 2243007b8..ec4e336cd 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
@@ -80,6 +80,8 @@ uint32_t DrawerFansStatus = 0;
 
 bool watchdogCriticalAlarm = false;
 
+uint32_t msec_millisecondCounter = 0;
+
 MillisecMotorDataStruc ScrewSetMaxSpeedPending = {0};
 MillisecMotorDataStruc ScrewMovePending = {0};
 MillisecMotorDataStruc MotorData[NUM_OF_MOTORS] = {0};
@@ -99,6 +101,7 @@ uint32_t Control_Delta_Position_Pass(uint32_t Current_Read,uint32_t Previous_Rea
 //**********************************************************************
 uint32_t MSBacklog[200]={0};
 uint8_t Motor_Id[200]={0};
+uint32_t MSTick[200]={0};
 uint16_t MsecLogindex = 0;
 
 void MillisecInit(void)
@@ -150,7 +153,6 @@ void MillisecStart(void)
     ADCAcquireStart(0,1);
 }
 
-uint32_t msec_millisecondCounter = 0;
 void OneMilliSecondMillisecInterrupt(UArg arg0)
 {
     MillisecMessageStruc Message;
@@ -191,14 +193,18 @@ int32_t MillisecReadFromTempSensor(uint32_t TempSensorId,  MSecFptr Callback)
 }
 //typedef  uint32_t (* MSecFptr)(uint32_t deviceID, uint32_t ReadValue);
 uint32_t MotorActivity = 0;
+
 int32_t MillisecMoveScrew(unsigned long Data, int Length, MSecFptr Callback)
 {
     //==========================
     MSBacklog[MsecLogindex]=Data;
     Motor_Id[MsecLogindex]=HARDWARE_MOTOR_TYPE__MOTO_SCREW;
+    MSTick[MsecLogindex]=msec_millisecondCounter;
     MsecLogindex++;
     if (MsecLogindex>=199)
         MsecLogindex = 0;
+    LOG_ERROR(Data, "MillisecMoveScrew");
+
     //==========================
 
     ScrewMovePending.Callback = Callback;
@@ -219,10 +225,12 @@ int32_t MillisecSetScrewSpeed(unsigned long Data, int Length, MSecFptr Callback)
     //==========================
     MSBacklog[MsecLogindex]=Data;
     Motor_Id[MsecLogindex]=HARDWARE_MOTOR_TYPE__MOTO_SCREW;
+    MSTick[MsecLogindex]=msec_millisecondCounter;
     MsecLogindex++;
     if (MsecLogindex>=199)
         MsecLogindex = 0;
     //==========================
+    LOG_ERROR(Data, "MillisecSetScrewSpeed");
 
     ScrewSetMaxSpeedPending.Callback = Callback;
     ScrewSetMaxSpeedPending.Data = Data;
@@ -241,6 +249,7 @@ int32_t MillisecSetMotorSpeed(TimerMotors_t MotorId, unsigned long Data, int Len
     if (MotorId >= NUM_OF_MOTORS) return -1;
     //==========================
     MSBacklog[MsecLogindex]=Data;
+    MSTick[MsecLogindex]=msec_millisecondCounter;
     Motor_Id[MsecLogindex]=MotorId;
     MsecLogindex++;
     if (MsecLogindex>=199)
@@ -276,6 +285,7 @@ int32_t MillisecWriteToMotor(TimerMotors_t MotorId, unsigned long Data, int Leng
     MillisecMotorDataStruc MotorData = {0};
     //==========================
     MSBacklog[MsecLogindex]=Data;
+    MSTick[MsecLogindex]=msec_millisecondCounter;
     Motor_Id[MsecLogindex]=MotorId;
     MsecLogindex++;
     if (MsecLogindex>=199)
@@ -298,6 +308,7 @@ int32_t MillisecReadFromMotor(TimerMotors_t MotorId, unsigned long Data, int Len
 //==========================
     MSBacklog[MsecLogindex]=Data;
     Motor_Id[MsecLogindex]=MotorId;
+    MSTick[MsecLogindex]=msec_millisecondCounter;
     MsecLogindex++;
     if (MsecLogindex>=199)
         MsecLogindex = 0;
@@ -360,23 +371,28 @@ uint32_t MillisecLoop(uint32_t tick)
                 MotorActivity--;
             }
 
-            if (ScrewSetMaxSpeedPending.Active == true)
+            if (Motor_i == HARDWARE_MOTOR_TYPE__MOTO_SCREW)
             {
-                MotorSendFPGARequest(HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewSetMaxSpeedPending.Data,ScrewSetMaxSpeedPending.Length);
-                MotorActivity--;
-                ScrewSetMaxSpeedPending.Active = false;
-                if (ScrewSetMaxSpeedPending.Callback)
-                    ScrewSetMaxSpeedPending.Callback(Motor_i,0);
+                if (ScrewSetMaxSpeedPending.Active == true)
+                {
+                    MotorSendFPGARequest(HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewSetMaxSpeedPending.Data,ScrewSetMaxSpeedPending.Length);
+                    MotorActivity--;
+                    ScrewSetMaxSpeedPending.Active = false;
+                    if (ScrewSetMaxSpeedPending.Callback)
+                        ScrewSetMaxSpeedPending.Callback(Motor_i,0);
 
-            }
-            else if (ScrewMovePending.Active == true)
-            {
-                MotorSendFPGARequest(HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewMovePending.Data,ScrewMovePending.Length);
-                MotorActivity--;
-                ScrewMovePending.Active = false;
-                if (ScrewMovePending.Callback)
-                    ScrewMovePending.Callback(Motor_i,0);
+                }
+                else if (ScrewMovePending.Active == true)
+                {
+                    MotorSendFPGARequest(HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewMovePending.Data,ScrewMovePending.Length);
+                    MotorActivity--;
+                    /*if (Screwindex>=50)
+                        Screwindex = 0;*/
+                    ScrewMovePending.Active = false;
+                    if (ScrewMovePending.Callback)
+                        ScrewMovePending.Callback(Motor_i,0);
 
+                }
             }
             if (SpeedSetPending[Motor_i].Active == true)
             {
@@ -441,13 +457,13 @@ uint32_t MillisecLoop(uint32_t tick)
     Dancer_Data[WINDER_DANCER] = Read_Dancer_Position(WINDER_DANCER);
     if (Ten_msTick)
     {
-        Speed_Data = Calculate_Speed_Sensor_Velocity();
+        //Speed_Data = Calculate_Speed_Sensor_Velocity();
         //MillisecReadFromTempSensor(Sensor_Read, NULL);
         //if (Sensor_Read++ >= MAX_TEMPERATURE_SENSOR_ID) Sensor_Read = 0;
     }
     if (Hundred_msTick)
     {
-//        Speed_Data = Calculate_Speed_Sensor_Velocity();
+        Speed_Data = Calculate_Speed_Sensor_Velocity();
         for (Sensor_i = 0;Sensor_i < MAX_TEMPERATURE_SENSOR_ID;Sensor_i++)
         {
             MillisecReadFromTempSensor(Sensor_i, NULL);