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

14 files changed, 190 insertions, 81 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.c b/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.c
index 931b6674f..73532a7d2 100644
--- a/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.c
+++ b/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.c
@@ -669,6 +669,8 @@ uint32_t AlarmHandling_ControlTrigger(uint32_t IfIndex, uint32_t ReadValue)
 
     return OK;
 }
+void AlarmHandlingInternalSetAlarm(uint32_t AlarmId, bool value);
+
 void AlarmHandlingSetAlarm(uint32_t AlarmId, bool value)
 {
     AlarmHandlingInternalSetAlarm( AlarmId, value);
@@ -1024,12 +1026,12 @@ void AlarmHandlingTask(UArg arg0, UArg arg1)
     }
 }
 //StartEventsNotificationResponse StartEventsNotification = START_EVENTS_NOTIFICATION_RESPONSE__INIT;
-StartEventsNotificationResponse response = START_EVENTS_NOTIFICATION_RESPONSE__INIT;
+StartEventsNotificationResponse EventsResponse = START_EVENTS_NOTIFICATION_RESPONSE__INIT;
 int LargeMessagesAH = 0;
 void SendEventNotifications(void)
 {
     MessageContainer responseContainer;
-    //StartEventsNotificationResponse response = START_EVENTS_NOTIFICATION_RESPONSE__INIT;
+    //StartEventsNotificationResponse EventsResponse = START_EVENTS_NOTIFICATION_RESPONSE__INIT;
     int i,e=0;
 
     if (AlarmHandlingActive == false)
@@ -1042,20 +1044,20 @@ void SendEventNotifications(void)
         return;
     }
 
-    response.n_events = 0;
+    EventsResponse.n_events = 0;
     UInt Key = Task_disable();
     for (i = 0;i<MAX_SYSTEM_ALARMS;i++)
     {
         if (AlarmState[i].Status == true)
         {
-            response.n_events++;
+            EventsResponse.n_events++;
         }
     }
 
-    if (response.n_events)
+    if (EventsResponse.n_events)
     {
-        response.events = (Event **)my_malloc(sizeof(Event*)*response.n_events);
-        if(response.events)
+        EventsResponse.events = (Event **)my_malloc(sizeof(Event*)*EventsResponse.n_events);
+        if(EventsResponse.events)
         {
             for (i = 0;i<MAX_SYSTEM_ALARMS;i++)
             {
@@ -1063,7 +1065,7 @@ void SendEventNotifications(void)
                 {
                     if (AlarmState[i].EventPtr)
                     {
-                        response.events[e]=AlarmState[i].EventPtr;
+                        EventsResponse.events[e]=AlarmState[i].EventPtr;
                         e++;
                     }
                 }
@@ -1071,17 +1073,17 @@ void SendEventNotifications(void)
         }
         else
         {
-            LOG_ERROR(response.n_events,"events malloc error");
+            LOG_ERROR(EventsResponse.n_events,"events malloc error");
             Task_restore(Key);
             return;
         }
     }
     else
     {
-        response.events = NULL;
+        EventsResponse.events = NULL;
     }
     Task_restore(Key);
-    responseContainer = createAllocatedContainer(MESSAGE_TYPE__StartEventsNotificationResponse,  AlarmHandlingToken, false, &response, &start_events_notification_response__pack, &start_events_notification_response__get_packed_size,&alarm_response_buffer);
+    responseContainer = createAllocatedContainer(MESSAGE_TYPE__StartEventsNotificationResponse,  AlarmHandlingToken, false, &EventsResponse, &start_events_notification_response__pack, &start_events_notification_response__get_packed_size,&alarm_response_buffer);
     responseContainer.continuous = true;
 
     uint8_t* container_buffer = my_malloc(message_container__get_packed_size(&responseContainer));
@@ -1096,8 +1098,8 @@ void SendEventNotifications(void)
             my_free(container_buffer);
         }
     }
