many logs. improve job handling and IDS. NEW WINDER ALGORITHM (Interrupts)

6 files changed, 184 insertions, 102 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Control/control.c b/Software/Embedded_SW/Embedded/Modules/Control/control.c
index 78cdd5324..2110029e4 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/control.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/control.c
@@ -202,7 +202,7 @@ uint32_t  AddControlCallback( ControlCBFunction Callback, CTRL_TIMING_ENUM CtrlF
     unsigned int key;
 
     uint32_t device_i;
-    uint32_t deviceId = 0xFFFFFFFF;
+    uint32_t deviceId = 0xFF;
     for(device_i = 0;device_i < MAX_TANGO_CONTROL_DEVICES;device_i++)
     {
         if (ControlArray[device_i].ControlActive == false)
@@ -211,8 +211,8 @@ uint32_t  AddControlCallback( ControlCBFunction Callback, CTRL_TIMING_ENUM CtrlF
             break;
         }
     }
-    if (deviceId == 0xFFFFFFFF)
-        return 0xFFFFFFFF;
+    if (deviceId == 0xFF)
+        return 0xFF;
     key = GateMutex_enter(gateControlDB);
     ControlArray[deviceId].ControlTiming = CtrlFrequency;
     ControlArray[deviceId].ControlCallbackPtr = Callback;
@@ -221,6 +221,7 @@ uint32_t  AddControlCallback( ControlCBFunction Callback, CTRL_TIMING_ENUM CtrlF
     ControlArray[deviceId].Parameter1 = Parameter1;
     ControlArray[deviceId].IfIndex = IfIndex;
     GateMutex_leave(gateControlDB, key);
+    //LOG_ERROR(deviceId, "Add Callback");
 
     return deviceId;
 
@@ -239,11 +240,17 @@ int     RemoveControlCallback(uint32_t deviceId , ControlCBFunction Callback)
         ControlArray[deviceId].ControlCallbackPtr = NULL;
         ControlArray[deviceId].ControlDataReadPtr = NULL;
         ControlArray[deviceId].ControlActive = false;
+        ControlArray[deviceId].Parameter1 = 0;
+        ControlArray[deviceId].IfIndex = 0;
+        //LOG_ERROR(deviceId, "Remove Callback ");
         GateMutex_leave(gateControlDB, key);
         return OK;
     }
     else
+    {
+        LOG_ERROR(deviceId, "Remove Callback failed");
         return ERROR;
+    }
 
 }
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.c b/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.c
index 01e683dae..50d5f2093 100644
--- a/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.c
+++ b/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.c
@@ -60,7 +60,7 @@ DiagnosticsMonitors DiagnosticsMonitor = DIAGNOSTICS_MONITORS__INIT;
 #define DIAGNOSTICS_LIMIT 5
 #define DIAGNOSTICS_DANCER_LIMIT 50
 int DiagnosticsIndex = 0;
-int DiagnosticCollectionLimit = 3;  //number of data samples to collect before sending to the host
+int DiagnosticCollectionLimit = 2;  //number of data samples to collect before sending to the host
 //int DiagnosticLimit = eHundredMillisecond; //frequency of data collection
 //int DiagnosticFastLimit = eTenMillisecond; //frequency of data collection
 int DiagnosticLimit =eHundredMillisecond; //frequency of data collection
@@ -249,9 +249,10 @@ void DiagnosticLoadTemperature(int HeaterId, int temperature)
 void DiagnosticLoadSpeedSensor(float value)
 {
     SpeedValue[SpeedCounterIndex] = (double)(value);
-    SpeedCounterIndex++;
-    if (SpeedCounterIndex>=DIAGNOSTICS_DANCER_LIMIT )
-        SpeedCounterIndex = 0;
+    if (SpeedCounterIndex<DIAGNOSTICS_DANCER_LIMIT )
+        SpeedCounterIndex++;
+    //if (SpeedCounterIndex>=DIAGNOSTICS_DANCER_LIMIT )
+    //    SpeedCounterIndex = 0;
 }
 
 void DiagnosticLoadDancer(int DancerId, uint16_t value)
@@ -262,9 +263,10 @@ void DiagnosticLoadDancer(int DancerId, uint16_t value)
         return;
     }
     DancerValue[DancerId][DancerCounterIndex[DancerId]] = value;
