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

8 files changed, 131 insertions, 84 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.c b/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.c
index 1c2b6662d..fa835ee2e 100644
--- a/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.c
+++ b/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.c
@@ -893,7 +893,7 @@ JobEndReasonEnum AlarmHandlingPrepareJob(void *CurrentJob)
 
         if (AlarmItem[Alarm_i].AlarmSource == ALARM_SOURCE_TYPE__CoversAlarm)
         {
-            if (AlarmState[Alarm_i].Status == true)
+            if ((AlarmState[Alarm_i].Status == true)&&(AlarmItem[Alarm_i].Severity == DEBUG_LOG_CATEGORY__Error))
             {
                 FoundReason = JOB_TAMPER_ALARM;
                 ReportWithPackageFilter(AlarmFilter,"tamper alarm preventing job", __FILE__,__LINE__,AlarmItem[Alarm_i].EventType, AlarmItem[Alarm_i].Severity, AlarmItem[Alarm_i].DeviceId, 0);
diff --git a/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.c b/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.c
index 8e01fe18e..421206b45 100644
--- a/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.c
+++ b/Software/Embedded_SW/Embedded/Modules/Diagnostics/Diagnostics.c
@@ -864,10 +864,10 @@ void DiagnosticOneSecCollection(void)
             tempFlow = HeadFlowMeter;
         }*/
         WasteLevel = GetWHSWasteTankLevelMiliLiter()/1000;//change from ml to litter
-        static double InitCounter = 0;
-        if (GetMachineState()<MACHINE_STATE_NO_PROCESS_PARAMS)
+        static double InitCounter = 60.0;
+        if ((GetMachineState()<MACHINE_STATE_NO_PROCESS_PARAMS) &&(GetMachineState()>=MACHINE_STATE_WAIT_FOR_COOLER))
         {
-            InitCounter+=1.0;
+            InitCounter-=1.0;
             DiagnosticsMonitor.chillertemperature = &InitCounter;
 
         }
@@ -1058,7 +1058,8 @@ void SendDiagnostics(void)
     DiagnosticsMonitor.dancer2angle = dancer2angle;
     DiagnosticsMonitor.dancer3angle = dancer3angle;
 */
-    if ((JobIsActive())&&(DiagnosticMode >= Diagnostic_Extended_Mode))
+//    if ((JobIsActive())&&(DiagnosticMode >= Diagnostic_Extended_Mode))
+    if (DiagnosticMode >= Diagnostic_Extended_Mode)
     {
         DiagnosticsMonitor.n_dancer1angle =  DancerCounterIndex[0];
         DiagnosticsMonitor.n_dancer2angle =  DancerCounterIndex[1];
diff --git a/Software/Embedded_SW/Embedded/Modules/General/GeneralHardware.c b/Software/Embedded_SW/Embedded/Modules/General/GeneralHardware.c
index bec8fc53f..27ed8d0e9 100644
--- a/Software/Embedded_SW/Embedded/Modules/General/GeneralHardware.c
+++ b/Software/Embedded_SW/Embedded/Modules/General/GeneralHardware.c
@@ -51,7 +51,7 @@
 #include "Drivers/I2C_Communication/Dispenser_Card/EEPROM/Dispenser_EEPROM.h"
 #include <Drivers/I2C_Communication/Dispenser_Card/IO_Ports/Dispenser_IO.h>
 #include "Modules/General/buttons.h"
-#include "Modules/Waste/Waste_ex.h"
+#include "Modules/Waste/Waste.h"
 #include "Drivers/I2C_Communication/Main_Board_EEPROM/Main_EEPROM.h"
 #include <Modules/Waste/newWHS_init.h>
 #include <Drivers/I2C_Communication/I2C_Task.h>
@@ -165,8 +165,12 @@ uint32_t HWConfigurationInit(void)
 #ifndef DISPESER_TEST
     if (WHS_Type == WHS_TYPE_NEW)
         newWHS_init();
+    //WHS_init(); // remove call to old WHS
 #endif
+
+    // Waste Init (WHS)
     Waste_Init();
+
     ADC_MUX_Init();
     GeneralHwReady = true;
 
diff --git a/Software/Embedded_SW/Embedded/Modules/General/Safety.c b/Software/Embedded_SW/Embedded/Modules/General/Safety.c
index 89ac8d020..099ac1769 100644
--- a/Software/Embedded_SW/Embedded/Modules/General/Safety.c
+++ b/Software/Embedded_SW/Embedded/Modules/General/Safety.c
@@ -46,7 +46,7 @@ uint32_t Safety_Main_State(uint32_t IfIndex, uint32_t BusyFlag)
    bool mDrierDoorAlarmState = false;
    bool mAirFlowAlarmState = false;
    bool mAirFilterAlarmState = false;
-   //bool mWasteOverflowAlarmState = false;
+   bool mWasteOverflowAlarmState = false;
 #ifdef CONTROL_DEBUG
     uint32_t tempp,tempq,delta;
     uint32_t sys_ticks_start = msec_millisecondCounter,sys_ticks_end,max = 0,dev = 0;
@@ -91,6 +91,16 @@ uint32_t Safety_Main_State(uint32_t IfIndex, uint32_t BusyFlag)
                    mDrierDoorAlarmState = true;
                    DrierDoorAlarmState = true;
                }
+               else
+               {
+                   //if (WHS_GPI_WASTE_OVERFULL()) - cannot read this switch
+                   {
+                       //report and handle waste overflow
+                       AlarmHandlingSetAlarm(EVENT_TYPE__CHILLER_DRY_CONTACT, true);
+                       mWasteOverflowAlarmState = true;
+                       WasteOverflowAlarmState = true;
+                   }
+               }
            }
        }
 