-    if (response.events)
-        my_free(response.events);
+    if (EventsResponse.events)
+        my_free(EventsResponse.events);
     if (responseContainer.data.data)
         my_free(responseContainer.data.data);
 }
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.c b/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.c
index dd9841777..6c1b647be 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.c
@@ -11,7 +11,7 @@ float PIDAlgorithmCalculation(float _setPoint,float _mesuredParam , PID_Config_P
 	error = _setPoint - _mesuredParam;
 
 	//In case of error too small then stop integration
-	if(abs(error) > params->epsilon)
+	if(fabs(error) > params->epsilon)
 	{
 		*_integral = *_integral + error*params->dt;
 	}
diff --git a/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.c b/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.c
index 184aa52be..b89fa43ed 100644
--- a/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.c
+++ b/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.c
@@ -22,6 +22,7 @@
 #include "drivers/FPGA/FPGA.h"
 #include "drivers/I2C_Communication/ADC_MUX/ADC_MUX.h"
 #include "drivers/I2C_Communication/DAC/Blower.h"
+#include "drivers/valves/valve.h"
 
 #include "modules/ids/ids_ex.h"
 #include "Modules/Control/MillisecTask.h"
diff --git a/Software/Embedded_SW/Embedded/Modules/General/GeneralHardware.c b/Software/Embedded_SW/Embedded/Modules/General/GeneralHardware.c
index 6fc728934..9c7e84bc2 100644
--- a/Software/Embedded_SW/Embedded/Modules/General/GeneralHardware.c
+++ b/Software/Embedded_SW/Embedded/Modules/General/GeneralHardware.c
@@ -416,6 +416,8 @@ uint32_t HWConfigurationFunc(MessageContainer* requestContainer)
 
     status = HWConfiguration(UploadRequest);
 
+    GeneralHwReady = true;
+
     responseContainer = createContainer(MESSAGE_TYPE__UploadHardwareConfigurationResponse, requestContainer->token, true, &response, &upload_hardware_configuration_response__pack, &upload_hardware_configuration_response__get_packed_size);
     if (status!= OK)
     {
diff --git a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_ex.h b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_ex.h
index a5165e7f2..4dbd2e89e 100644
--- a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_ex.h
+++ b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_ex.h
@@ -44,6 +44,7 @@ uint32_t IDSPrepareState(void *JobDetails);
 void IDSPrepareStart(void);
 
 uint32_t IDSPreSegmentState(void *SegmentDetails, int SegmentId);
+uint32_t IDSCheckSegmentData(void *SegmentDetails, int SegmentId);
 uint32_t IDSSegmentState(void *SegmentDetails, int SegmentId);
 uint32_t IDSDistanceToSpoolState(void);
 uint32_t IDSEndState(void);
diff --git a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_maint.c b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_maint.c
index d312dcb78..0907c4a67 100644
--- a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_maint.c
+++ b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_maint.c
@@ -205,6 +205,10 @@ uint32_t IDS_StopHomeDispenser (uint32_t DispenserId)
     Disable_MidTank_Pressure_Reading(DispenserId);
     Valve_Set(IDS_Id_to_AirValve[DispenserId], Atm_MidTank_OFF);
     Control3WayValvesWithCallback ((Valves_t)DispenserId, MidTank_Dispenser, NULL); //direction: MidTank_Dispenser or Dispenser_Mixer
+    if (DispenserId == LUBRICANT_DISPENSER)
+    {
+        Lubricant_2Way_Valve (START);
+    }
 
     MotorSetMicroStep(MotorId, MotorsCfg[MotorId].microstep);
     CurrentDispenserSpeed[DispenserId] = 0;
@@ -227,6 +231,36 @@ uint32_t IDS_CheckDispenserLimitSwitch (LimitSwitchAlarms LS_Id)
     else
         return FPGA_Read_limit_Switches(Dispenser_Id_to_Alarm_LS_Id[LS_Id]);
 }
+uint32_t IDS_EmptyDispenserCallback(uint32_t motorId, uint32_t ReadValue)
+{
+    uint8_t DispenserId = motorId-HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_1;
+
+    Read_MidTank_Pressure_Sensor(DispenserId);
+    //close dry air valve in the dispenser
+    Valve_Set(IDS_Id_to_AirValve[DispenserId], Atm_MidTank_OFF);
+    Disable_MidTank_Pressure_Reading(DispenserId);
+
+    MotorStop(motorId,Hard_Hiz);
+    CurrentDispenserSpeed[DispenserId] = 0;
+    MotorSetMicroStep(motorId, MotorsCfg[motorId].microstep);
+    HomingActive[DispenserId]= false;
+    if (DispenserId == LUBRICANT_DISPENSER)
+    {
+        Lubricant_2Way_Valve (STOP);
+    }
+
+    Report("End Priming",__FILE__,millisecondCounter,(int)DispenserId,RpWarning,(int)DispenserHomingTime[DispenserId],0);
+    //MotorSetMicroStep(motorId, MotorsCfg[motorId].microstep);
+    if (HomingRequestCallback[DispenserId])
+    {
+        HomingRequestCallback[DispenserId](DispenserId,0);
+        HomingRequestCallback[DispenserId] = NULL;
+    }
+
+
+return OK;
+
+}
 
 uint32_t IDS_EmptyDispenser (uint32_t DispenserId, uint32_t speed , callback_fptr callback)
 {
@@ -254,8 +288,13 @@ uint32_t IDS_EmptyDispenser (uint32_t DispenserId, uint32_t speed , callback_fpt
         IDS_Dispenser_RefillStarted(DispenserId);
         IDS_Dispenser_MovingDirection(DispenserId,UP);
 
+        if (DispenserId == LUBRICANT_DISPENSER)
+        {
+            Lubricant_2Way_Valve (START);
+        }
+
 
-        MotorMovetoLimitSwitch (MotorId,MotorsCfg[MotorId].directionthreadwize, speed,  Dispenser_Id_to_LS_Empty_Id[DispenserId], IDS_HomeDispenserCallback,0);
+        MotorMovetoLimitSwitch (MotorId,MotorsCfg[MotorId].directionthreadwize, speed,  Dispenser_Id_to_LS_Empty_Id[DispenserId], IDS_EmptyDispenserCallback,0);
         CurrentDispenserSpeed[DispenserId] = speed;
         return OK;
     }
diff --git a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c
index efedfe545..aea16bb60 100644
--- a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c
@@ -7,6 +7,7 @@
 #include "ids.h"
 #include "ids_ex.h"
 #include "../control/control.h"
+#include "../control/MillisecTask.h"
 #include "../general/process.h"
 #include "../control/pidalgo.h"
 #include "../thread/thread.h"
@@ -850,6 +851,40 @@ uint32_t IDS_Cleaning_Stop_Cleaning_Solution (callback_fptr callback);
      }
      return OK;
  }