-    DancerCounterIndex[DancerId]++;
-    if (DancerCounterIndex[DancerId]>=DIAGNOSTICS_DANCER_LIMIT )
-        DancerCounterIndex[DancerId] = 0;
+    if (DancerCounterIndex[DancerId]<DIAGNOSTICS_DANCER_LIMIT )
+        DancerCounterIndex[DancerId]++;
+    //if (DancerCounterIndex[DancerId]>=DIAGNOSTICS_DANCER_LIMIT )
+    //    DancerCounterIndex[DancerId] = 0;
 }
 void DiagnosticLoadDancerError(int DancerId, double value)
 {
@@ -274,9 +276,10 @@ void DiagnosticLoadDancerError(int DancerId, double value)
         return;
     }
     DancerErrorValue[DancerId][DancerErrorCounterIndex[DancerId]] = value;
-    DancerErrorCounterIndex[DancerId]++;
-    if (DancerErrorCounterIndex[DancerId]>=DIAGNOSTICS_DANCER_LIMIT )
-        DancerErrorCounterIndex[DancerId] = 0;
+    if (DancerErrorCounterIndex[DancerId]<DIAGNOSTICS_DANCER_LIMIT )
+        DancerErrorCounterIndex[DancerId]++;
+    //if (DancerErrorCounterIndex[DancerId]>=DIAGNOSTICS_DANCER_LIMIT )
+    //    DancerErrorCounterIndex[DancerId] = 0;
 
 }
 void DiagnosticTenMsecCollection(void)
@@ -587,20 +590,20 @@ void SendDiagnostics(void)
                 {
                     DiagnosticsStop();
                 }
-	            else
+	            /*else
 	            {
 	                Report("Diagnostic Messgage",__FILE__,__LINE__,container_size,RpWarning,msec_millisecondCounter,  msec_millisecondCounter);
-	            }
-                diag_size[diag_index] = container_size;
-                diag_time[diag_index] = msec_millisecondCounter;
-                if (diag_index++>=MAX_DIAG_LOG)
-                    diag_index = 0;
+	            }*/
+                //diag_size[diag_index] = container_size;
+                //diag_time[diag_index] = msec_millisecondCounter;
+                //if (diag_index++>=MAX_DIAG_LOG)
+                //    diag_index = 0;
             }
             else
             {
                 LOG_ERROR(Task_self(),"my_malloc failed");
                 DiagnosticsStop();
-                CommunicationMailboxFlush();
+                //CommunicationMailboxFlush();
             }
         }
         else
diff --git a/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c b/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c
index 6222378bb..0e4466d81 100644
--- a/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c
@@ -358,7 +358,13 @@ uint32_t PrepareHeater(int HeaterId, uint32_t SetTemperatue)
 #endif
         InitialHeating = true;
         HeaterReady[HeaterId] = false;
