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

3 files changed, 87 insertions, 7 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
index 177a3678f..6f234e26a 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
@@ -77,6 +77,8 @@ uint32_t DrawerFansStatus = 0;
 
 bool watchdogCriticalAlarm = false;
 
+MillisecMotorDataStruc ScrewSetMaxSpeedPending = {0};
+MillisecMotorDataStruc ScrewMovePending = {0};
 MillisecMotorDataStruc MotorData[NUM_OF_MOTORS] = {0};
 MillisecMotorDataStruc SpeedSetPending[NUM_OF_MOTORS] = {0};
 MillisecMotorDataStruc PT100Data[MAX_TEMPERATURE_SENSOR_ID] = {0};
@@ -186,6 +188,51 @@ 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;
+    MsecLogindex++;
+    if (MsecLogindex>=199)
+        MsecLogindex = 0;
+    //==========================
+
+    ScrewMovePending.Callback = Callback;
+    ScrewMovePending.Data = Data;
+    ScrewMovePending.Length = Length;
+    ScrewMovePending.DataRequired = false;
+    if (ScrewMovePending.Active == false)
+    {
+        MotorActivity++;
+        ScrewMovePending.Active = true;
+    }
+
+    return OK;
+}
+
+int32_t MillisecSetScrewSpeed(unsigned long Data, int Length, MSecFptr Callback)
+{
+    //==========================
+    MSBacklog[MsecLogindex]=Data;
+    Motor_Id[MsecLogindex]=HARDWARE_MOTOR_TYPE__MOTO_SCREW;
+    MsecLogindex++;
+    if (MsecLogindex>=199)
+        MsecLogindex = 0;
+    //==========================
+
+    ScrewSetMaxSpeedPending.Callback = Callback;
+    ScrewSetMaxSpeedPending.Data = Data;
+    ScrewSetMaxSpeedPending.Length = Length;
+    ScrewSetMaxSpeedPending.DataRequired = false;
+    if (ScrewSetMaxSpeedPending.Active == false)
+    {
+        MotorActivity++;
+        ScrewSetMaxSpeedPending.Active = true;
+    }
+
+    return OK;
+}
 int32_t MillisecSetMotorSpeed(TimerMotors_t MotorId, unsigned long Data, int Length, MSecFptr Callback)
 {
     if (MotorId >= NUM_OF_MOTORS) return -1;
@@ -273,11 +320,12 @@ uint32_t MillisecLoop(uint32_t tick)
     //call all modules Millisec functions
     //test dancers and speed encoders
     //check all callback units (state machine waiting for completion of a change)
-    bool Ten_msTick, Hundred_msTick, Onesecond_Tick,O900Millisecond_Tick,Tick98;
+    bool Ten_msTick, Hundred_msTick, Onesecond_Tick,O900Millisecond_Tick,Tick98,OneMinute_Tick;
     Ten_msTick      =   (tick%eTenMillisecond == 0)      ?true:false;
     Hundred_msTick  =   (tick%eHundredMillisecond == 0)  ?true:false;
     O900Millisecond_Tick  =   (tick%eOneSecond == 900)           ?true:false;
     Onesecond_Tick  =   (tick%eOneSecond == 0)           ?true:false;
+    OneMinute_Tick  =   (tick%eOneMinute == 0)           ?true:false;
     Tick98          =   (tick%eHundredMillisecond == 99) ?true:false;
     //gather Motor data from FPGA
     //ROM_IntMasterDisable();
@@ -308,6 +356,25 @@ uint32_t MillisecLoop(uint32_t tick)
                 }
                 MotorActivity--;
             }
+
+            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);
+
+            }
             if (SpeedSetPending[Motor_i].Active == true)
             {
                 MotorSendFPGARequest((HardwareMotorType)Motor_i,SpeedSetPending[Motor_i].Data,SpeedSetPending[Motor_i].Length);
@@ -377,6 +444,7 @@ uint32_t MillisecLoop(uint32_t tick)
     }
     if (Hundred_msTick)
     {
+//        Speed_Data = Calculate_Speed_Sensor_Velocity();
         for (Sensor_i = 0;Sensor_i < MAX_TEMPERATURE_SENSOR_ID;Sensor_i++)
         {
             MillisecReadFromTempSensor(Sensor_i, NULL);
@@ -393,11 +461,6 @@ uint32_t MillisecLoop(uint32_t tick)
         {
             CalculateDispenserPressure(Disp_i);
         }
-        for (Motor_i = 0;Motor_i < NUM_OF_MOTORS;Motor_i++)
-        {
-            if (isMotorConfigured(Motor_i))
-                MotorGetStatusFromFPGA(Motor_i);
-        }
         FPGA_GetAllDispensersValveBusyOCD();
         if (watchdogCriticalAlarm == false)
         {
@@ -405,6 +468,20 @@ uint32_t MillisecLoop(uint32_t tick)
         }
         DrawerFansStatus = Read_Fans_Tacho();
     }
+    if (OneMinute_Tick)
+    {
+        for (Disp_i = 0;Disp_i < MAX_SYSTEM_DISPENSERS;Disp_i++)
+        {
+            Read_MidTank_Pressure_Sensor(Disp_i);
+        }
+        for (Motor_i = 0;Motor_i < NUM_OF_MOTORS;Motor_i++)
+        {
+            if (Motor_i == HARDWARE_MOTOR_TYPE__MOTO_SCREW)
+                continue;  //
+            if (isMotorConfigured(Motor_i))
+                MotorGetStatusFromFPGA(Motor_i);
+        }
+    }
     //ROM_IntMasterEnable();
 
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.h b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.h
index 1c1989c1e..517716c08 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.h
+++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.h
@@ -15,6 +15,8 @@
 
 typedef  uint32_t (* MSecFptr)(uint32_t deviceID, uint32_t ReadValue);
 int32_t MillisecWriteToMotor(TimerMotors_t MotorId, unsigned long Data, int Length, MSecFptr Callback);
+int32_t MillisecMoveScrew(unsigned long Data, int Length, MSecFptr Callback);
+int32_t MillisecSetScrewSpeed(unsigned long Data, int Length, MSecFptr Callback);
 int32_t MillisecSetMotorSpeed(TimerMotors_t MotorId, unsigned long Data, int Length, MSecFptr Callback);
 int32_t MillisecReadFromMotor(TimerMotors_t MotorId, unsigned long Data, int Length, MSecFptr Callback);
 int MillisecFlushMsgQ(TimerMotors_t MotorId);
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/control.h b/Software/Embedded_SW/Embedded/Modules/Control/control.h
index d032c7bd1..0aba4b4ba 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/control.h
+++ b/Software/Embedded_SW/Embedded/Modules/Control/control.h
@@ -18,7 +18,8 @@ typedef enum {
     eOneMillisecond = 1,
     eTenMillisecond = 10,
     eHundredMillisecond = 100,
-    eOneSecond = 1000
+    eOneSecond = 1000,
+    eOneMinute = 60000
 }CTRL_TIMING_ENUM;
 typedef enum
 {