+uint32_t IDSCheckSegmentData(void *SegmentDetails, int SegmentId)
+{
+    JobDescriptionFileSegment* PrevSegment = SegmentDetails;
+    uint32_t status = OK;
+    JobDescriptionFileBrushStop * tFileBrushStop;
+    int Brush_i;
+
+//check and close previous segment
+    if (PrevSegment)
+    {
+        if (JobBrushStopId<PrevSegment->brushstopscount) //we did not finish reading the brushstops of the previous segment
+        {
+            Report("Unhandled brushstops remained",__FILE__,__LINE__,(int)JobBrushStopId,RpWarning,(int)PrevSegment->brushstopscount,0);
+            //REPORT_MSG (Segment->brushstopscount, "Segment->brushstopscount");
+            for (Brush_i=JobBrushStopId;Brush_i<PrevSegment->brushstopscount;Brush_i++)
+            {
+                if (status == ERROR)
+                    break;
+                tFileBrushStop = GetNextBrushStopFromJobFile();
+                if (tFileBrushStop)
+                {
+                    FreeBrushStopFileData(tFileBrushStop);
+                    Report("Unhandled brushstops handled",__FILE__,__LINE__,(int)Brush_i,RpWarning,(int)PrevSegment->brushstopscount,0);
+                }
+                else
+                {
+                    status = ERROR;
+                    break;
+                }
+            }//for brushstops
+        }
+    }
+    return status;
+}
 uint32_t IDSPreSegmentState(void *SegmentDetails, int SegmentId)
 {
     JobSegment* Segment = SegmentDetails;
@@ -864,7 +899,7 @@ uint32_t IDSPreSegmentState(void *SegmentDetails, int SegmentId)
     /* wait for all dispensers  to get to the required pressure
      * move the presegment ready when all dispensers are ready.
      */
-    REPORT_MSG(SegmentId,"IDSPreSegmentState");
+    REPORT_MSG(Segment->n_brushstops,"IDSPreSegmentState");
     if (JobBrushStopId>=Segment->n_brushstops)
     {
         LOG_ERROR(Segment->n_brushstops,"Error JobBrushStopId");
@@ -873,7 +908,6 @@ uint32_t IDSPreSegmentState(void *SegmentDetails, int SegmentId)
         return ERROR;
     }
 
-
     if ((EnableIntersegment == true)&&(IntersegmentLength>0))
     {
         Valve_Set(VALVE_MIXCHIP_WASTECH, Mixer_Waste); //if intersegment is defined throw the ink away
@@ -1026,7 +1060,7 @@ void IDS_StartBrushStop(int n_dispensers,  JobDispenser** Dispensers)
                 /*IDS_Dispenser_Start_Motor_and_Open_Valve(DispenserId,
                                                          segmentfirst_speed,
                                                          NULL);*/
-                Control3WayValvesWithCallback (DispenserId, Dispenser_Mixer, NULL); //direction: MidTank_Dispenser or Dispenser_Mixer
+                //Control3WayValvesWithCallback (DispenserId, Dispenser_Mixer, NULL); //direction: MidTank_Dispenser or Dispenser_Mixer
                 MotorSetSpeed(HW_Motor_Id, segmentfirst_speed);
                 CurrentDispenserSpeed[DispenserId] = segmentfirst_speed;
                 usnprintf(IdsMessage, 80,
@@ -1034,7 +1068,7 @@ void IDS_StartBrushStop(int n_dispensers,  JobDispenser** Dispensers)
                           DispenserId,
                           (int) Dispensers[Dispenser_i]->nanolitterpersecond,
                           (int) Dispensers[Dispenser_i]->nanoliterperpulse,
-                          (int) segmentfirst_speed,Dispensers[Dispenser_i]->dispenserstepdivision,MotorsCfg[HW_Motor_Id].microstep);
+                          (int) (segmentfirst_speed*1000),Dispensers[Dispenser_i]->dispenserstepdivision,MotorsCfg[HW_Motor_Id].microstep);
                 //REPORT_MSG(segmentfirst_speed,IdsMessage);
                 Report(IdsMessage, __FILE__, __LINE__, Dispenser_i, RpWarning,   segmentfirst_speed, 0);
                 //SendJobProgress(0.0, 0, false, IdsMessage);
diff --git a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_Motor.c b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_Motor.c
index 0854aba40..53a860970 100644
--- a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_Motor.c
+++ b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_Motor.c
@@ -449,7 +449,7 @@ void Stub_MotorSpeedRequest(MessageContainer* requestContainer)
         //deley TODO
         SysCtlDelay(10000);
         //response.speed = (double)(CurrentSpdCalc(MotorGetSpeedFromFPGA_Res(Motor_Id)));
-        response.speed = MotorGetSpeedFromFPGA_Res(Motor_Id);
+        response.speed = (double)(MotorGetSpeedFromFPGA_Res(Motor_Id));
 
     #endif
     //////////////////////////////////////////////////////////////////
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
index 19201c708..0093a5a2a 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread.h
@@ -56,8 +56,8 @@ extern int32_t BreakSensordebouncetimemilli;
 extern HardwarePidControl           MotorsControl[MAX_THREAD_MOTORS_NUM];
 //extern InternalWinderConfigStruc    InternalWinderCfg;
 extern HardwareDancer               DancersCfg[MAX_SYSTEM_DANCERS];
-extern uint32_t CurrentControlledSpeed[MAX_THREAD_MOTORS_NUM];
-extern int OriginalMotorSpd_2PPS[MAX_THREAD_MOTORS_NUM];
+extern double CurrentControlledSpeed[MAX_THREAD_MOTORS_NUM];
+extern double OriginalMotorSpd_2PPS[MAX_THREAD_MOTORS_NUM];
 
 #define MAX_CONTROL_SAMPLES 10
 extern int32_t MotorSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES];