-        Turn_the_Blower_Off();
+        if (BlowerCfg.enabled == true)
+        {
+            Turn_the_Blower_On();//Turn on with the Default_Voltage
+            if (BlowerCfg.voltage)
+                Control_Voltage_To_Blower(BlowerCfg.voltage-500);
+
+        }
     }
 /*    if (HeaterId == HARDWARE_PID_CONTROL_TYPE__DryerHeaterMain) //Dryer Heaters
     {
diff --git a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c
index 64855daed..57b346513 100644
--- a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c
@@ -192,11 +192,11 @@ bool IDS_isDispenserUsedNextSegment(void *JobDetails,int DispenserId, int Segmen
      {
          if (DispenserPreSegmentReady[i] == false)
          {
-             REPORT_MSG(i,"IDS_Valve_Presegment Not Ready");
+             //REPORT_MSG(i,"IDS_Valve_Presegment Not Ready");
              return OK; //not all configured heaters are ready
          }
      }
-     REPORT_MSG(deviceID,"IDS_Valve_PresegmentReady");
+     //REPORT_MSG(deviceID,"IDS_Valve_PresegmentReady");
      PreSegmentReady(Module_IDS,ModuleDone);
   return OK; // all configured heaters are ready
  }
@@ -234,13 +234,22 @@ uint32_t IDSPreSegmentState(void *JobDetails, int SegmentId)
         n_dispensers = JobTicket->segments[SegmentId]->brushstops[JobBrushStopId]->n_dispensers;
         for (Dispenser_i = 0;Dispenser_i < n_dispensers;Dispenser_i++)
         {
+            //prepare the SW structures
+            DispenserId = JobTicket->segments[SegmentId]->brushstops[JobBrushStopId]->dispensers[Dispenser_i]->index;
+            DispenserPreSegmentReady[DispenserId] = false;
+            LOG_ERROR(DispenserId,"Dispenser PreSegment Start");
+        }
+        for (Dispenser_i = 0;Dispenser_i < n_dispensers;Dispenser_i++)
+        {
             DispenserId = JobTicket->segments[SegmentId]->brushstops[JobBrushStopId]->dispensers[Dispenser_i]->index;
             HW_Motor_Id = DispenserIdToMotorId[DispenserId];
             if (MotorsCfg[HW_Motor_Id].hardwaremotortype != DispenserIdToMotorId[DispenserId])//unconfigured dispenser
+            {
+                LOG_ERROR(DispenserId,"Dispenser PreSegment not configured");
                 continue;
-            DispenserPreSegmentReady[DispenserId] = false;
-            LOG_ERROR(DispenserId,"Dispenser PreSegment Start");
-            if (IDS_isDispenserUsedNextSegment(JobDetails,DispenserId,SegmentId) == false)
+            }
+            if (JobTicket->segments[SegmentId]->brushstops[0]->dispensers[Dispenser_i]->nanolitterpersecond==0)
+            //if (IDS_isDispenserUsedNextSegment(JobDetails,Dispenser_i,SegmentId) == false)
             {
                 Control3WayValvesWithCallback ((Valves_t)DispenserId, MidTank_Dispenser, IDS_Valve_PresegmentReady); //direction: MidTank_Dispenser or Dispenser_Mixer
                 LOG_ERROR(DispenserId,"Dispenser Not Used Next Segment");
@@ -258,7 +267,6 @@ uint32_t IDSPreSegmentState(void *JobDetails, int SegmentId)
                 MotorSetMicroStep(HW_Motor_Id,JobTicket->segments[SegmentId]->brushstops[JobBrushStopId]->dispensers[Dispenser_i]->dispenserstepdivision);
             else*/
             //MotorSetMicroStep(HW_Motor_Id,MotorsCfg[HW_Motor_Id].microstep);
-
         }
     }
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
index ce4395e0f..305c0e767 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
@@ -21,23 +21,35 @@
 #include "drivers/FPGA/FPGA_SPI_Comm.h"
 #include "drivers/FPGA/FPGA_GPIO/FPGA_GPIO.h"
 
+#include <driverlib/timer.h>
+#include <inc/hw_ints.h>
+#include <inc/hw_memmap.h>
+
+
 bool Winder_ScrewHoming = false;
-bool Winder_Active = false;
 
 uint32_t Winder_ScrewAtOffsetCallback(uint32_t deviceID, uint32_t BusyFlag);
 uint32_t Winder_PrepareStage2(uint32_t deviceID, uint32_t ReadValue);
 
 bool ScrewCurrentDirection = false;  //holds current screw direction
 bool BusyfirstCall = true;           //Ignores first call after activating the screw - too early
-double ScrewSpeed = 0;               //ScrewSpeed
+double ScrewSpeed = 0;
+double ScrewRunningTime = 0;
+bool ScrewDirection = false;
+bool SCREW_TimerActivated = false;
+//ScrewSpeed
 uint32_t ScrewControlId = 0xFF;
 uint32_t ScrewNumberOfSteps = 0;     //holds the current number of steps for the next screw run - will be used to build the cone
 uint32_t DirectionChangeCounter = 0; //holds the current number of runs of the screw  - will be used to build the cone
 
 InternalWinderConfigStruc InternalWinderCfg = {0};
+void ScrewTimerInterruptInit(void);
+void ScrewsStopControlTimer (void);
+void ScrewsStartControlTimer (void);
 
 uint32_t Winder_Init(void)
 {
+    ScrewTimerInterruptInit();
     return OK;
 }
 
