2 files changed, 78 insertions, 1 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
index fa168067f..4ff47b56c 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
@@ -33,6 +33,7 @@
 #include "drivers/Heater/TemperatureSensor.h"
 #include "drivers/FPGA/FPGA_SPI_Comm.h"
 #include "drivers/FPGA/FPGA.h"
+#include <drivers/FPGA/FPGA_Comm.h>
 #include "drivers/Valves/Valve.h"
 #include "drivers/FPGA/FPGA_GPIO/FPGA_GPIO.h"
 
@@ -67,6 +68,21 @@ typedef struct MillisecMessage{
     uint32_t tick;
     uint8_t messageData[20];
 }MillisecMessageStruc;
+typedef enum
+{
+    ScrewMessage,
+    //ScrewCalculateTemperature,
+}ScrewMessages;
+
+typedef struct ScrewMessage{
+    uint16_t messageId;
+    uint16_t msglen;
+    uint32_t tick;
+  //  uint32_t SensorId;
+  //  uint32_t Data;
+}ScrewMessageStruc;
+Task_Handle Screw_Task_Handle;
+Mailbox_Handle          ScrewMsgQ = NULL;
 
 //uint32_t  ADC_Data[MAX_ADC_DEVICES] = {0};
 //uint32_t  TemperatureSensor_Data[MAX_TEMPERATURE_SENSOR_ID] = {0};
@@ -111,7 +127,9 @@ void MillisecInit(void)
 
     Error_init(&eb);
 
-    MillisecMsgQ = Mailbox_create(sizeof(MillisecMessageStruc), 2, NULL,&eb);
+    MillisecMsgQ = Mailbox_create(sizeof(MillisecMessageStruc), 5, NULL,&eb);
+    ScrewMsgQ = Mailbox_create(sizeof(ScrewMessageStruc), 5, NULL,&eb);
+
     for (i=0;i<NUM_OF_MOTORS;i++)
     {
         MotorsMsgQ[i] = Mailbox_create(sizeof(MillisecMotorDataStruc), 5, NULL,&eb);
@@ -152,6 +170,16 @@ void MillisecStart(void)
     ROM_TimerIntEnable(Millisec_timerBase, TIMER_TIMA_TIMEOUT);
     ADCAcquireStart(0,1);
 }
+void ScrewFreeInterrupt(void)
+{
+    MillisecMessageStruc Message;
+    Message.messageId = ScrewMessage;
+    if (ScrewMsgQ != NULL)
+        Mailbox_post(ScrewMsgQ , &Message, BIOS_NO_WAIT);
+
+    ROM_IntMasterEnable();
+    return ;
+}
 
 void OneMilliSecondMillisecInterrupt(UArg arg0)
 {
@@ -507,6 +535,27 @@ uint32_t MillisecLoop(uint32_t tick)
 
     return OK;
 }
+int SendNextScrewMessage()
+{
+    volatile uint32_t temp; // read the busy value
+
+    if (Mailbox_pend(MotorsMsgQ[HARDWARE_MOTOR_TYPE__MOTO_SCREW] , &MotorData[HARDWARE_MOTOR_TYPE__MOTO_SCREW], BIOS_NO_WAIT)==true)
+    {
+        if (MotorSendFPGARequest((HardwareMotorType)HARDWARE_MOTOR_TYPE__MOTO_SCREW,MotorData[HARDWARE_MOTOR_TYPE__MOTO_SCREW].Data,MotorData[HARDWARE_MOTOR_TYPE__MOTO_SCREW].Length) != OK) //sent the data to the FPGA
+        {
+            LOG_ERROR(MotorData[HARDWARE_MOTOR_TYPE__MOTO_SCREW].Data,"SCREW Send message failed");
+        }
+        else
+        {
+            LOG_ERROR(MotorData[HARDWARE_MOTOR_TYPE__MOTO_SCREW].Data,"SCREW Send message OK");
+            MotorActivity--;
+        }
+    }
+    ROM_SysCtlDelay(SYS_CLK_FREQ / 50000);
+
+    temp = F1_Moto_Driver_NBUSY2_L; // read the latch in order to clear the latch value (read it after moving to prevent interrupt before movement)
+
+}
 /******************************************************************************
  *  ======== messageTsk ========
  *  Task for this function is created statically. See the project's .cfg file.
@@ -567,3 +616,30 @@ uint32_t getDrawerFansStatus(void)
 {
     return DrawerFansStatus;
 }
+
+/******************************************************************************
+ *  ======== messageTsk ========
+ *  Task for this function is created statically. See the project's .cfg file.
+ *  this message task is created statically in system initialization,
+ ******************************************************************************/
+void ScrewTask(UArg arg0, UArg arg1)
+{
+    ScrewMessageStruc Message;
+    //char str[60];
+    //uint16_t length;
+    //Clock_setTimeout(HostKAClock, 1000);
+    //Clock_start(HostKAClock);
+    Screw_Task_Handle = Task_self();
+    while(1)
+    {
+        Mailbox_pend(ScrewMsgQ , &Message, BIOS_WAIT_FOREVER);
+        switch (Message.messageId)
+        {
+        case ScrewMessage:
+            SendNextScrewMessage();
+            break;
+            default:
+                break;
+        }
+    }
+}
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.h b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.h
index 517716c08..6e11471cc 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.h
+++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.h
@@ -35,6 +35,7 @@ uint32_t getDrawerFansStatus(void);
 void MillisecInit(void);
 void MillisecStop(void);
 void MillisecStart(void);
+void ScrewFreeInterrupt(void);
 
 extern bool watchdogCriticalAlarm;