@@ -65,7 +65,7 @@ extern int MotorSamplePointer[MAX_THREAD_MOTORS_NUM];
 extern double NormalizedErrorCoEfficient[MAX_THREAD_MOTORS_NUM];
 extern int DancerStopActivityLimit[MAX_THREAD_MOTORS_NUM];
 extern    MotorControlConfig_t MotorControlConfig[MAX_THREAD_MOTORS_NUM];
-extern int32_t MotorSpeedSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES];
+extern double MotorSpeedSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES];
 extern int MotorSpeedSamplePointer[MAX_THREAD_MOTORS_NUM];
 
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
index ab5075ab9..66d6baed5 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
@@ -594,7 +594,7 @@ uint32_t ThreadLoadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
         MotorControlConfig[index].m_calculatedError = (-1*MotorControlConfig[index].m_calculatedError);
     }*/
     calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*OriginalMotorSpd_2PPS[index];
-    if (abs(calculated_speed-CurrentControlledSpeed[index])> MotorControlConfig[index].m_ingnoreValue)
+    if (fabs(calculated_speed-CurrentControlledSpeed[index])> MotorControlConfig[index].m_ingnoreValue)
     {
         CurrentControlledSpeed[index] = calculated_speed;
         MotorSetSpeed(ThreadMotorIdToMotorId[index], calculated_speed);
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
index f1c8956ff..a68995168 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
@@ -43,10 +43,11 @@ uint32_t ScrewDirectionChangeCounter = 1; //holds the current number of runs of
 //uint32_t ScrewChangeCounter = 0;
 //uint32_t ScrewChangeLimit = 0;
 uint32_t CalculationDirectionChangeCounter = 1; //holds the current number of runs of the screw  - will be used to build the cone
-uint16_t WinderMotorSpeed[MAX_WINDER_SPEED_CALCULATION];
+double WinderMotorSpeed[MAX_WINDER_SPEED_CALCULATION];
 uint16_t WinderMotorSpeedCounter = 0;
 bool     WinderMotorSpeedRollOver = false;
-double   ScrewSpeed = 0;
+#define DEFAULT_SCREW_SPEED 1400
+double   ScrewSpeed = DEFAULT_SCREW_SPEED;
 double   ScrewRunningTime = 0;
 uint32_t ScrewNumberOfSteps = 0;     //holds the current number of steps for the next screw run - will be used to build the cone
 bool     SCREW_TimerActivated = false;
@@ -96,7 +97,7 @@ uint32_t Winder_Prepare(void)
     uint32_t status = 0;
     //JobTicket* JobTicket = JobDetails;
     //float process_speed = JobTicket->processparameters->dyeingspeed;
-    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
+    double ScrewSpeed = DEFAULT_SCREW_SPEED;//(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");
     /*
@@ -143,9 +144,9 @@ uint32_t Winder_PrepareStage2(uint32_t deviceID, uint32_t ReadValue)
 
     //MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_SCREW,InternalWinderCfg.segmentoffsetpulses);
     //REPORT_MSG(numOfSteps, "Winder_PrepareStage2");
-    //Read_Screw_Encoder();
-    //ScrewLocationLimitSwitch = Screw_RotEnc.Position;
-    //REPORT_MSG(ScrewLocationLimitSwitch, "Winder_PrepareStage2 Encoder Location");
+    Read_Screw_Encoder();
+    ScrewLocationLimitSwitch = Screw_RotEnc.Position;
+    REPORT_MSG(ScrewLocationLimitSwitch, "Winder_PrepareStage2 Encoder Location");
 
 
     REPORT_MSG(millisecondCounter/*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].maxfrequency*/, "Winder_PrepareStage2");