@@ -224,6 +234,12 @@ uint32_t Safety_Main_State(uint32_t IfIndex, uint32_t BusyFlag)
        AlarmHandlingSetAlarm(EVENT_TYPE__AIR_FILTER_NOT_INSTALLED, false);
        AirFilterAlarmState = mAirFilterAlarmState;
    }
+   if  ((mWasteOverflowAlarmState != WasteOverflowAlarmState)|| (mWasteOverflowAlarmState == false))
+   {
+      //alarm went off
+       AlarmHandlingSetAlarm(EVENT_TYPE__WASTE_CONTAINER_OVERFLOW, false);
+       WasteOverflowAlarmState = mWasteOverflowAlarmState;
+   }
 #ifdef CONTROL_DEBUG
                 tempq = HibernateRTCSSGet();
                 if (tempq < tempp)
diff --git a/Software/Embedded_SW/Embedded/Modules/General/buttons.h b/Software/Embedded_SW/Embedded/Modules/General/buttons.h
index 646861bc7..6a15d3765 100644
--- a/Software/Embedded_SW/Embedded/Modules/General/buttons.h
+++ b/Software/Embedded_SW/Embedded/Modules/General/buttons.h
@@ -59,7 +59,6 @@ uint32_t Buttons_Init(void);
 uint32_t Button_load_Init(void);
 uint32_t Button_JOG_Init(void);
 bool SetPowerMachineState(PBmachineState state);
-void Ink_Cart_Led();
 
 
 
diff --git a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c
index c444c7017..ac9bd0deb 100644
--- a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c
@@ -43,6 +43,8 @@ typedef struct
 HardwarePidControl *DispensersControl;
 HardwarePidControl DispensersCtrl[MAX_SYSTEM_DISPENSERS];
 #define MAX_DYE_DISPENSERS 6
+#define IDS_PRESEGMENT_TIME_STEP 50
+
 int32_t DispenserSamples[MAX_SYSTEM_DISPENSERS][MAX_CONTROL_SAMPLES] = {0};
 int DispenserSamplePointer[MAX_SYSTEM_DISPENSERS] = {0};
 double DispenserNormalizedErrorCoEfficient[MAX_SYSTEM_DISPENSERS] = {0};