@@ -73,9 +85,9 @@ uint32_t Winder_Prepare(void)
 {
     uint32_t status = 0;
     //JobTicket* JobTicket = JobDetails;
-    Winder_Active = true;
     //float process_speed = JobTicket->processparameters->dyeingspeed;
-    double ScrewSpeed = 1000;//(process_speed*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].pulleyradius);  // we will use pulley radius of the screw for this purpose, as of now
+    double ScrewSpeed = 1500;//(process_speed*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].pulleyradius);  // we will use pulley radius of the screw for this purpose, as of now
+    //MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_SCREW,InternalWinderCfg.segmentoffsetpulses);
     //REPORT_MSG(ScrewSpeed, "Winder_Prepare");
     /*
      * 1. move home to the limit switch (check that the cart is clear from the limit switch, start moving, with acceleration to maximal speed. enable interrupt on the limit switch, upon interrupt stop.
@@ -84,12 +96,12 @@ uint32_t Winder_Prepare(void)
      */
     if (FPGA_Read_limit_Switches(GPI_LS_SCREW_RIGHT)==LIMIT)
     {
-        REPORT_MSG(LIMIT, "Winder_Prepare at limit");
+        //REPORT_MSG(LIMIT, "Winder_Prepare at limit");
         Winder_PrepareStage2(0,0);
     }
     else
     {
-        REPORT_MSG(ScrewSpeed, "Winder_Prepare");
+        //REPORT_MSG(ScrewSpeed, "Winder_Prepare");
         Winder_ScrewHoming = true;
         //REPORT_MSG(MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize, "Winder_Prepare move to limit");
         status = MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_SCREW,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize, ScrewSpeed, GPI_LS_SCREW_RIGHT, Winder_PrepareStage2);
@@ -107,7 +119,7 @@ uint32_t Winder_PrepareStage2(uint32_t deviceID, uint32_t ReadValue)
     uint32_t status=OK;
     uint32_t numOfSteps = InternalWinderCfg.startoffsetpulses*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].microstep;
 
-    MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_SCREW,InternalWinderCfg.segmentoffsetpulses);
+    //MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_SCREW,InternalWinderCfg.segmentoffsetpulses);
     //REPORT_MSG(numOfSteps, "Winder_PrepareStage2");
     REPORT_MSG(MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].maxfrequency, "Winder_PrepareStage2");
 
@@ -119,16 +131,15 @@ uint32_t Winder_PrepareStage2(uint32_t deviceID, uint32_t ReadValue)
 
 uint32_t Winder_ScrewAtOffsetCallback(uint32_t deviceID, uint32_t BusyFlag)
 {
-    SetMotHome(HARDWARE_MOTOR_TYPE__MOTO_SCREW);  //set this point as the spool home
+    //SetMotHome(HARDWARE_MOTOR_TYPE__MOTO_SCREW);  //set this point as the spool home
+    //MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_SCREW,temp_MaxFrequency);
 
     ScrewCurrentDirection = false;
-    BusyfirstCall = true;
     ScrewSpeed = 0;
     ScrewControlId = 0xFF;
     ScrewNumberOfSteps = 0;
     DirectionChangeCounter = 0;
     REPORT_MSG(BusyFlag, "Winder_ScrewAtOffsetCallback");
-
     PrepareReady(Module_Winder, ModuleDone);
     return OK;
 }
@@ -137,15 +148,6 @@ uint32_t Winder_ScrewAtOffsetCallback(uint32_t deviceID, uint32_t BusyFlag)
  * this is the main operational function of the screw - run back and forth until stopped
  */
 /*
-
-bool ScrewCurrentDirection = false;
-bool BusyfirstCall = true;
-double ScrewSpeed = 0;
-uint32_t ScrewControlId = 0xFF;
-uint32_t ScrewNumberOfSteps = 0;
-uint32_t DirectionChangeCounter = 0;
-*/
-/*
     InternalWinderCfg.segmentoffsetpulses = request->segmentoffsetpulses;
     InternalWinderCfg.spoolbackingrate = request->backingrate;
     InternalWinderCfg.startoffsetpulses = request->startoffsetpulses;
@@ -171,80 +173,55 @@ numOfSteps = InternalWinderCfg.startoffsetpulses*MotorsCfg[HARDWARE_MOTOR_TYPE__
 */
 uint32_t CalculateNumberOfSteps (uint32_t Counter, bool direction)
 {
-    uint32_t NumberOfSteps = InternalWinderCfg.segmentoffsetpulses*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].microstep;
+    uint32_t NumberOfSteps = InternalWinderCfg.segmentoffsetpulses;//*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].microstep;
     float screw_horizontal_speed = 0;
     float RotationsPerSecond;
 