@@ -169,8 +170,13 @@ uint32_t Winder_ScrewAtOffsetCallback(uint32_t deviceID, uint32_t BusyFlag)
     //SetMotHome(HARDWARE_MOTOR_TYPE__MOTO_SCREW);  //set this point as the spool home
     //MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_SCREW,temp_MaxFrequency);
 
-    //Read_Screw_Encoder();
-    //ScrewLocationStart = Screw_RotEnc.Position;
+    MotorStop (HARDWARE_MOTOR_TYPE__MOTO_SCREW,Soft_Hiz); //per L6470 errata between mov and run commands
+    Task_sleep(5);
+    Reset_Screw_Encoder();
+    Task_sleep(5);
+    Read_Screw_Encoder();
+    Task_sleep(5);
+    ScrewLocationStart = Screw_RotEnc.Position;
 
     REPORT_MSG(ScrewLocationStart, "Winder_ScrewAtOffsetCallback Encoder Location");
 
@@ -180,7 +186,6 @@ uint32_t Winder_ScrewAtOffsetCallback(uint32_t deviceID, uint32_t BusyFlag)
     ScrewControlId = 0xFF;
     ScrewNumberOfSteps = 0;
     REPORT_MSG(millisecondCounter, "Winder_ScrewAtOffsetCallback");
-    MotorStop (HARDWARE_MOTOR_TYPE__MOTO_SCREW,Soft_Hiz); //per L6470 errata between mov and run commands
     PrepareReady(Module_Winder, ModuleDone);
     return OK;
 }
@@ -212,26 +217,28 @@ InternalWinderCfg.segmentoffsetpulses
   
 numOfSteps = InternalWinderCfg.startoffsetpulses*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].microstep;
 */
-char ScrewStr[100];
+char ScrewStr[150];
 //char TempScrewStr[100];
 double WinderReferenceSpeed=0;
-int32_t  TotalWinderSpeed=0;
+double  TotalWinderSpeed=0;
 bool Add100 = false;
 double Rotations = 6.0;
+int flipflop = 0;
+uint32_t motspeed;
+float speedf;
+int WinderCalculation = 0;
 uint32_t Screw100msecDirectionChange(uint32_t deviceID, uint32_t BusyFlag)
 {
     //uint32_t Steps;
     double temp,tempScrewSpeed;
     double screw_horizontal_speed = 0;
     double RotationsPerSecond;
-    int32_t Averagewinderspeed = 0;
+    double Averagewinderspeed = 0;
 
-    //ScrewChangeCounter++;
-    //if ((ScrewChangeCounter>3)&&(ScrewChangeCounter<(ScrewChangeLimit-2))) //do not take the winder speed near the limits
-    {
-        TotalWinderSpeed-=WinderMotorSpeed[WinderMotorSpeedCounter];
-        WinderMotorSpeed[WinderMotorSpeedCounter] = CurrentControlledSpeed[WINDER_MOTOR];
-        TotalWinderSpeed+=WinderMotorSpeed[WinderMotorSpeedCounter];
+//    {
+//        TotalWinderSpeed-=WinderMotorSpeed[WinderMotorSpeedCounter];
+//        WinderMotorSpeed[WinderMotorSpeedCounter] = CurrentControlledSpeed[WINDER_MOTOR];
+//        TotalWinderSpeed+=WinderMotorSpeed[WinderMotorSpeedCounter];
         if (WinderMotorSpeedCounter++>=MAX_WINDER_SPEED_CALCULATION)
         {
             if (WinderMotorSpeedRollOver == false)
@@ -239,20 +246,34 @@ uint32_t Screw100msecDirectionChange(uint32_t deviceID, uint32_t BusyFlag)
                 Add100 = true;
                 LOG_ERROR(Add100, "Add100 = true");
             }
-            WinderMotorSpeedCounter=0;
+//            WinderMotorSpeedCounter=0;
             WinderMotorSpeedRollOver=true;
         }
+//    }
+    if (flipflop == 0)
+    {
+        MotorGetSpeedFromFPGA1(HARDWARE_MOTOR_TYPE__MOTO_SCREW);
     }
+    else if (flipflop == 1)
+    {
+        speedf = MotorGetSpeedFromFPGA_Res(HARDWARE_MOTOR_TYPE__MOTO_SCREW);
+    }
+    flipflop ++;
 
     if (ScrewDirectionChangeCounter == CalculationDirectionChangeCounter)
         return OK;
+    //deley TODO
+    flipflop = 0;
 
     ScrewCurrentDirection = 1-ScrewCurrentDirection;
     CalculationDirectionChangeCounter++;
-    //REPORT_MSG(ScrewLocationRun[1] - ScrewLocationRun[0], "Screw Run NumberOfSteps");
-    //usnprintf(ScrewStr, 100, "Winder Encoder: 0 0x%x 1 0x%x diff %d ",ScrewLocationRun[0],ScrewLocationRun[1],abs(ScrewLocationRun[1] - ScrewLocationRun[0]));
+    //double calcsteps = (ScrewRunningTime/SYS_CLK_FREQ)*ScrewSpeed;
+    //REPORT_MSG((abs(ScrewLocationRun[1] - ScrewLocationRun[0]), "Screw Run NumberOfSteps");
+//    usnprintf(ScrewStr, 100, "Winder Encoder: 0 0x%x 1 0x%x diff %d intent %d rot %d",ScrewLocationRun[0],ScrewLocationRun[1],abs(ScrewLocationRun[1] - ScrewLocationRun[0]),ScrewNumberOfSteps,Rotations*10);
+    usnprintf(ScrewStr, 150, "Winder Encoder:id, diff, intended,  winderspeed,  rotation, speed, time, mot speed {, %d, %d, %d, %d, %d, %d, %d, %d, }",CalculationDirectionChangeCounter,
+              abs(ScrewLocationRun[1] - ScrewLocationRun[0]),ScrewNumberOfSteps,(int)(WinderReferenceSpeed),(int)(Rotations*10),(int)ScrewSpeed,(int)ScrewRunningTime,(int)speedf);
     //usnprintf(ScrewStr, 100, "Winder Encoder: 0 %d 1 %d diff %d ",ScrewLocationRun[0],ScrewLocationRun[1],ScrewLocationRun[1] - ScrewLocationRun[0]);
-    //Report(ScrewStr,__FILE__,__LINE__,ScrewLocationLimitSwitch,RpWarning,ScrewLocationStart, 0);
+    Report(ScrewStr,__FILE__,__LINE__,CalculationDirectionChangeCounter,RpWarning,ScrewLocationStart, 0);
 
     if (ScrewCurrentDirection == 1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize) //next time going out
     {
@@ -265,8 +286,7 @@ uint32_t Screw100msecDirectionChange(uint32_t deviceID, uint32_t BusyFlag)
         if ((CalculationDirectionChangeCounter/2)%InternalWinderCfg.spoolbackingrate == 0)
         {
             ScrewNumberOfSteps--;
-            REPORT_MSG(ScrewNumberOfSteps, "Head Backing ScrewNumberOfSteps");
-
+            Report("Head Backing",__FILE__,__LINE__,CalculationDirectionChangeCounter,RpWarning,ScrewNumberOfSteps, 0);
         }
     }
     else //next time going back
@@ -274,42 +294,46 @@ uint32_t Screw100msecDirectionChange(uint32_t deviceID, uint32_t BusyFlag)
         if ((CalculationDirectionChangeCounter/2)%InternalWinderCfg.SpoolBottomBackingRate == 0)
         {
             ScrewNumberOfSteps++;
-            REPORT_MSG(ScrewNumberOfSteps, "Bottom Backing ScrewNumberOfSteps");
+            Report("Bottom Backing ",__FILE__,__LINE__,CalculationDirectionChangeCounter,RpWarning,ScrewNumberOfSteps, 0);
         }
     }