@@ -117,8 +119,9 @@ void IDS_Dispenser_SetPreSegmentWFCFValues(double dispenserpresegmentwfcf, doubl
          LeftRockerSpeed = ids_leftcleaningmotorspeed;
      if ( ids_rightcleaningmotorspeed)
          RightRockerSpeed = ids_rightcleaningmotorspeed;
-     Report("IDS_Dispenser_SetPreSegmentCleaningValues ",__FILE__,__LINE__,CleaningDispenserSpeed,RpWarning,(int)LeftRockerSpeed,0);
+     Report("IDS_Dispenser_SetPreSegmentCleaningValues ",__FILE__,__LINE__,RightRockerSpeed,RpWarning,(int)LeftRockerSpeed,0);
      Report("IDS_Dispenser_SetPreSegmentCleaningValues ",__FILE__,__LINE__,InterSegmentStartSprayCleaner,RpWarning,(int)InterSegmentCenterRockers,0);
+     Report("IDS_Dispenser actuator times ",__FILE__,(int)LeftRockerSpeed/100*IDS_PRESEGMENT_TIME_STEP,(int)LeftRockerSpeed%100*IDS_PRESEGMENT_TIME_STEP,RpWarning,(int)RightRockerSpeed*IDS_PRESEGMENT_TIME_STEP,0);
 
  }
 
@@ -1034,7 +1037,6 @@ uint32_t InactiveDispenserHome(uint32_t DispenserId, uint32_t ReadValue)
 
 
 //********************************************************************************************************************
-#define IDS_PRESEGMENT_TIME_STEP 50
  uint32_t IDSPreSegmentStateCallbackRunner(uint32_t IfIndex, uint32_t ReadValue)
  {
      JobDispenser **Dispensers;
diff --git a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Progress.c b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Progress.c
index e5afba3b9..bfea7b13b 100644
--- a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Progress.c
+++ b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Progress.c
@@ -526,6 +526,14 @@ void Stub_ProgressRequest(MessageContainer* requestContainer)
         response.has_progress = true;
     }
     else