-    if (Counter)
-    {
-        if (Counter%InternalWinderCfg.spoolbackingrate == 1)
+        if (Counter%InternalWinderCfg.spoolbackingrate == 0)
         {
             NumberOfSteps -= (Counter/InternalWinderCfg.spoolbackingrate);
-            REPORT_MSG(ScrewNumberOfSteps, "Head Backing");
+            REPORT_MSG(ScrewNumberOfSteps, "Head Backing ScrewNumberOfSteps");
             ScrewNumberOfSteps--;
-            /*
-            screw_horizontal_speed = InternalWinderCfg.segmentoffsetpulses /  InternalWinderCfg.NumberOfRotationPerPassage;
+            screw_horizontal_speed = ScrewNumberOfSteps /  InternalWinderCfg.NumberOfRotationPerPassage;
             // calculation input#2: number of rotations per second - (basically: speed/winder perimeter. later - according to winder actual speed - calculate according to winder position accumulation in the last second.
-            RotationsPerSecond = dyeingspeed / (InternalWinderCfg.diameter * PI);
-            RotationsPerSecond =  CurrentControlledSpeed[WINDER_MOTOR] / MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_WINDER].pulseperround;
+            RotationsPerSecond =  OriginalMotorSpd_2PPS[WINDER_MOTOR] / MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_WINDER].pulseperround;
             // calculation input#3: speed = rotation per second * traverse per rotation = traverse per second. speed set: traverse per second (mm) * pulses per mm.
             ScrewSpeed = screw_horizontal_speed*RotationsPerSecond;
 
-            MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewSpeed);
-            REPORT_MSG(ScrewSpeed, "CalculateNumberOfSteps");
+            //MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewSpeed);
+            //REPORT_MSG(ScrewSpeed, "CalculateNumberOfSteps ScrewSpeed");
+            //REPORT_MSG(CurrentControlledSpeed[WINDER_MOTOR], "CalculateNumberOfSteps Winder speed");
             CurrentControlledSpeed[SCREW_MOTOR] = ScrewSpeed;
-            */
+
         }
         if ((Counter%InternalWinderCfg.SpoolBottomBackingRate == 0)||(Counter%InternalWinderCfg.SpoolBottomBackingRate == 1))
         {
-            if (direction != MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize)
+            if (direction == MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize)//because the direction is already reversed at the interrupt
             {
                 NumberOfSteps += (Counter/InternalWinderCfg.SpoolBottomBackingRate);
                 ScrewNumberOfSteps++;
-                REPORT_MSG(ScrewNumberOfSteps, "Bottom Backing");
+                REPORT_MSG(ScrewNumberOfSteps, "Bottom Backing ScrewNumberOfSteps");
             }
 
         }
-    }
     return NumberOfSteps;
 
 }