-    if (WinderMotorSpeedRollOver)
+   /* if (WinderMotorSpeedRollOver)
     {
-        /*for (i=0;i<MAX_WINDER_SPEED_CALCULATION;i++)
+        if (WinderCalculation%60000 == 0)//100 minutes
         {
-            TotalWinderSpeed+=WinderMotorSpeed[i];
-        }*/
-        Averagewinderspeed = TotalWinderSpeed/MAX_WINDER_SPEED_CALCULATION;
-        //REPORT_MSG(winderspeed, "WinderSpeedUpdated");
-        //Report("WinderSpeedUpdated",__FILE__,__LINE__,TotalWinderSpeed,RpWarning,Averagewinderspeed,0);
-
-        WinderReferenceSpeed = Averagewinderspeed;
-    }
+            Averagewinderspeed = TotalWinderSpeed/MAX_WINDER_SPEED_CALCULATION;
+            //Report("WinderSpeedUpdated",__FILE__,__LINE__,(int)TotalWinderSpeed,RpWarning,(int)Averagewinderspeed,0);
+            WinderReferenceSpeed = Averagewinderspeed;
+        }
+        WinderCalculation++;
+    }*/
+    //WinderReferenceSpeed = 1000;
+    //ScrewNumberOfSteps = 1000;
     screw_horizontal_speed = ScrewNumberOfSteps /  Rotations;//InternalWinderCfg.NumberOfRotationPerPassage;
-    if (Rotations > 6.6)//7.0)
-        Rotations = 6.0;
+  //  if (Rotations > 6.6)//7.0)
+  //      Rotations = 6.0;
     RotationsPerSecond =  WinderReferenceSpeed / (double)MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_WINDER].pulseperround;
     tempScrewSpeed = screw_horizontal_speed*RotationsPerSecond;
     //ROM_IntMasterDisable();
+    tempScrewSpeed = DEFAULT_SCREW_SPEED;
+
     CurrentControlledSpeed[SCREW_MOTOR] = ScrewSpeed;