+    if(request->amount == 0xAD9) //halt
+    {
+        LOG_ERROR(request->delay,"halt");
+        memset (0,0,100000);
+        response.progress = IgnoreConeMissing;
+        response.has_progress = true;
+    }
+    else
     if((request->amount == 0x01) && ((request->delay &0x010000) == 0x010000)) //change mode powerset01
     {
         response.progress = Power_Step_01_Mode(((request->delay &0x00FF00)>>8), request->delay &0x0000FF);
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
index 42a56ae8a..1c52a9b68 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
@@ -352,9 +352,9 @@ uint32_t PoolerThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue)
     }
     if ((CurrentProcessedLength>=CurrentRequestedLength )&&(CurrentRequestedLength > 0.0))
     {
-        usnprintf(Lenstr, 100, "Total processed length: Feeder: %d Pooler %d",(int)TotalProcessedLength,(int)PoolerTotalProcessedLength);
+        usnprintf(Lenstr, 100, "Total processed length: Feeder: %d Puller %d",(int)TotalProcessedLength,(int)PoolerTotalProcessedLength);
         SendJobProgress(0.0,0,false, Lenstr);
-        ReportWithPackageFilter(ThreadFilter,Lenstr,__FILE__,__LINE__,(int)TotalProcessedLength,RpWarning,(int)PoolerTotalProcessedLength,0);
+        ReportWithPackageFilter(ThreadFilter,Lenstr,__FILE__,__LINE__,(int)(TotalProcessedLength*100),RpWarning,(int)(PoolerTotalProcessedLength*100),0);
         // segment/intersegment/distance to spool finished
         if (ProcessedLengthFuncPtr)
             ProcessedLengthFuncPtr();
@@ -696,18 +696,19 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
             }*`/
         }*/
         calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*OriginalMotorSpd_2PPS[index];
+        calculated_speed = calculated_speed*InitialDryerSpeed/OriginalMotorSpd_2PPS[DRYER_MOTOR];
         //calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*CurrentControlledSpeed[index];
-        //if (JobCounter % eHundredMillisecond == 50)
-        if (FirstCalcInJob == true)
+        if (0)//(JobCounter % 1000 == 0)
+        //if (JobCounter < 100)//(FirstCalcInJob == true)
         {
-            if (index == POOLER_MOTOR)
+            if (index == FEEDER_MOTOR)
             {
-                FirstCalcInJob = false;
-                len = usnprintf(TMessage, 150, "read %d avg %d error(6) %d integral(9) %d,delta(9) %d, calc(3) %d speed %d",
-                                ReadValue,avreageSampleValue,(int)(MotorControlConfig[index].m_mesuredParam*1000000),
+           //     FirstCalcInJob = false;
+                len = usnprintf(TMessage, 150, "read %d avg %d error(6) %d integral(9) %d,delta(9) %d, calc(3) %d speed %d %d",
+                                TranslatedReadValue,avreageSampleValue,(int)(MotorControlConfig[index].m_mesuredParam*1000000),
                                 (int)(MotorControlConfig[index].m_integral*1000000000),(int)((MotorControlConfig[index].m_mesuredParam*MotorControlConfig[index].m_params.dt)*1000000000),
-                                (int)(MotorControlConfig[index].m_calculatedError*1000),(int)calculated_speed);
-                ReportWithPackageFilter(ThreadFilter,TMessage,__FILE__,__LINE__,DancerId,RpError,ReadValue,0);
+                                (int)(MotorControlConfig[index].m_calculatedError*1000),(int)calculated_speed, (int)(InitialDryerSpeed*100/OriginalMotorSpd_2PPS[DRYER_MOTOR]));
+                ReportWithPackageFilter(ThreadFilter,TMessage,__FILE__,MotorSamplePointer[index],JobCounter,RpError,ReadValue,0);
 
             }
         }
@@ -720,6 +721,11 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
         {
             if (calculated_speed>5.0)
             {
+                if (calculated_speed>(CurrentControlledSpeed[index]+100))
+                {
+                        ReportWithPackageFilter(ThreadFilter,"limit acceleration",__FILE__,calculated_speed,CurrentControlledSpeed[index],RpError,CurrentControlledSpeed[index]+100,0);
+                        calculated_speed=CurrentControlledSpeed[index]+100;
+                }
                 CurrentControlledSpeed[index] = calculated_speed;
                 MotorSetSpeed(ThreadMotorIdToMotorId[index], calculated_speed);
             }
@@ -845,6 +851,7 @@ uint32_t Release_Right_TFU_Tension()
     return status;
 }
 int SecondFeederCorrection = 4;
+int PrepareWaitCount = 0;
 uint32_t Adjust_Right_TFU_Tension_2nd_Callback(uint32_t MotorId, uint32_t ReadValue)
 {
     MotorStop (HARDWARE_MOTOR_TYPE__MOTO_RDANCER,Soft_Stop); //per L6470 errata between mov and run commands
@@ -854,6 +861,18 @@ uint32_t Adjust_Right_TFU_Tension_2nd_Callback(uint32_t MotorId, uint32_t ReadVa
         Report("release tension - job aborted",__FILE__,__LINE__,MotorId,RpMessage,0,0);
         Release_Right_TFU_Tension();
     }
+    if (PrepareWaitCount)
+    {
+        ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_TensionCallback",__FILE__,__LINE__,2,RpWarning,PrepareWaitCount,0);
+        PrepareWaitCount--;
+    }
+    if ((PrepareWaitCount == 0)&&(PrepareState == true))
+    {
+        PrepareState = false;
+        ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_TensionCallback  Prepare Ready",__FILE__,__LINE__,2,RpWarning,PrepareWaitCount,0);
+        PrepareReady(Module_Thread,ModuleDone);
+    }
+
     return OK;
 }
 uint32_t Adjust_Right_TFU_Tension_Callback(uint32_t MotorId, uint32_t ReadValue)
@@ -869,16 +888,16 @@ uint32_t Adjust_Right_TFU_Tension(double tension)
     uint32_t status = OK;
     if (tension > 0.5) //0 = lower position, 1 = high position
     {
+        //PrepareWaitCount++;
         if (FPGA_Read_limit_Switches(GPI_LS_RDANCER_UP) == NO_LIMIT)
         {
-            MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RDANCER,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].directionthreadwize, 40,  GPI_LS_RDANCER_UP, Adjust_Right_TFU_Tension_Callback,15000);
+            MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RDANCER,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].directionthreadwize, 15,  GPI_LS_RDANCER_UP, Adjust_Right_TFU_Tension_Callback,15000);
             Report("Adjust_Right_TFU_Tension",__FILE__,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].directionthreadwize,HARDWARE_MOTOR_TYPE__MOTO_RDANCER,RpMessage,GPI_LS_RDANCER_UP,0);
         }
     }
 
     return status;
 }
-int PrepareWaitCount = 0;
 uint32_t ThreadPrepare_TensionCallback (int DancerId, double tension)
 {
     if (PrepareWaitCount)
@@ -886,7 +905,7 @@ uint32_t ThreadPrepare_TensionCallback (int DancerId, double tension)
         ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_TensionCallback",__FILE__,__LINE__,DancerId,RpWarning,PrepareWaitCount,0);
         PrepareWaitCount--;
     }
-    if (PrepareWaitCount == 0)
+    if ((PrepareWaitCount == 0)&&(PrepareState == true))
     {
         PrepareState = false;
         ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_TensionCallback  Prepare Ready",__FILE__,__LINE__,DancerId,RpWarning,PrepareWaitCount,0);
@@ -1046,71 +1065,73 @@ uint32_t ThreadPrepareState(void *JobDetails)
     {
         HW_Motor_Id = ThreadMotorIdToMotorId[Motor_i];
         Pid_Id = Motor_i;/*ThreadMotorIdToControlId[Motor_i];*/
-            MotorControlConfig[Motor_i].m_params.MAX = 1;
-            MotorControlConfig[Motor_i].m_params.MIN = MotorsControl[Pid_Id].outputproportionalpowerlimit*-1;
-            MotorControlConfig[Motor_i].m_params.Kd = MotorsControl[Pid_Id].derivativetime;
-            MotorControlConfig[Motor_i].m_params.Kp = MotorsControl[Pid_Id].proportionalgain;
-            MotorControlConfig[Motor_i].m_params.Ki = MotorsControl[Pid_Id].integraltime;
-            MotorControlConfig[Motor_i].m_params.IntegralErrorMultiplier = MotorsControl[Pid_Id].setpointramprateorsoftstartramp;
-            MotorControlConfig[Motor_i].m_params.ProportionalErrorMultiplier = MotorsControl[Pid_Id].outputonoffhysteresisvalue;
-            MotorControlConfig[Motor_i].m_params.epsilon = MotorsControl[Pid_Id].epsilon;
-            MotorControlConfig[Motor_i].m_params.dt = MotorsControl[Pid_Id].controloutputtype;
-            MotorControlConfig[Motor_i].m_ingnoreValue =  MotorsControl[Pid_Id].sensorcorrectionadjustment; // the minimal change required to change the motor speed in pulses
-            MotorControlConfig[Motor_i].m_calculatedError = 0;
-            MotorControlConfig[Motor_i].m_integral = 0;
-            MotorControlConfig[Motor_i].m_isEnabled = true;
-            MotorControlConfig[Motor_i].m_isReady = true;
-            MotorControlConfig[Motor_i].m_mesuredParam = 0;
-            MotorControlConfig[Motor_i].m_preError = 0;
-            MotorControlConfig[Motor_i].m_SetParam = 0;//need to update SetParams on presegment stage
+        MotorControlConfig[Motor_i].m_params.MAX = 1;
+        MotorControlConfig[Motor_i].m_params.MIN = MotorsControl[Pid_Id].outputproportionalpowerlimit*-1;
+        MotorControlConfig[Motor_i].m_params.Kd = MotorsControl[Pid_Id].derivativetime;
+        MotorControlConfig[Motor_i].m_params.Kp = MotorsControl[Pid_Id].proportionalgain;
+        MotorControlConfig[Motor_i].m_params.Ki = MotorsControl[Pid_Id].integraltime;
+        MotorControlConfig[Motor_i].m_params.IntegralErrorMultiplier = MotorsControl[Pid_Id].setpointramprateorsoftstartramp;
+        MotorControlConfig[Motor_i].m_params.ProportionalErrorMultiplier = MotorsControl[Pid_Id].outputonoffhysteresisvalue;
+        MotorControlConfig[Motor_i].m_params.epsilon = MotorsControl[Pid_Id].epsilon;
+        MotorControlConfig[Motor_i].m_params.dt = MotorsControl[Pid_Id].controloutputtype;
+        MotorControlConfig[Motor_i].m_ingnoreValue =  MotorsControl[Pid_Id].sensorcorrectionadjustment; // the minimal change required to change the motor speed in pulses
+        MotorControlConfig[Motor_i].m_calculatedError = 0;
+        MotorControlConfig[Motor_i].m_integral = 0;
+        MotorControlConfig[Motor_i].m_isEnabled = true;
+        MotorControlConfig[Motor_i].m_isReady = true;
+        MotorControlConfig[Motor_i].m_mesuredParam = 0;
+        MotorControlConfig[Motor_i].m_preError = 0;
+        MotorControlConfig[Motor_i].m_SetParam = 0;//need to update SetParams on presegment stage
 
-		    HandleJobThreadControlParameters(JobTicket->threadparameters);  //OVERRIDES CONFIGURATION PARAMETERS!!!
+        HandleJobThreadControlParameters(JobTicket->threadparameters);  //OVERRIDES CONFIGURATION PARAMETERS!!!
 
-            temp_dt = MotorControlConfig[Motor_i].m_params.dt/0.001;
-            MotorTiming[Motor_i] = (int)temp_dt;
-            if (MotorTiming[Motor_i])
-            {
-                MotorTimer[Motor_i] = MotorTiming[Motor_i]-1;
-                ReportWithPackageFilter(ThreadFilter,"MotorTiming",__FILE__,Motor_i,MotorTiming[Motor_i],RpWarning,MotorTimer[Motor_i],0);
-            }
+        temp_dt = MotorControlConfig[Motor_i].m_params.dt/0.001;
+        MotorTiming[Motor_i] = (int)temp_dt;
+        if (MotorTiming[Motor_i])
+        {
+            MotorTimer[Motor_i] = MotorTiming[Motor_i]-1;
+            ReportWithPackageFilter(ThreadFilter,"MotorTiming",__FILE__,Motor_i,MotorTiming[Motor_i],RpWarning,MotorTimer[Motor_i],0);
+        }
 //////////////////////////////////////////////////
-            for (i = 0;i < (int)MotorsControl[Motor_i].pvinputfilterfactormode; i++)
-            {
-                //if (Motor_i == DRYER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled
-                    MotorSamples[Motor_i][i] = 0;
-               // else if ((Motor_i == POOLER_MOTOR)||(Motor_i == FEEDER_MOTOR))
-                //    MotorSamples[Motor_i][i] = DancersCfg[ThreadMotorIdToDancerId[Motor_i]].zeropoint;
-                //MotorSpeedSamples[Motor_i][i] = 0;
-            }
-            MotorSamplePointer[Motor_i] = 0;
+        for (i = 0;i < (int)MotorsControl[Motor_i].pvinputfilterfactormode; i++)
+        {
+            //if (Motor_i == DRYER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled
+                MotorSamples[Motor_i][i] = 0;
+                if (Motor_i == FEEDER_MOTOR)
+                    MotorSamples[Motor_i][i] = -500;
+           // else if ((Motor_i == POOLER_MOTOR)||(Motor_i == FEEDER_MOTOR))
+            //    MotorSamples[Motor_i][i] = DancersCfg[ThreadMotorIdToDancerId[Motor_i]].zeropoint;
+            //MotorSpeedSamples[Motor_i][i] = 0;
+        }
+        MotorSamplePointer[Motor_i] = 0;
 /////////////////////////////////////////////////////
-            MotorSetDirection((TimerMotors_t)HW_Motor_Id,MotorsCfg[HW_Motor_Id].directionthreadwize);
+        MotorSetDirection((TimerMotors_t)HW_Motor_Id,MotorsCfg[HW_Motor_Id].directionthreadwize);
 
-            if (Motor_i == FEEDER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled
+        if (Motor_i == FEEDER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled
+        {
+            ReportWithPackageFilter(ThreadFilter,"Feeder Control",__FILE__,Motor_i,MotorControlConfig[Motor_i].m_params.Kp,RpWarning,MotorControlConfig[Motor_i].m_params.Ki,0);
+            if (SpeedControlId != 0xFF)
             {
-                ReportWithPackageFilter(ThreadFilter,"Feeder Control",__FILE__,Motor_i,MotorControlConfig[Motor_i].m_params.Kp,RpWarning,MotorControlConfig[Motor_i].m_params.Ki,0);
-                if (SpeedControlId != 0xFF)
-                {
-                    RemoveControlCallback(SpeedControlId,ThreadLengthCBFunction);
-                    SpeedControlId = 0xFF;
-                }
-                //SetMotHome(ThreadMotorIdToMotorId[Motor_i]);
-                LengthCalculationMultiplier = (MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].pulleyradius*2*PI)/(MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].pulseperround*MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].microstep);
-                SpeedControlId = AddControlCallback(NULL,ThreadLengthCBFunction, eHundredMillisecond,MotorGetPositionFromFPGA,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],Motor_i);
+                RemoveControlCallback(SpeedControlId,ThreadLengthCBFunction);
+                SpeedControlId = 0xFF;
             }
-            if (Motor_i == POOLER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled
+            //SetMotHome(ThreadMotorIdToMotorId[Motor_i]);
+            LengthCalculationMultiplier = (MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].pulleyradius*2*PI)/(MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].pulseperround*MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].microstep);
+            SpeedControlId = AddControlCallback(NULL,ThreadLengthCBFunction, eHundredMillisecond,MotorGetPositionFromFPGA,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],Motor_i);
+        }
+        if (Motor_i == POOLER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled
+        {
+            ReportWithPackageFilter(ThreadFilter,"Puller Control",__FILE__,Motor_i,MotorControlConfig[Motor_i].m_params.Kp,RpWarning,MotorControlConfig[Motor_i].m_params.Ki,0);
+            if (PoolerSpeedControlId != 0xFF)
             {
-                ReportWithPackageFilter(ThreadFilter,"Puller Control",__FILE__,Motor_i,MotorControlConfig[Motor_i].m_params.Kp,RpWarning,MotorControlConfig[Motor_i].m_params.Ki,0);
-                if (PoolerSpeedControlId != 0xFF)
-                {
-                    if (RemoveControlCallback(PoolerSpeedControlId,PoolerThreadLengthCBFunction)!=OK)
-                        ReportWithPackageFilter(ThreadFilter,"Remove Control Failed.",__FILE__,__LINE__,(int)Motor_i,RpError,(int)PoolerSpeedControlId,0);
-                     PoolerSpeedControlId = 0xFF;
-                }
-                //SetMotHome(ThreadMotorIdToMotorId[Motor_i]);
-                PoolerLengthCalculationMultiplier = (MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].pulleyradius*2*PI)/(MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].pulseperround*MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].microstep);
-                PoolerSpeedControlId = AddControlCallback(NULL,PoolerThreadLengthCBFunction, eHundredMillisecond,MotorGetPositionFromFPGA,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],Motor_i);
+                if (RemoveControlCallback(PoolerSpeedControlId,PoolerThreadLengthCBFunction)!=OK)
+                    ReportWithPackageFilter(ThreadFilter,"Remove Control Failed.",__FILE__,__LINE__,(int)Motor_i,RpError,(int)PoolerSpeedControlId,0);
+                 PoolerSpeedControlId = 0xFF;
             }
+            //SetMotHome(ThreadMotorIdToMotorId[Motor_i]);
+            PoolerLengthCalculationMultiplier = (MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].pulleyradius*2*PI)/(MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].pulseperround*MotorsCfg[ThreadMotorIdToMotorId[Motor_i]].microstep);
+            PoolerSpeedControlId = AddControlCallback(NULL,PoolerThreadLengthCBFunction, eHundredMillisecond,MotorGetPositionFromFPGA,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],Motor_i);
+        }
             if (Motor_i == FEEDER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled
             {
                 if (ControlIdtoMotorId[Motor_i] != 0xFF)
@@ -1152,10 +1173,6 @@ uint32_t ThreadPrepareState(void *JobDetails)
                 ControlIdtoMotorId[Motor_i] = AddControlCallback(NULL,ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i);
 #endif
             }
-//            if (HW_Motor_Id == HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled
-//                AddControlCallback(ThreadSpeedControlCBFunction, eOneMillisecond,TemplateDataReadCBFunction,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],0);
-            if (Motor_i == ThreadMotorIdToMotorId[DRYER_MOTOR]) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will not be controlled
-                continue;
     }
 
 #ifdef TEST_PID_THREAD
@@ -1382,7 +1399,7 @@ char Endstr[150];
      int Motor_i;
      ThreadControlActive = false;
      uint32_t status = OK;
-     usnprintf(Endstr, 100, "Total _processed length: Feeder: %d Pooler %d",(int)TotalProcessedLength,(int)PoolerTotalProcessedLength);
+     usnprintf(Endstr, 100, "Total _processed length: Feeder: %d Puller %d",(int)TotalProcessedLength,(int)PoolerTotalProcessedLength);
      SendJobProgress(0.0,0,false, Endstr);
      ReportWithPackageFilter(ThreadFilter,Endstr,__FILE__,__LINE__,(int)TotalProcessedLength,RpWarning,(int)PoolerTotalProcessedLength,0);