-uint32_t wtick=0,prevwtick = 0;
-uint32_t wgap[100];
-uint32_t wgap_counter=0;
 uint32_t ScrewDirectionChange(uint32_t deviceID, uint32_t BusyFlag)
 {
     uint32_t Steps;
+    double temp;
     if (BusyfirstCall)
     {
         BusyfirstCall = false;
+		DirectionChangeCounter++;
         return OK;
     }
-//    if (Winder_Active == false)
-//        return OK;
-    wtick++;
     //ScrewCurrentDirection: false moves out, true moves home
     if (BusyFlag == NOTBUSY)
     {
-        if (wtick-prevwtick>10)
-        {
-            wgap[wgap_counter++] = wtick-prevwtick;
-            prevwtick = wtick;
-            if (wgap_counter >=99)
-                wgap_counter = 0;
-        }
-        else
-            return OK;
         CalculateNumberOfSteps (DirectionChangeCounter++, ScrewCurrentDirection);
         Steps = ScrewNumberOfSteps;
-        if (ScrewCurrentDirection == false)
-        {
-            ScrewCurrentDirection = true;
-        }
-        else
-        {
-            ScrewCurrentDirection = false;
-        }
 /*        if (FPGA_Read_limit_Switches(GPI_LS_SCREW_RIGHT)==LIMIT)
         {
             ScrewCurrentDirection = (1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize);
@@ -257,8 +234,17 @@ uint32_t ScrewDirectionChange(uint32_t deviceID, uint32_t BusyFlag)
             REPORT_MSG(LIMIT, "Winder at left limit");
         }
 */
-        MotorMove (HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewCurrentDirection,Steps); //process: set point 0, set max speed, move to the specified length, return back.
-        REPORT_MSG(ScrewCurrentDirection, "ScrewDirectionChange");
+        temp = SYS_CLK_FREQ;
+        temp *= Steps;
+        temp /= ScrewSpeed;
+        //LOG_ERROR(Steps,"ScrewDirectionChange - ScrewNumberOfSteps");
+
+        /*if (ScrewRunningTime != temp)
+        {
+            REPORT_MSG(temp, "ScrewDirectionChange ScrewRunningTime changed");
+        }*/
+        ScrewRunningTime = temp;//(SYS_CLK_FREQ*Steps)/ScrewSpeed;
+        //MotorMove (HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewCurrentDirection,Steps); //process: set point 0, set max speed, move to the specified length, return back.
     }
     return OK;
 
@@ -275,7 +261,7 @@ uint32_t Winder_Presegment(void *JobDetails, uint32_t SegmentId)
     float screw_horizontal_speed = 0;
     float RotationsPerSecond;
     char Message[80];
-
+	double temp = 0;
     if (dyeingspeed == 0)
     {
         LOG_ERROR (dyeingspeed," job speed zero");
@@ -292,31 +278,36 @@ uint32_t Winder_Presegment(void *JobDetails, uint32_t SegmentId)
         //     * calculation input: traverse length in milimeters/pulses, number of rotations per traverse ==> length of traverse per rotation.
         screw_horizontal_speed = InternalWinderCfg.segmentoffsetpulses /  InternalWinderCfg.NumberOfRotationPerPassage;
         // calculation input#2: number of rotations per second - (basically: speed/winder perimeter. later - according to winder actual speed - calculate according to winder position accumulation in the last second.
-        RotationsPerSecond = dyeingspeed / (InternalWinderCfg.diameter * PI);
-        RotationsPerSecond =  CurrentControlledSpeed[WINDER_MOTOR] / MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_WINDER].pulseperround;
+        //RotationsPerSecond = dyeingspeed / (MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_WINDER].pulleyradius * PI);
+        RotationsPerSecond =  OriginalMotorSpd_2PPS[WINDER_MOTOR] / MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_WINDER].pulseperround;
         // calculation input#3: speed = rotation per second * traverse per rotation = traverse per second. speed set: traverse per second (mm) * pulses per mm.
         ScrewSpeed = screw_horizontal_speed*RotationsPerSecond;
 
-        MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewSpeed);
+        //MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewSpeed);
         usnprintf(Message, 80, "SCREW speed Rot/sec %d horizon %d pulses %d",(int)RotationsPerSecond,(int)screw_horizontal_speed,(int)ScrewSpeed);
         //LOG_ERROR(segmentfirst_speed,Message);
         //Report(Message,__FILE__,__LINE__,Dispenser_i,RpWarning,segmentfirst_speed,0);
         SendJobProgress(0.0,0,false, Message);
 
         CurrentControlledSpeed[SCREW_MOTOR] = ScrewSpeed;
+        OriginalMotorSpd_2PPS[SCREW_MOTOR] = ScrewSpeed;
         //screw_horizontal_speed = InternalWinderCfg.milimetersperrotation
     //     * 2. determine optimal micro-step setting
     //     * 3. calculate cart travel length from winding parameters
     //     * 4. start move of travel length
     //     * 5. register motor nBusy callback. this callback will flip between move(traverse length, hardstop) and goto(0), with handline og the coneshape and adjusting maxspeed