+
     temp = SYS_CLK_FREQ;
     temp *= ScrewNumberOfSteps;
     temp /= tempScrewSpeed;
     if ((ScrewRunningTime != temp)||(ScrewSpeed != tempScrewSpeed))
     {
         ScrewSpeed = tempScrewSpeed;
+        ScrewSpeed = DEFAULT_SCREW_SPEED;
         ScrewRunningTime = temp;//(SYS_CLK_FREQ*Steps)/ScrewSpeed;
         //ROM_IntMasterEnable();
         //usnprintf(TempScrewStr, 100, "Winder: Horizon,Rotation, PPR, RPP{ %d, %d ,%d, %d} ",(int)screw_horizontal_speed,(int)RotationsPerSecond,(int)InternalWinderCfg.NumberOfRotationPerPassage,(int)MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_WINDER].pulseperround);
-        //usnprintf(ScrewStr, 100, "Winder: Steps,Speed, Time, WinderSpeed{ %d, %d ,%d, %d} ",(int)ScrewNumberOfSteps,(int)ScrewSpeed,(int)temp,(int)WinderReferenceSpeed);
+        usnprintf(ScrewStr, 100, "Winder: Steps,Speed, Time, WinderSpeed{ %d, %d ,%d, %d} ",(int)ScrewNumberOfSteps,(int)ScrewSpeed,(int)temp,(int)WinderReferenceSpeed);
         //        Report(logmsg[index],__FILE__,__LINE__,index,RpWarning,index,  Counter[index]);
         //        #warning PID is now only proportional (above)
         //Report(TempScrewStr,__FILE__,__LINE__,0,RpWarning,0, 0);
-        //Report(ScrewStr,__FILE__,__LINE__,ScrewCurrentDirection,RpWarning,CalculationDirectionChangeCounter, 0);
+        Report(ScrewStr,__FILE__,__LINE__,ScrewCurrentDirection,RpWarning,CalculationDirectionChangeCounter, 0);
         //REPORT_MSG(temp , "new winder speed");
         //Report("new winder speed",__FILE__,ScrewNumberOfSteps,temp,RpWarning,ScrewSpeed,0);
     }
@@ -353,6 +377,7 @@ uint32_t Winder_Presegment(void *SegmentDetails, uint32_t SegmentId)
         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;
+        ScrewSpeed = DEFAULT_SCREW_SPEED;
 
         //MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewSpeed);
         usnprintf(ScrewStr, 100, "SCREW speed Rot/sec %d horizon %d pulses %d",(int)RotationsPerSecond,(int)screw_horizontal_speed,(int)ScrewSpeed);
@@ -458,20 +483,20 @@ void ScrewsStartControlTimer (void)
     return;
 }
 
-int random = 0;
+
 void ScrewTimerInterrupt(int ARG0)
 {
     ROM_TimerIntClear(Screw_timerBase, TIMER_TIMA_TIMEOUT);  // Clear the timer interrupt
     ROM_IntMasterDisable();
 
-    //Read_Screw_Encoder();
-    //ScrewLocationRun[ScrewCurrentDirection] = Screw_RotEnc.Position;
 
     if (SCREW_TimerActivated == true)
     {
+        Read_Screw_Encoder();
         ROM_TimerLoadSet(Screw_timerBase, TIMER_A,(int)ScrewRunningTime);
         MotorSetDirection (HARDWARE_MOTOR_TYPE__MOTO_SCREW, ScrewCurrentDirection);
-        MotorSetSpeedDirect(HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewSpeed+random);
+        MotorSetSpeedDirect(HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewSpeed);
+        ScrewLocationRun[ScrewCurrentDirection] = Screw_RotEnc.Position;
       //  ScrewChangeCounter = 0;
       //  ScrewChangeLimit = ScrewRunningTime/12000000;
         ScrewDirectionChangeCounter++;
@@ -481,7 +506,8 @@ void ScrewTimerInterrupt(int ARG0)
         TimerDisable(Screw_timerBase, TIMER_A);
     }
     ROM_IntMasterEnable();
-    Rotations+=0.03;
+    //MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewSpeed);
+    //Rotations+=0.03;
 
     return ;
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h
index 91eedeb6b..ead318758 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_ex.h
@@ -28,7 +28,7 @@ void ThreadSetBreakSensorLimit(int limit);
 uint32_t InternalWindingConfigMessage(JobSpool* request);
 uint32_t ThreadConfigBreakSensor(void *request);
 
-uint32_t ThreadGetMotorSpeed(threadMotorsEnum MotorId);
+double ThreadGetMotorSpeed(threadMotorsEnum MotorId);
 double ThreadGetMotorCalculatedError(int DancerId);
 uint32_t ThreadPrepareState(void *JobDetails);
 uint32_t ThreadPreSegmentState(void *SegmentDetails, uint32_t SegmentId);
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
index 3fb6f3af2..b741f33f3 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_init.c
@@ -22,7 +22,7 @@
 MotorDriverConfigStruc  MotorsCfg[NUM_OF_MOTORS]={0};
 HardwarePidControl MotorsControl[MAX_THREAD_MOTORS_NUM] = {0};
 
-int32_t MotorSpeedSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES] = {0};
+double MotorSpeedSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES] = {0};
 int MotorSpeedSamplePointer[MAX_THREAD_MOTORS_NUM] = {0};
 
 int32_t MotorSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES] = {0};
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
index 25485290b..a285d4194 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
@@ -39,7 +39,7 @@
 //by recieved esign flow of the user from the UI
 ///////////////////////////////////////////////////////////////////////////////////////////
 