-        ScrewNumberOfSteps = InternalWinderCfg.segmentoffsetpulses*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].microstep;
-        MotorMove (HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewCurrentDirection,ScrewNumberOfSteps); //process: set point 0, set max speed, move to the specified length, return back.
-        wtick = 0;prevwtick = 0;
-        ScrewControlId = AddControlCallback(ScrewDirectionChange, eOneMillisecond,MotorControlGetnBusyState,(IfTypeMotors*0x100+HARDWARE_MOTOR_TYPE__MOTO_SCREW), HARDWARE_MOTOR_TYPE__MOTO_SCREW, 0);
-
-        // MotorSetSpeedWithCallback (HARDWARE_MOTOR_TYPE__MOTO_SCREW, screw_horizontal_speed,WinderPresegmentReady);
-        //in a callback: calculate backing rate for top and bottom, update point 0, update passing length, call the appropriate move to 0 / move;
-
+        ScrewNumberOfSteps = InternalWinderCfg.segmentoffsetpulses;//*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].microstep;
+        temp = SYS_CLK_FREQ;
+        temp *= InternalWinderCfg.segmentoffsetpulses;
+        temp /= ScrewSpeed;
+        ScrewRunningTime = temp;//(SYS_CLK_FREQ*InternalWinderCfg.segmentoffsetpulses)/ScrewSpeed;
+        LOG_ERROR(ScrewNumberOfSteps,"Winder pre segment - ScrewNumberOfSteps");
+        LOG_ERROR(ScrewRunningTime,"Winder pre segment - ScrewRunningTime");
+//        MotorSetDirection (HARDWARE_MOTOR_TYPE__MOTO_SCREW, ScrewCurrentDirection);
+        //ScrewDirection = 1-ScrewDirection;
+        LOG_ERROR(ScrewSpeed,"Winder pre segment - ScrewSpeed");
+        //MotorSetSpeedDirect(HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewSpeed);
+        ScrewsStartControlTimer ();
     }
     PreSegmentReady(Module_Winder,ModuleDone);
 
@@ -327,11 +318,11 @@ uint32_t Winder_End(void)
     int pend;
     //stop screw
     ScrewNumberOfSteps = 0;
-//    Winder_Active = false;
     if (ScrewControlId != 0xFF)
         RemoveControlCallback(ScrewControlId,ScrewDirectionChange);
     CurrentControlledSpeed[SCREW_MOTOR] = 0;
-    pend = MillisecFlushMsgQ(HARDWARE_MOTOR_TYPE__MOTO_SCREW);
+    ScrewsStopControlTimer();
+
     return MotorStop (HARDWARE_MOTOR_TYPE__MOTO_SCREW,Hard_Hiz);
 }
 void Winder_ScrewHomeLimitSwitchInterrupt(void)
@@ -351,3 +342,60 @@ void Winder_ScrewOutLimitSwitchInterrupt(void)
     MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_SCREW,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize);//make sure to move the cart out
 }
 
+uint32_t  Screw_timerBase = TIMER3_BASE;        //Timer handle
+
+bool     Screw_Restart = false;
+void ScrewTimerInterruptInit(void)
+{
+    ROM_TimerConfigure(Screw_timerBase, TIMER_CFG_PERIODIC);   // 32 bits Timer
+    ROM_TimerEnable(Screw_timerBase, TIMER_A);
+    ROM_IntEnable(INT_TIMER3A);
+    ROM_TimerIntEnable(Screw_timerBase, TIMER_TIMA_TIMEOUT);
+    Screw_Restart = false;
+
+}
+void ScrewsStopControlTimer (void)
+{
+    SCREW_TimerActivated = false;
+    ROM_TimerDisable(Screw_timerBase, TIMER_A);
+    //ROM_IntDisable(INT_TIMER3A);
+    //ROM_TimerIntDisable(Screw_timerBase, TIMER_TIMA_TIMEOUT);
+}
+void ScrewsStartControlTimer (void)
+{
+    if (SCREW_TimerActivated == true)
+        return;
+    SCREW_TimerActivated = true;
+    //ROM_TimerConfigure(Screw_timerBase, TIMER_CFG_PERIODIC);   // 32 bits Timer
+    TimerEnable(Screw_timerBase, TIMER_A);
+    ROM_IntEnable(INT_TIMER3A);
+    ROM_TimerIntEnable(Screw_timerBase, TIMER_TIMA_TIMEOUT);
+    ROM_TimerLoadSet(Screw_timerBase, TIMER_A,(int)1200000/*10 millisec*/ );
+    Report("ScrewsStartControlTimer direction,speed ", __FILE__,__LINE__,ScrewCurrentDirection, RpMessage, ScrewSpeed, 0);
+    return;
+}
+void ScrewTimerInterrupt(int ARG0)
+{
+    bool dir = ScrewCurrentDirection;
+    ROM_TimerIntClear(Screw_timerBase, TIMER_TIMA_TIMEOUT);  // Clear the timer interrupt
+    ROM_IntMasterDisable();
+    ScrewDirectionChange(0,NOTBUSY);
+    if (SCREW_TimerActivated == true)
+    {
+        MotorSetDirection (HARDWARE_MOTOR_TYPE__MOTO_SCREW, ScrewCurrentDirection);
+        ScrewCurrentDirection = 1-ScrewCurrentDirection;
+        MotorSetSpeedDirect(HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewSpeed);
+        ROM_TimerLoadSet(Screw_timerBase, TIMER_A,(int)ScrewRunningTime);
+    }
+    else
+    {
+        TimerDisable(Screw_timerBase, TIMER_A);
+    }
+    //Report("ScrewTimerInterrupt duration, speed", __FILE__,__LINE__,ScrewRunningTime, RpMessage, ScrewSpeed, 0);
+    //
+    // Enable all interrupts.
+    //
+    ROM_IntMasterEnable();
+    return ;
+
+}
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
index 3b17cd5dd..e189e2639 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
@@ -42,7 +42,7 @@ uint32_t CurrentControlledSpeed[MAX_THREAD_MOTORS_NUM] = {0};
 
 TimerMotors_t ThreadMotorIdToMotorId[MAX_THREAD_MOTORS_NUM] = {HARDWARE_MOTOR_TYPE__MOTO_RDRIVING,HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING,HARDWARE_MOTOR_TYPE__MOTO_LDRIVING,HARDWARE_MOTOR_TYPE__MOTO_WINDER,HARDWARE_MOTOR_TYPE__MOTO_SCREW};
 HardwareDancerType ThreadMotorIdToDancerId[MAX_THREAD_MOTORS_NUM] = {FEEDER_DANCER,NUM_OF_DANCERS,POOLER_DANCER,WINDER_DANCER,NUM_OF_DANCERS};
-uint32_t    ControlIdtoMotorId [MAX_THREAD_MOTORS_NUM] = {0xFF};
+uint32_t    ControlIdtoMotorId [MAX_THREAD_MOTORS_NUM] = {0xFF,0xFF,0xFF,0xFF,0xFF};
 uint32_t    SpeedControlId=0xFF;
 uint32_t    PoolerSpeedControlId=0xFF;
 
@@ -706,16 +706,20 @@ uint32_t ThreadPreSegmentState(void *JobDetails)
 
     return OK;
 }
+int REPSegmentId = 0;
 void ThreadInterSegmentEnded(void)
 {
+    LOG_ERROR (REPSegmentId,"ThreadInterSegmentEnded");
     PreSegmentReady(Module_Thread,ModuleDone);
 }
 void ThreadSegmentEnded(void)
 {
+    LOG_ERROR (REPSegmentId," ThreadSegmentState");
     SegmentReady(Module_Thread,ModuleDone);
 }
 void ThreadDistanceToSpoolEnded(void)
 {
+    LOG_ERROR (REPSegmentId," ThreadDistanceToSpoolEnded");
     DistanceToSpoolReady(Module_Thread,ModuleDone);
 }
 double seglength = 0.0;
@@ -723,8 +727,10 @@ double seglength = 0.0;
 uint32_t ThreadSegmentState(void *JobDetails, int SegmentId)
 {
     JobTicket* JobTicket = JobDetails;
+    REPSegmentId = SegmentId;
     seglength = JobTicket->segments[SegmentId]->length;
     CurrentSegmentId = SegmentId;
+    LOG_ERROR (seglength," ThreadSegmentState");
     ThreadUpdateProcessLength (seglength,(void *)ThreadSegmentEnded);
     return OK;
 }
@@ -733,6 +739,7 @@ uint32_t ThreadSegmentState(void *JobDetails, int SegmentId)
 uint32_t ThreadDistanceToSpoolState(void )
 {
     seglength = dryerbufferlength;
+    LOG_ERROR (seglength,"ThreadDistanceToSpoolState");
     ThreadUpdateProcessLength (seglength,(void *)ThreadDistanceToSpoolEnded);
     return OK;
 }
@@ -766,7 +773,10 @@ char Endstr[150];
      {
          if (ControlIdtoMotorId[Motor_i] != 0xFF)
          {
-             RemoveControlCallback(ControlIdtoMotorId[Motor_i],ThreadControlCBFunction);
+             if(RemoveControlCallback(ControlIdtoMotorId[Motor_i],ThreadControlCBFunction) == OK)
+                 ControlIdtoMotorId[Motor_i] == 0xFF;
+             else
+                 LOG_ERROR (ControlIdtoMotorId[Motor_i],"Remove Control failed");
          }
         MotorStop(ThreadMotorIdToMotorId[Motor_i],Hard_Hiz);
      }