-uint32_t CurrentControlledSpeed[MAX_THREAD_MOTORS_NUM] = {0};
+double 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};
@@ -48,7 +48,7 @@ uint32_t    SpeedControlId=0xFF;
 uint32_t    PoolerSpeedControlId=0xFF;
 
 double DancerError[NUM_OF_DANCERS] = {0.0};
-int OriginalMotorSpd_2PPS[MAX_THREAD_MOTORS_NUM] = {0};
+double OriginalMotorSpd_2PPS[MAX_THREAD_MOTORS_NUM] = {0};
 uint32_t JobCounter = 0;
 
 MotorControlConfig_t MotorControlConfig[MAX_THREAD_MOTORS_NUM];
@@ -186,6 +186,7 @@ uint32_t ThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue)
             usnprintf(Lenstr, 100, "length huge: length %d, diff 0x%x, pos 0x%x prev 0x%x",(int)length*100,(int)positionDiff,PreviousPosition,prevprev);
             SendJobProgress(0.0,0,false, Lenstr);
             Report(Lenstr,__FILE__,__LINE__,(int)TotalProcessedLength,RpWarning,(int)PoolerTotalProcessedLength,0);
+            length = 0;
 
         }
 
@@ -275,7 +276,7 @@ uint32_t ThreadSpeedControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
     int index=MAX_THREAD_MOTORS_NUM;
     int32_t i, avreageSampleValue = 0;
     //double tempcalcspeed = 0;
-    uint32_t calculated_speed;
+    double calculated_speed;
     float speed = getSensorSpeedData();
     if (IfIndex>>8 != IfTypeThread)
     {
@@ -297,7 +298,7 @@ uint32_t ThreadSpeedControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
                                                                               &MotorControlConfig[index].m_params,   &MotorControlConfig[index].m_preError, &MotorControlConfig[index].m_integral);
         //SetMotorFreq (index, MotorControlConfig[index].m_calculatedError);
         calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*OriginalMotorSpd_2PPS[index];
-        if (abs(calculated_speed-CurrentControlledSpeed[index])>2)
+        if (fabs(calculated_speed-CurrentControlledSpeed[index])>2)
         {
             CurrentControlledSpeed[index] = calculated_speed;
             MotorSetSpeed(ThreadMotorIdToMotorId[index], calculated_speed);
@@ -305,7 +306,7 @@ uint32_t ThreadSpeedControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
     }
  return OK;
 }
-uint32_t _speed;
+float _speed;
 uint32_t ThreadControlSpeedReadFunction(uint32_t IfIndex, uint32_t ReadValue)
 {
     int index;
@@ -393,7 +394,7 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
     int DancerId;
     int32_t TranslatedReadValue, avreageSampleValue = 0,avreageMotorSampleValue = 0;
     //double tempcalcspeed = 0;
-    uint32_t calculated_speed;
+    double calculated_speed;
     double NormalizedError;
 
     if (ThreadControlActive == false)
@@ -551,17 +552,17 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
         calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*OriginalMotorSpd_2PPS[index];
         //calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*CurrentControlledSpeed[index];
 #ifndef TEST_PID_THREAD
-        if (abs(calculated_speed-CurrentControlledSpeed[index])> MotorControlConfig[index].m_ingnoreValue)
+        if (fabs(calculated_speed-CurrentControlledSpeed[index])> MotorControlConfig[index].m_ingnoreValue)
 #else
         if (index == FEEDER_MOTOR) //feeder unit handles errors opposite to left unit
 #endif
         {
             CurrentControlledSpeed[index] = calculated_speed;
             MotorSetSpeed(ThreadMotorIdToMotorId[index], calculated_speed);
-            if (((JobCounter % 2000) == index*100)&&(index == WINDER_MOTOR)) //feeder unit handles errors opposite to left unit
+            /*if (((JobCounter % 2000) == index*100)&&(index == WINDER_MOTOR)) //feeder unit handles errors opposite to left unit
             {
-                Report("MotorSpeedUpdated",__FILE__,index,OriginalMotorSpd_2PPS[index],RpWarning,CurrentControlledSpeed[index],0);
-            }
+                Report("MotorSpeedUpdated",__FILE__,index,(int)OriginalMotorSpd_2PPS[index],RpWarning,(int)CurrentControlledSpeed[index],0);
+            }*/
 #ifdef TEST_PID_THREAD
             int len;
             if ((JobCounter % 2000) == index*100)
@@ -587,7 +588,10 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
 #endif
         }
 		else
+		{
            MotorFailedSample[index]++;
+           //LOG_ERROR(index,"No change in speed");
+		}
 
     }
 
@@ -595,7 +599,7 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
 }
 
 //********************************************************************************************************************
-uint32_t ThreadGetMotorSpeed(threadMotorsEnum MotorId)
+double ThreadGetMotorSpeed(threadMotorsEnum MotorId)
 {
     return  CurrentControlledSpeed[MotorId];
 }