Last-Modified: Wed, 15 Jul 2026 17:30:56 GMT Expires: Sat, 12 Jul 2036 17:30:56 GMT ProtoProperty.cs « Tango.CodeGeneration « Visual_Studio « Software - Tango - Twine softwares
aboutsummaryrefslogtreecommitdiffstats
path: root/Software/Visual_Studio/Tango.CodeGeneration/ProtoProperty.cs
blob: d63348b6bb5c90f0d73e930d697b90e8845a1dad (plain) 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace Tango.CodeGeneration
{
    public class ProtoProperty : Property
    {
        public bool Repeated { get; set; }

        public String Description { get; set; }

        public ProtoProperty()
        {

        }

        public ProtoProperty(String name, String type) : this()
        {
            Name = name;
            Type = type;
        }
    }
}
generated by cgit v1.3.1 (git 2.54.0) at 2026-07-15 17:30:56 +0000
/************************************************************************************************************************
 * Thread_print.c
 * Printing module is responsible for :
     * operating diffrent winding algorithms with predefined parameters from the UI
     * operating the dispensers according to predefined dispensing rate from the UI
 **************************************************************************************************************************/
#include "include.h"
#include <DataDef.h>
#include "thread.h"
#include "thread_ex.h"
#include "../control/control.h"
#include "../control/pidalgo.h"
#include "PMR/Hardware/HardwareMotor.pb-c.h"
#include "PMR/Hardware/HardwareMotorType.pb-c.h"
#include "PMR/Hardware/HardwareDancerType.pb-c.h"
#include "PMR/Printing/JobSegment.pb-c.h"
#include "PMR/Printing/JobTicket.pb-c.h"
#include "PMR/Printing/ThreadParameters.pb-c.h"
#include  <PMR/Diagnostics/EventType.pb-c.h>

#include <utils/ustdlib.h>

#include "StateMachines/Printing/PrintingSTM.h"

#include "drivers/Motors/Motor.h"
//#include "drivers/SSI_Comm/ssi_comm.h"
#include "drivers/SSI_Comm/Dancer/Dancer.h"
#include "drivers/Heater/TemperatureSensor.h"
#include "drivers/Heater/Heater.h"
#include "drivers/Motors/Motor.h"
#include "drivers/FPGA/FPGA_GPIO/FPGA_GPIO.h"
#include "drivers/FPGA/FPGA_SPI_Comm.h"
#include "modules/heaters/heaters.h"
#include "modules/General/process.h"
#include "modules/ids/ids_ex.h"
#include "Modules/AlarmHandling/AlarmHandling.h"
#include "Control/MillisecTask.h"
#include "drivers/Flash_ram/MCU_E2Prom.h"

#include "drivers/SSI_Comm/SSI_Comm.h"

////////////////////////////////State machine operation////////////////////////////////////
//the state machine operation is used to operate in runtime correct profile flow execution
//by recieved esign flow of the user from the UI
///////////////////////////////////////////////////////////////////////////////////////////

double CurrentControlledSpeed[MAX_THREAD_MOTORS_NUM] = {0};

#ifdef FOUR_WINDERS
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,Winder_2_Motor,Winder_3_Motor,Winder_4_Motor,HARDWARE_MOTOR_TYPE__MOTO_SCREW};
Dancers_4_Winders ThreadMotorIdToDancerId[MAX_THREAD_MOTORS_NUM] = {NUM_OF_DANCERS,NUM_OF_DANCERS,HARDWARE_DANCER_1,HARDWARE_DANCER_0,HARDWARE_DANCER_4,HARDWARE_DANCER_3,HARDWARE_DANCER_2,NUM_OF_DANCERS};
uint32_t    ControlIdtoMotorId [MAX_THREAD_MOTORS_NUM] = {0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF};
#else
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,0xFF,0xFF,0xFF,0xFF};
#endif
uint32_t    SpeedControlId=0xFF;
uint32_t    PoolerSpeedControlId=0xFF;

double DancerError[NUM_OF_DANCERS] = {0.0};
double OriginalMotorSpd_2PPS[MAX_THREAD_MOTORS_NUM] = {0};
double InitialDryerSpeed = 0.0;
uint32_t JobCounter = 0;

MotorControlConfig_t MotorControlConfig[MAX_THREAD_MOTORS_NUM];
uint32_t DeviceId2Motor[MAX_THREAD_MOTORS_NUM];

int MotorTiming[MAX_THREAD_MOTORS_NUM];
int MotorTimer[MAX_THREAD_MOTORS_NUM];


uint32_t PreviousPosition = 0, CurrentPosition = 0;
double CurrentRequestedLength = 0.0;
double CurrentProcessedLength = 0.0;
double TotalProcessedLength = 0.0;
double LengthCalculationMultiplier;

uint32_t PoolerPreviousPosition = 0, PoolerCurrentPosition = 0;
double PoolerTotalProcessedLength = 0.0;
double PoolerLengthCalculationMultiplier;

double TempPoolerTotalProcessedLength = 0.0;
double TempTotalProcessedLength = 0.0;

bool InitialProcess = false;
bool PrepareState = false;

// job parameters
bool EnableLubrication = false;
bool EnableIntersegment = false;
double IntersegmentLength = 0;
bool Thread_Rockers_Bypass = false;


int CurrentSegmentId = 0;
typedef  void (* ProcessedLengthFunc)(void);
ProcessedLengthFunc ProcessedLengthFuncPtr = NULL;
// segment/intersegment/distance to spool finished
void ThreadSegmentEnded(void);
void ThreadInterSegmentEnded(void);
void ThreadDistanceToSpoolEnded(void);
uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue);

bool SegmentState = false;
bool PreSegmentState = false;
bool DTSState = false;
void SendSegmentFail(void);

double KeepNormalizedError = 0;
bool ThreadControlActive = false;
////////////////////////Slow Motor State////////////////////////////////////
//uint32_t ThreadPreSegmentState(void *JobDetails);

////////////////////////////////////////////////////////////////////////////
/********************************************************************
*
*    Name        : GTIME_Delta_Time_Pass
*
*    Parameters  : start_time.
*
*    Return      : time pass from start time
*
*    Description :
*
*********************************************************************/

uint32_t Control_Delta_Position_Pass(uint32_t Current_Read,uint32_t Previous_Read)
{
    uint32_t Time_Pass;
//  #define   MAX_COUNTER 0x3FFF  //14 bits
    #define   MAX_COUNTER 0x3FFFFF  //22 bits


  if (Current_Read < Previous_Read)
  {
    Time_Pass = (MAX_COUNTER - Previous_Read) + Current_Read + 1;
    ReportWithPackageFilter(ThreadFilter,"Length rollover",__FILE__,Time_Pass,(int)Current_Read,RpWarning,(int)Previous_Read,0);
  }
  else
    Time_Pass = Current_Read - Previous_Read;

  return (Time_Pass);
}
/*****************************************************************************************
 *
 *
 *
 *
 *
 *
 * **************************************************************************************/
uint32_t initialpos = 0xFFFF;
uint32_t Poolerinitialpos = 0xFFFF;
#define SPEED_STORE_SIZE 20
float PullerSpeedStore[SPEED_STORE_SIZE];
float PullerSpeedAverage;
int PullerSpeedIndex = 0;
float FeederSpeedStore[SPEED_STORE_SIZE];
float FeederSpeedAverage;
int FeederSpeedIndex = 0,Speed_i;

void ThreadUpdateProcessLength (double length, void *Funcptr)
{
    REPORT_MSG(length,"ThreadUpdateProcessLength");
    CurrentRequestedLength = length*100;//Centimetres
    CurrentProcessedLength = 0;
    ProcessedLengthFuncPtr = (ProcessedLengthFunc)Funcptr;
}
char Lenstr[160];
uint32_t ThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue)
{
    uint32_t positionDiff = 0,prevprev;
    double length = 0.0;

    int index = MAX_THREAD_MOTORS_NUM;
#ifndef FEEDER_LENGTH_CALCULATION
    if (ThreadControlActive == false)
        return OK;
    if (PrepareState == true)
        return OK;
#endif
    if (IfIndex>>8 != IfTypeThread)
    {
        ReportWithPackageFilter(ThreadFilter,"Wrong  Interface type",__FILE__,__LINE__,(int)IfIndex,RpError,(int)PoolerTotalProcessedLength,0);
        return 0xFFFFFFFF;
    }
    index = IfIndex&0xFF;
//    if (CurrentRequestedLength == 0.0)
//        return OK;
    if (index != FEEDER_MOTOR)
    {
        ReportWithPackageFilter(ThreadFilter,"Wrong  Motor",__FILE__,__LINE__,(int)IfIndex,RpError,(int)index,0);
        return 0xFFFFFFFF;
    }
    CurrentPosition = MotorGetPosition(ThreadMotorIdToMotorId[index]);
    if (CurrentPosition != 0)
    {
        if (initialpos == 0xFFFF)
        {
            PreviousPosition = CurrentPosition;
            initialpos = 0;
        }
        prevprev = PreviousPosition;
        if (Extended_Motor_Param[ThreadMotorIdToMotorId[index]] == true) //powerstep driver reverses the direction
            positionDiff = Control_Delta_Position_Pass(PreviousPosition,CurrentPosition);
        else
            positionDiff = Control_Delta_Position_Pass(CurrentPosition,PreviousPosition);
        //positionDiff = positionDiff / MotorsCfg[ThreadMotorIdToMotorId[index]].microstep;
        PreviousPosition = CurrentPosition;

        // total length = (position diff / full cycle) * pulley perimeter
        //(positionDiff/pulseperround)*((2*PI*motor_Radius)

        //positionDiff = positionDiff / MotorsCfg[ThreadMotorIdToMotorId[index]].microstep;
        length = (double)(positionDiff)*LengthCalculationMultiplier;

        if (length > 1000)
        {
            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);
            ReportWithPackageFilter(ThreadFilter,Lenstr,__FILE__,__LINE__,(int)TotalProcessedLength,RpWarning,(int)PoolerTotalProcessedLength,0);
            length = 0;

        }

    }
    /*FeederSpeedStore[FeederSpeedIndex++] = length;
    if (FeederSpeedIndex>=SPEED_STORE_SIZE)
    {
        FeederSpeedIndex = 0;
        FeederSpeedAverage = 0;
        for (Speed_i = 0;Speed_i<SPEED_STORE_SIZE;Speed_i++)
            FeederSpeedAverage+=FeederSpeedStore[Speed_i];
        FeederSpeedAverage = FeederSpeedAverage/SPEED_STORE_SIZE;
        ReportWithPackageFilter(ThreadFilter,"Avg len 100ms last 2 sec",__FILE__,(int)PoolerTotalProcessedLength,(int)(FeederSpeedAverage*1000),RpWarning,(int)(PullerSpeedAverage*1000),0);
    }*/
    TotalProcessedLength += (length/100);
    TempTotalProcessedLength = TotalProcessedLength;
#ifdef FEEDER_LENGTH_CALCULATION
    CurrentProcessedLength+=length;

    static int feeder_counter = 0;
    feeder_counter++;
    if (feeder_counter%10 == 0)
    {
        if (PrepareState == true)
        {
            //later - add temperatures
             TemperatureListString(Lenstr);

            SendJobProgress(0.0,0,false, Lenstr);
        }
        else
        {
            SendJobProgress(TotalProcessedLength,0,false, NULL);
        }

    }
    if ((CurrentProcessedLength>=CurrentRequestedLength )&&(CurrentRequestedLength > 0.0))
    {
        usnprintf(Lenstr, 100, "Total processed length: Feeder: %d Pooler %d",(int)TotalProcessedLength,(int)PoolerTotalProcessedLength);
        SendJobProgress(0.0,0,false, Lenstr);
        ReportWithPackageFilter(ThreadFilter,Lenstr,__FILE__,__LINE__,(int)TotalProcessedLength,RpWarning,(int)PoolerTotalProcessedLength,0);
        // segment/intersegment/distance to spool finished
        if (ProcessedLengthFuncPtr)
            ProcessedLengthFuncPtr();

    }
#endif

    return OK;
}
uint32_t PoolerThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue)
{
    uint32_t positionDiff = 0,prevprev;
    double length = 0.0;
    int index = MAX_THREAD_MOTORS_NUM;
#ifdef FEEDER_LENGTH_CALCULATION
    if (ThreadControlActive == false)
        return OK;
    if (PrepareState == true)
        return OK;
#endif
    if (IfIndex>>8 != IfTypeThread)
    {
        ReportWithPackageFilter(ThreadFilter,"Wrong  Interface type",__FILE__,__LINE__,(int)IfIndex,RpError,(int)PoolerTotalProcessedLength,0);
        return 0xFFFFFFFF;
    }
    index = IfIndex&0xFF;
//    if (CurrentRequestedLength == 0.0)
//        return OK;
    if (index != POOLER_MOTOR)
    {
        ReportWithPackageFilter(ThreadFilter,"Wrong  Motor",__FILE__,__LINE__,(int)IfIndex,RpError,(int)index,0);
        return 0xFFFFFFFF;
    }
    PoolerCurrentPosition = MotorGetPosition(ThreadMotorIdToMotorId[index]);
    //if (PoolerCurrentPosition != 0)
    //{
        if (Poolerinitialpos == 0xFFFF)
        {
            PoolerPreviousPosition = PoolerCurrentPosition;
            Poolerinitialpos = 0;
        }
        prevprev = PoolerPreviousPosition;
        if (Extended_Motor_Param[ThreadMotorIdToMotorId[index]] == true) //powerstep driver reverses the direction
            positionDiff = Control_Delta_Position_Pass(PoolerPreviousPosition,PoolerCurrentPosition);
        else
            positionDiff = Control_Delta_Position_Pass(PoolerCurrentPosition,PoolerPreviousPosition);
        //positionDiff = positionDiff / MotorsCfg[ThreadMotorIdToMotorId[index]].microstep;
        PoolerPreviousPosition = PoolerCurrentPosition;

        // total length = (position diff / full cycle) * pulley perimeter
        //(positionDiff/pulseperround)*((2*PI*motor_Radius)

        //positionDiff = positionDiff / MotorsCfg[ThreadMotorIdToMotorId[index]].microstep;
        length = (double)(positionDiff)*PoolerLengthCalculationMultiplier;

        if (length > 1000)
        {
            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);
            ReportWithPackageFilter(ThreadFilter,Lenstr,__FILE__,__LINE__,(int)TotalProcessedLength,RpWarning,(int)PoolerTotalProcessedLength,0);
            length = 0;

        }
        /*PullerSpeedStore[PullerSpeedIndex++] = length;
        if (PullerSpeedIndex>=SPEED_STORE_SIZE)
        {
            PullerSpeedIndex = 0;
            PullerSpeedAverage = 0;
            for (Speed_i = 0;Speed_i<SPEED_STORE_SIZE;Speed_i++)
                PullerSpeedAverage+=PullerSpeedStore[Speed_i];
            PullerSpeedAverage = PullerSpeedAverage/SPEED_STORE_SIZE;
            //ReportWithPackageFilter(ThreadFilter,"Average Speed 2 second",__FILE__,__LINE__,(int)(FeederSpeedAverage*100),RpWarning,(int)(PullerSpeedAverage*100),0);
        }**/

    //}

    PoolerTotalProcessedLength+= (length/100);
    TempPoolerTotalProcessedLength = PoolerTotalProcessedLength;
#ifndef FEEDER_LENGTH_CALCULATION
    CurrentProcessedLength+=length;

    static int puller_counter = 0;
    puller_counter++;
    if (puller_counter%10 == 0)
    {
        if (PrepareState == true)
        {
            TemperatureListString(Lenstr);
            SendJobProgress(0.0,0,false, Lenstr);
        }
        else
        {
            SendJobProgress(PoolerTotalProcessedLength,0,false, NULL);
        }

    }
    if ((CurrentProcessedLength>=CurrentRequestedLength )&&(CurrentRequestedLength > 0.0))
    {
        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*100),RpWarning,(int)(PoolerTotalProcessedLength*100),0);
        // segment/intersegment/distance to spool finished
        if (ProcessedLengthFuncPtr)
            ProcessedLengthFuncPtr();

    }
#endif

return OK;
}



float SpeedSamples[MAX_CONTROL_SAMPLES] = {0};

uint32_t ThreadSpeedControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
{
    //read value is the dancer angle
    int index=MAX_THREAD_MOTORS_NUM;
    int32_t i, avreageSampleValue = 0;
    //double tempcalcspeed = 0;
    double calculated_speed;
    float speed = getSensorSpeedData();
    if (IfIndex>>8 != IfTypeThread)
    {
        ReportWithPackageFilter(ThreadFilter,"Wrong  Interface type",__FILE__,__LINE__,(int)IfIndex,RpError,(int)PoolerTotalProcessedLength,0);
        return 0xFFFFFFFF;
    }
    index = IfIndex&0xFF;
    SpeedSamples[MotorSamplePointer[index]] = speed;//(-1 * TranslatedReadValue);
    MotorSamplePointer[index]++;
    if (MotorSamplePointer[index] >= (int)MotorsControl[index].pvinputfilterfactormode)
        MotorSamplePointer[index] = 0;
    for (i=0;i<(int)MotorsControl[index].pvinputfilterfactormode;i++)
        avreageSampleValue += SpeedSamples[i];
    avreageSampleValue = avreageSampleValue / (int)MotorsControl[index].pvinputfilterfactormode;
    if(MotorControlConfig[index].m_isEnabled && (MotorControlConfig[index].m_SetParam != 0))
    {
        MotorControlConfig[index].m_mesuredParam = ReadValue;
        MotorControlConfig[index].m_calculatedError = PIDAlgorithmCalculation(MotorControlConfig[index].m_SetParam , MotorControlConfig[index].m_mesuredParam,
                                                                              &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 (fabs(calculated_speed-CurrentControlledSpeed[index])>2)
        {
            CurrentControlledSpeed[index] = calculated_speed;
            MotorSetSpeed(ThreadMotorIdToMotorId[index], calculated_speed);
        }
    }
 return OK;
}
float _speed;
uint32_t ThreadControlSpeedReadFunction(uint32_t IfIndex, uint32_t ReadValue)
{
    int index;
    if (IfIndex>>8 != IfTypeThread)
    {
        ReportWithPackageFilter(ThreadFilter,"Wrong  Interface type",__FILE__,__LINE__,(int)IfIndex,RpError,(int)PoolerTotalProcessedLength,0);
        return 0xFFFFFFFF;
    }
    index = IfIndex&0xFF;

    if(MotorControlConfig[index].m_isEnabled )
    {
        int MotorId = ThreadMotorIdToMotorId[index];
        _speed = MotorGetSpeedFromFPGA_Res ((TimerMotors_t)MotorId);
    }
    return OK;
}
uint16_t BreakSensorCounter = 0;
uint16_t BreakSensorLatchCounter = 0;
char TMessage[150];
#ifdef FOUR_WINDERS
char ATMessage[MAX_THREAD_MOTORS_NUM][150];
int c = 0;
#endif
uint32_t checkBreakSensor(uint32_t index)
{
    if (BreakSensorenabled == true)
    {
        if (JobCounter > eOneSecond)
        {
            if (ReadBreakSensor()==ERROR)
            {
                BreakSensorCounter++;
                BreakSensorLatchCounter++;
                if (BreakSensorCounter>=BreakSensordebouncetimemilli)
                {
                    //consider applying the debouce parameters later
                    usnprintf(TMessage, 60, "ReadBreakSensor Error");
                    //BreakSensordebouncetimemilli
                    JobEndReason = JOB_THREAD_BREAK;
                    ThreadControlActive = false;
                    SendJobProgress(0.0,0,false, TMessage);
                    SendSegmentFail();
                    //AlarmHandlingSetAlarm(EVENT_TYPE__THREAD_BREAK,true);
                    //EndState(CurrentJob,"ReadBreakSensor Error" );
                    ReportWithPackageFilter(ThreadFilter,"ReadBreakSensor Error",__FILE__,BreakSensorCounter,(int)index,RpError,(int)JobCounter,0);
                    return ERROR;
                } //passed limit
            }//ReadBreakSensor()==ERROR
            else //reset counter - we are looking for consequent calls
            {
                if (BreakSensorCounter)
                {
                    ReportWithPackageFilter(ThreadFilter,"ReadBreakSensor Spike",__FILE__,BreakSensorCounter,(int)index,RpError,(int)JobCounter,0);
                }
                BreakSensorCounter = 0;
            }
            if (CurrentControlledSpeed[index] < (OriginalMotorSpd_2PPS[index]/3))
            {
                BreakSensorCounter++;
                BreakSensorLatchCounter++;
                if (BreakSensorCounter>=BreakSensordebouncetimemilli)
                {
                    //consider applying the debouce parameters later
                    usnprintf(TMessage, 60, "thread speed too low");
                    JobEndReason = JOB_THREAD_BREAK;
                    ThreadControlActive = false;
                    SendJobProgress(0.0,0,false, TMessage);
                    SendSegmentFail();
                    //AlarmHandlingSetAlarm(EVENT_TYPE__THREAD_BREAK,true);
                    //EndState(CurrentJob,"ReadBreakSensor Error" );
                    ReportWithPackageFilter(ThreadFilter,"thread speed too low Error",__FILE__,BreakSensorCounter,(int)index,RpError,(int)JobCounter,0);
                    return ERROR;
                }
            }

        }
    }
    return OK;

}
int controlIndex = 0;
bool keepdata = true;
int32_t KeepReadValue = 0;
//double eNormalizedError[100];
//int    TranslatedreadValue[100];
#ifdef TEST_PID_THREAD
#define MAX_THREAD_CONTROL_LOG 100
double calculatedError[MAX_THREAD_CONTROL_LOG+1];
double NormError[MAX_THREAD_CONTROL_LOG+1];
double mIntegral[MAX_THREAD_CONTROL_LOG+1];
int    MotorId[MAX_THREAD_CONTROL_LOG+1];
int    readValue[MAX_THREAD_CONTROL_LOG+1];
int    AveragereadValue[MAX_THREAD_CONTROL_LOG+1];
int    calculatedspeed[MAX_THREAD_CONTROL_LOG+1];
int    timestamp[MAX_THREAD_CONTROL_LOG+1];

void testDancersControl()
{
    int mm20,mm10,mm5,mm2,mm1;
    mm20 = (20*DancerStopActivityLimit[FEEDER_MOTOR])/(DancersCfg[HARDWARE_DANCER_TYPE__RightDancer].maximalmovementmm*2);
    mm2 = mm20/10;
    mm5 = mm20/4;
    mm10 = mm20/2;
    mm1 = mm20/20;
    char time[150];
    int len;
    ThreadControlActive = true;
    SetOriginMotorSpeed(50.0);
    MotorControlConfig[FEEDER_MOTOR].m_params.epsilon = 0;
    MotorsControl[FEEDER_MOTOR].controloutputtype = 0.001;
    MotorControlConfig[FEEDER_MOTOR].m_params.dt = 0.001;
    DancersCfg[HARDWARE_DANCER_TYPE__RightDancer].zeropoint = 10000;
    MotorsControl[FEEDER_MOTOR].pvinputfilterfactormode = 1;
    len = usnprintf(time, 150, "params: speed 50, divider %d p %d * %d i %d * %d Dt*1000 %d Norm Coef %d initial speed %d",NORMAL_COEF_DIVIDER,(int)MotorsControl[FEEDER_MOTOR].proportionalgain,(int)MotorsControl[FEEDER_MOTOR].outputonoffhysteresisvalue,
                    (int)MotorsControl[FEEDER_MOTOR].integraltime,(int)MotorsControl[FEEDER_MOTOR].setpointramprateorsoftstartramp,(int)(MotorsControl[FEEDER_MOTOR].controloutputtype*1000),
                    (int)(NormalizedErrorCoEfficient[FEEDER_MOTOR]*1000000000),OriginalMotorSpd_2PPS[FEEDER_MOTOR]);
    ReportWithPackageFilter(ThreadFilter,time,__FILE__,__LINE__,111,RpError,111,0);
    Task_sleep(100);
    ThreadControlCBFunction(IfTypeThread*0x100+FEEDER_MOTOR, DancersCfg[HARDWARE_DANCER_TYPE__RightDancer].zeropoint - mm20);
    ThreadControlCBFunction(IfTypeThread*0x100+FEEDER_MOTOR, DancersCfg[HARDWARE_DANCER_TYPE__RightDancer].zeropoint - mm10);
    ThreadControlCBFunction(IfTypeThread*0x100+FEEDER_MOTOR, DancersCfg[HARDWARE_DANCER_TYPE__RightDancer].zeropoint - mm5);
    ThreadControlCBFunction(IfTypeThread*0x100+FEEDER_MOTOR, DancersCfg[HARDWARE_DANCER_TYPE__RightDancer].zeropoint - mm2);
    ThreadControlCBFunction(IfTypeThread*0x100+FEEDER_MOTOR, DancersCfg[HARDWARE_DANCER_TYPE__RightDancer].zeropoint - mm1);
    ThreadControlCBFunction(IfTypeThread*0x100+FEEDER_MOTOR, DancersCfg[HARDWARE_DANCER_TYPE__RightDancer].zeropoint);
    ThreadControlCBFunction(IfTypeThread*0x100+FEEDER_MOTOR, DancersCfg[HARDWARE_DANCER_TYPE__RightDancer].zeropoint + mm1);
    ThreadControlCBFunction(IfTypeThread*0x100+FEEDER_MOTOR, DancersCfg[HARDWARE_DANCER_TYPE__RightDancer].zeropoint + mm2);
    ThreadControlCBFunction(IfTypeThread*0x100+FEEDER_MOTOR, DancersCfg[HARDWARE_DANCER_TYPE__RightDancer].zeropoint + mm5);
    ThreadControlCBFunction(IfTypeThread*0x100+FEEDER_MOTOR, DancersCfg[HARDWARE_DANCER_TYPE__RightDancer].zeropoint + mm10);
    ThreadControlCBFunction(IfTypeThread*0x100+FEEDER_MOTOR, DancersCfg[HARDWARE_DANCER_TYPE__RightDancer].zeropoint + mm20);
    ThreadControlActive = false;
}
#endif
int MotorFailedSample[MAX_THREAD_MOTORS_NUM] = {0,0,0,0,0};
//char time[150];
bool FirstCalcInJob = true;
uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
{
//#define MAX_CONTROL_SAMPLES 6
//extern uint32_t MotorSamples[MAX_THREAD_MOTORS_NUM][MAX_CONTROL_SAMPLES];
//extern int MotorSamplePointer[MAX_THREAD_MOTORS_NUM];
    //read value is the dancer angle
    int i,index=MAX_THREAD_MOTORS_NUM;
    int len;
    int DancerId;
    int32_t TranslatedReadValue, avreageSampleValue = 0;//,avreageMotorSampleValue = 0;
    //double tempcalcspeed = 0;
    double calculated_speed;
    double NormalizedError;

    if (ThreadControlActive == false)
        return OK;
#ifndef TEST_PID_THREAD
    if (PrepareState == true)
        return OK;
#endif
    if (IfIndex>>8 != IfTypeThread)
    {
        ReportWithPackageFilter(ThreadFilter,"Wrong  Interface type",__FILE__,__LINE__,(int)IfIndex,RpError,(int)PoolerTotalProcessedLength,0);
        return 0xFFFFFFFF;
    }
    index = IfIndex&0xFF;

    if (index == POOLER_MOTOR) //move break sensor handling up to ensure handling even if tiing control is > 1 msec
    {
        if (checkBreakSensor(index) == ERROR)
            return OK;
    }
    if (MotorTiming[index]>1)
    {
        MotorTimer[index]++;
        if (MotorTimer[index]>=MotorTiming[index])
        {
            MotorTimer[index]=0;
        }
        else
        {
            return OK;
        }
    }
    if(MotorControlConfig[index].m_isEnabled )
    {
        //if (MotorDriverResponse[ThreadMotorIdToMotorId[index]].Busy == true)
        //    return OK;
        DancerId = ThreadMotorIdToDancerId[index];
/*        if (ReadValue < 10)
        {
            MotorFailedSample[index]++;
            ReportWithPackageFilter(ThreadFilter,"Dancer value read too small.",__FILE__,__LINE__,DancerId,RpError,ReadValue,0);
            return OK;
        }*/
        if (ReadValue == 0x3FFF)
        {
            if (Read_Dryer_Status(DancerId) != OK)
            {
                ReportWithPackageFilter(ThreadFilter,"Dancer value invalid.",__FILE__,ReadValue,(int)DancerId,RpError,(int)Read_Dryer_Status(DancerId),0);
                MotorFailedSample[index]++;
                return OK;
            }
        }
        KeepReadValue = ReadValue;
        TranslatedReadValue = ReadValue - DancersCfg[DancerId].zeropoint;
        if (abs(TranslatedReadValue) > 0x2000)
        {
            TranslatedReadValue = 0x3FFF- TranslatedReadValue; //overcome zero environment
        }
        if ((index == POOLER_MOTOR)||((index == FEEDER_MOTOR)&&(DancersCfg[DancerId].assemblydirectionright == true)))
        {
			//pooler dancer is right sided: data is opposite
            TranslatedReadValue = (-1*TranslatedReadValue);
        }
#ifdef FOUR_WINDERS
        if (index == WINDER_MOTOR)
        {
            c++;
        }
        if (index == WINDER_2_MOTOR)
        {
            c++;
        }
        if (index == WINDER_3_MOTOR)
        {
            c++;
        }
        if (index == WINDER_4_MOTOR)
        {
            c++;
        }
        if ((index == WINDER_2_MOTOR)||(index == WINDER_3_MOTOR))
        {
            //pooler dancer is right sided: data is opposite
            TranslatedReadValue = (-1*TranslatedReadValue);
        }
#endif
        if (index == POOLER_MOTOR)
        {
            //pooler dancer is right sided: data is opposite
            JobCounter++;
        }
        //TranslatedReadValue = 0;//test
        MotorSamples[index][MotorSamplePointer[index]] = TranslatedReadValue;//(-1 * TranslatedReadValue);
        MotorSamplePointer[index]++;
        if (MotorSamplePointer[index] >= (int)MotorsControl[index].pvinputfilterfactormode)
            MotorSamplePointer[index] = 0;
#ifdef TEST_LONGER_PID_THREAD
        else // test: handle tension once in pvinputfilterfactormode milliseconds
            return OK;
#endif
        for (i=0;i<(int)MotorsControl[index].pvinputfilterfactormode;i++)
            avreageSampleValue += MotorSamples[index][i];
        avreageSampleValue = avreageSampleValue / (int)MotorsControl[index].pvinputfilterfactormode;


        //Stop Execution if the dancer moves too much
        if ((abs(avreageSampleValue)> DancerStopActivityLimit[index])&&(JobCounter > eOneSecond))
        {
            keepdata = false;
            usnprintf(TMessage, 60, "Dancer %d limit %d value %d Zero %d",DancerId,DancerStopActivityLimit[index],avreageSampleValue,DancersCfg[DancerId].zeropoint);
            ReportWithPackageFilter(ThreadFilter,TMessage,__FILE__,__LINE__,avreageSampleValue,RpWarning,DancerStopActivityLimit[index],0);
            //JobAbortedByUser = true;
            ThreadControlActive = false;
            //MotorGetStatusFromFPGA(ThreadMotorIdToMotorId[index]);
            JobEndReason = JOB_WINDER_DANCER_FAIL+DancerId;
#ifdef FOUR_WINDERS
            if (DancerId>HARDWARE_DANCER_2)
                JobEndReason = JOB_WINDER_DANCER_FAIL+HARDWARE_DANCER_0;
#endif
            SendJobProgress(0.0,0,false, TMessage);
            //EndState(CurrentJob,TMessage );
            SendSegmentFail();
            /*switch (index)
            {
                case POOLER_MOTOR:
                    AlarmHandlingSetAlarm(EVENT_TYPE__THREAD_TENSION_CONTROL_FAILURE_PULLER_DANCER,true);
                    break;
                case FEEDER_MOTOR:
                    AlarmHandlingSetAlarm(EVENT_TYPE__THREAD_TENSION_CONTROL_FAILURE_FEEDER_DANCER,true);
                    break;
                case WINDER_MOTOR:
                    AlarmHandlingSetAlarm(EVENT_TYPE__THREAD_TENSION_CONTROL_FAILURE_WINDER_DANCER,true);
                    break;
            }*/
            ReportWithPackageFilter(ThreadFilter,"Dancer Failure",__FILE__,DancerId,(int)avreageSampleValue,RpError,(int)JobCounter,0);
            return OK;
        }
        NormalizedError = avreageSampleValue*NormalizedErrorCoEfficient[index];
        if ((index != FEEDER_MOTOR)||(MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].speedmaster == false)) //feeder unit handles errors opposite to left unit
        {
            NormalizedError = (-1*NormalizedError);
        }

        MotorControlConfig[index].m_mesuredParam = NormalizedError;
        DancerError[DancerId] = NormalizedError;
        MotorControlConfig[index].m_calculatedError = PIDAlgorithmCalculation((float)MotorControlConfig[index].m_SetParam , (float)MotorControlConfig[index].m_mesuredParam,
                                                                              &MotorControlConfig[index].m_params,   &MotorControlConfig[index].m_preError, &MotorControlConfig[index].m_integral);
        /*else
        {
            //KeepNormalizedError = NormalizedError;
        }*/
        /*if ((JobCounter % 100) == 0)
        {
            //if (index == WINDER_MOTOR) //feeder unit handles errors opposite to left unit
            //{
              //  ReportWithPackageFilter(ThreadFilter,"MotorSpeedUpdated",__FILE__,index,OriginalMotorSpd_2PPS[index],RpWarning,CurrentControlledSpeed[index],0);
            //}
            /`*if (JobCounter >= 3000)
            {
                MotorSpeedSamples[index][MotorSpeedSamplePointer[index]] = CurrentControlledSpeed[index];//(-1 * TranslatedReadValue);
                MotorSpeedSamplePointer[index]++;
                if (MotorSpeedSamplePointer[index] >= MAX_CONTROL_SAMPLES)
                    MotorSpeedSamplePointer[index] = 0;
                for (i=0;i<MAX_CONTROL_SAMPLES;i++)
                    avreageMotorSampleValue += MotorSpeedSamples[index][i];
                avreageMotorSampleValue = avreageMotorSampleValue / MAX_CONTROL_SAMPLES;
                //ReportWithPackageFilter(ThreadFilter,"MotorSpeedUpdated",__FILE__,index,OriginalMotorSpd_2PPS[index],RpWarning,avreageMotorSampleValue,0);
                OriginalMotorSpd_2PPS[index] = avreageMotorSampleValue;
            }*`/
        }*/
        calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*OriginalMotorSpd_2PPS[index];
        if (index < WINDER_MOTOR)
            calculated_speed = calculated_speed*InitialDryerSpeed/OriginalMotorSpd_2PPS[DRYER_MOTOR];
        //calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*CurrentControlledSpeed[index];
        //if (0)//(JobCounter % 1000 == 0)
#ifdef FOUR_WINDERS
        if (JobCounter % 500 < 4)//(FirstCalcInJob == true)
        {
            if (index >= WINDER_MOTOR)
            {
           //     FirstCalcInJob = false;
                len = usnprintf(ATMessage[index], 150, "index %d read %d avg %d error(6) %d integral(9) %d,delta(9) %d, calc(3) %d speed %d %d",index,
                                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, (int)(InitialDryerSpeed*100/OriginalMotorSpd_2PPS[DRYER_MOTOR]));
                ReportWithPackageFilter(ThreadFilter,ATMessage[index],__FILE__,MotorSamplePointer[index],JobCounter,RpError,ReadValue,0);

            }
            JobCounter++;
        }
#endif
#ifndef TEST_PID_THREAD
        if (fabs(calculated_speed-CurrentControlledSpeed[index])> MotorControlConfig[index].m_ingnoreValue)
#else
        if (index == FEEDER_MOTOR) //feeder unit handles errors opposite to left unit
#endif
        {
            if (calculated_speed>5.0)
            {
                /*if (calculated_speed>(CurrentControlledSpeed[index]+100))
                {
                        ReportWithPackageFilter(ThreadFilter,"limit acceleration",__FILE__,calculated_speed,CurrentControlledSpeed[index],RpError,index,0);
                        calculated_speed=CurrentControlledSpeed[index]+100;
                }*/
                CurrentControlledSpeed[index] = calculated_speed;
                MotorSetSpeed(ThreadMotorIdToMotorId[index], calculated_speed);
            }
            else
            {
                if (calculated_speed<0)
                ReportWithPackageFilter(ThreadFilter,"Negative speed",__FILE__,index,(int)OriginalMotorSpd_2PPS[index],RpWarning,(int)CurrentControlledSpeed[index],0);
            }

            /*if (((JobCounter % 2000) == index*100)&&(index == WINDER_MOTOR)) //feeder unit handles errors opposite to left unit
            {
                ReportWithPackageFilter(ThreadFilter,"MotorSpeedUpdated",__FILE__,index,(int)OriginalMotorSpd_2PPS[index],RpWarning,(int)CurrentControlledSpeed[index],0);
            }*/
#ifdef TEST_PID_THREAD
            int len;
            if ((JobCounter % 2000) == index*100)
             //if (keepdata == true)
            {
                /*calculatedError[controlIndex] = MotorControlConfig[index].m_calculatedError;
                MotorId[controlIndex] = index;
                readValue[controlIndex] = ReadValue;
                AveragereadValue[controlIndex] = avreageSampleValue;
                calculatedspeed[controlIndex] = calculated_speed;
                NormError[controlIndex] = MotorControlConfig[index].m_mesuredParam;
                mIntegral[controlIndex] = MotorControlConfig[index].m_integral;
                timestamp[controlIndex] = msec_millisecondCounter;*/
                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),
                                (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);
                //Task_sleep(100);
                //if (controlIndex++>=MAX_THREAD_CONTROL_LOG)
                //    controlIndex = 0;
            }
#endif
        }
		else
		{
           MotorFailedSample[index]++;
           //LOG_ERROR(index,"No change in speed");
		}

    }

 return OK;
}

//********************************************************************************************************************
double ThreadGetMotorSpeed(threadMotorsEnum MotorId)
{
    return  CurrentControlledSpeed[MotorId];
}
//********************************************************************************************************************
double ThreadGetMotorCalculatedError(int DancerId)
{
    switch (DancerId)
    {
        case FEEDER_DANCER:
            return  (double)MotorControlConfig[FEEDER_MOTOR].m_calculatedError;
        case POOLER_DANCER:
            return  (double)MotorControlConfig[POOLER_MOTOR].m_calculatedError;
        case WINDER_DANCER:
            return  (double)MotorControlConfig[WINDER_MOTOR].m_calculatedError;

    }
    return 0;
}

//********************************************************************************************************************
uint32_t HandleJobThreadControlParameters(ThreadParameters* ThreadParams)
{
    if (ThreadParams == NULL)
    {
        return OK;
    }
    if((ThreadParams->bypassrockers != true)&&(ThreadParams->bypassrockers != false))
    {
        ReportWithPackageFilter(ThreadFilter,"incorrect Thread parameters ",__FILE__,__LINE__,(int)ThreadParams->feederp,RpWarning,(int)ThreadParams->bypassrockers,0);
        return OK;
    }
    if ((ThreadParams->feederp>100000)||(ThreadParams->feederi>100000))
    {
        ReportWithPackageFilter(ThreadFilter,"incorrect Thread parameters ",__FILE__,__LINE__,(int)ThreadParams->feederp,RpWarning,(int)ThreadParams->bypassrockers,0);
        return OK;
    }
    if(ThreadParams->feederp)
        MotorControlConfig[FEEDER_MOTOR].m_params.Kp = ThreadParams->feederp;
    if(ThreadParams->feederi)
        MotorControlConfig[FEEDER_MOTOR].m_params.Ki = ThreadParams->feederi;
    if(ThreadParams->feederd)
        MotorControlConfig[FEEDER_MOTOR].m_params.Kd = ThreadParams->feederd;

    if(ThreadParams->pullerp)
        MotorControlConfig[POOLER_MOTOR].m_params.Kp = ThreadParams->pullerp;
    if(ThreadParams->pulleri)
        MotorControlConfig[POOLER_MOTOR].m_params.Ki = ThreadParams->pulleri;
    if(ThreadParams->pullerd)
        MotorControlConfig[POOLER_MOTOR].m_params.Kd = ThreadParams->pullerd;

    if(ThreadParams->winderp)
    {
        MotorControlConfig[WINDER_MOTOR].m_params.Kp = ThreadParams->winderp;
#ifdef FOUR_WINDERS
        MotorControlConfig[WINDER_2_MOTOR].m_params.Kp = ThreadParams->winderp;
        MotorControlConfig[WINDER_3_MOTOR].m_params.Kp = ThreadParams->winderp;
        MotorControlConfig[WINDER_4_MOTOR].m_params.Kp = ThreadParams->winderp;
#endif
    }
    if(ThreadParams->winderi)
    {
        MotorControlConfig[WINDER_MOTOR].m_params.Ki = ThreadParams->winderi;
#ifdef FOUR_WINDERS
        MotorControlConfig[WINDER_2_MOTOR].m_params.Ki = ThreadParams->winderi;
        MotorControlConfig[WINDER_3_MOTOR].m_params.Ki = ThreadParams->winderi;
        MotorControlConfig[WINDER_4_MOTOR].m_params.Ki = ThreadParams->winderi;
#endif
    }
    if(ThreadParams->winderd)
    {
        MotorControlConfig[WINDER_MOTOR].m_params.Kd = ThreadParams->winderd;
#ifdef FOUR_WINDERS
        MotorControlConfig[WINDER_2_MOTOR].m_params.Kd = ThreadParams->winderd;
        MotorControlConfig[WINDER_3_MOTOR].m_params.Kd = ThreadParams->winderd;
        MotorControlConfig[WINDER_4_MOTOR].m_params.Kd = ThreadParams->winderd;
#endif
    }
    ReportWithPackageFilter(ThreadFilter,"Rockers activity",__FILE__,__LINE__,(int)Thread_Rockers_Bypass,RpWarning,(int)ThreadParams->bypassrockers,0);
    if(ThreadParams->bypassrockers)
        Thread_Rockers_Bypass = true;

    return OK;
}
bool RTFU_Up = false;

uint32_t Release_Right_TFU_TensionCallback(uint32_t deviceID, uint32_t BusyFlag)
{
    //MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_SCREW,temp_MaxFrequency);
    Report("Release_Right_TFU_TensionCallback",__FILE__,deviceID,0,RpMessage,0,0);

    MotorStop (HARDWARE_MOTOR_TYPE__MOTO_RDANCER,Soft_Hiz); //per L6470 errata between mov and run commands
    return OK;
}
uint32_t Release_Right_TFU_Tension()
{
    uint32_t status = OK;
#ifndef FOUR_WINDERS
    if (RTFU_Up == true)
    {
        Report("Release_Right_TFU_Tension",__FILE__,__LINE__,HARDWARE_MOTOR_TYPE__MOTO_RDANCER,RpMessage,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].pulseperround/4,0);
        RTFU_Up = false;
        status = MotorMoveWithCallback(HARDWARE_MOTOR_TYPE__MOTO_RDANCER, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].directionthreadwize, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].pulseperround/4* MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].microstep, Release_Right_TFU_TensionCallback,1000);
    }
#endif
    return status;
}
int SecondFeederCorrection = 4;
int PrepareWaitCount = 0;
uint32_t Adjust_Right_TFU_Tension_2nd_Callback(uint32_t MotorId, uint32_t ReadValue)
{
#ifndef FOUR_WINDERS
    MotorStop (HARDWARE_MOTOR_TYPE__MOTO_RDANCER,Soft_Stop); //per L6470 errata between mov and run commands
    Report("Adjust_Right_TFU_Tension_2ndCallback x more steps",__FILE__,__LINE__,MotorId,RpMessage,SecondFeederCorrection,0);
    if (JobIsActive()==false)
    {
        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);
    }
#endif
    return OK;
}
uint32_t Adjust_Right_TFU_Tension_Callback(uint32_t MotorId, uint32_t ReadValue)
{
#ifndef FOUR_WINDERS
    Report("Adjust_Right_TFU_Tension_Callback",__FILE__,__LINE__,MotorId,RpMessage,0,0);
    MotorMoveWithCallback(HARDWARE_MOTOR_TYPE__MOTO_RDANCER, 1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].directionthreadwize,SecondFeederCorrection* MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].microstep, Adjust_Right_TFU_Tension_2nd_Callback,1000);
    RTFU_Up = true;
#endif
    return OK;
}

uint32_t Adjust_Right_TFU_Tension(double tension)
{
    uint32_t status = OK;
#ifndef FOUR_WINDERS
    if (tension > 0.5) //0 = lower position, 1 = high position
    {
        if (FPGA_Read_limit_Switches(GPI_LS_RDANCER_UP) == NO_LIMIT)
        {
            PrepareWaitCount++;
            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,PrepareWaitCount,0);
        }
    }
#endif
    return status;
}
uint32_t ThreadPrepare_TensionCallback (int MotorId, double tension)
{
    //MotorStop(MotorId,Hard_Hiz);
    ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_TensionCallback",__FILE__,__LINE__,MotorId,RpWarning,PrepareWaitCount,0);
    if (PrepareWaitCount)
    {
        PrepareWaitCount--;
    }
    if ((PrepareWaitCount == 0)&&(PrepareState == true))
    {
        PrepareState = false;
        ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_TensionCallback  Prepare Ready",__FILE__,__LINE__,MotorId,RpWarning,PrepareWaitCount,0);
        PrepareReady(Module_Thread,ModuleDone);
    }
    return OK;
}
uint32_t ThreadPrepare_Tension (int DancerId, double tension)
{
    int current, request = (int)tension,movement;
    int HW_Motor_Id;
    bool direction;
    uint32_t status = OK, address = 0;
    switch (DancerId)
    {
    case HARDWARE_DANCER_TYPE__LeftDancer:
        address = EEPROM_WINDER_TENSION_POSITION;
        HW_Motor_Id = HARDWARE_MOTOR_TYPE__MOTO_LDANCER1;
        break;
    case HARDWARE_DANCER_TYPE__MiddleDancer:
        if (Is_PP_Machine() == false) //LP machine - old LTFU
            return OK;
        address = EEPROM_PULLER_TENSION_POSITION;
        HW_Motor_Id = HARDWARE_MOTOR_TYPE__MOTO_LDANCER2;
        break;
    case HARDWARE_DANCER_TYPE__RightDancer:
        return Adjust_Right_TFU_Tension(tension);
        //break;
    default:
        return ERROR;
    }
    if (tension < 100)
        return OK; //do not handle tension of zero
    status |= MCU_E2PromRead(address,&current);
    if ((status!= OK )||(current == 0xFFFF))
        return status;
    if (abs(current - request)<100)
        return status;
    else
    {
        if (current < request) //go down
        {
            direction = MotorsCfg[HW_Motor_Id].directionthreadwize;
            movement = request - current;
        }
        else
        {
            direction = 1-MotorsCfg[HW_Motor_Id].directionthreadwize;
            movement = current - request;
        }
        MotorSetMaxSpeed (HW_Motor_Id, 800);
        MotorMoveWithCallback (HW_Motor_Id, direction, (movement*MotorsCfg[HW_Motor_Id].microstep), ThreadPrepare_TensionCallback,20000);
        PrepareWaitCount++;
        ReportWithPackageFilter(ThreadFilter,"PrepareWaitCount",__FILE__,PrepareWaitCount,current,RpWarning,request,0);
        status |= MCU_E2PromProgram(address,request);
    }
    if (DancerId == HARDWARE_DANCER_TYPE__LeftDancer)
    {
        usnprintf(Lenstr, 100, "ThreadPrepare_Tension Dancer %d Request: %d Current %d movement %d dir %d motor %d address %d call %d",
                  DancerId,request,current,movement,direction,HW_Motor_Id,address,PrepareWaitCount);
        ReportWithPackageFilter(ThreadFilter,Lenstr,__FILE__,address,current,RpWarning,request,0);

    }
    else
    {
        usnprintf(TMessage, 100, "ThreadPrepare_Tension Dancer %d Request: %d Current %d movement %d dir %d motor %d address %d call %d",
                  DancerId,request,current,movement,direction,HW_Motor_Id,address,PrepareWaitCount);
        ReportWithPackageFilter(ThreadFilter,TMessage,__FILE__,address,current,RpWarning,request,0);

    }
    return status;
}
//********************************************************************************************************************
bool SkipOpenLids = true;
uint32_t ThreadPrepareState(void *JobDetails)
{
    int Motor_i,i, HW_Motor_Id, Pid_Id;
    JobTicket* JobTicket = JobDetails;
    uint32_t status = OK;
    CurrentSegmentId = 0;
    float temp_dt = 0;
    JobCounter = 0;
    TotalProcessedLength = 0.0;
    PoolerTotalProcessedLength = 0.0;
    InitialProcess = true;
    initialpos = 0xFFFF;
    Poolerinitialpos = 0xFFFF;
    PrepareState = true;
    AlarmHandlingSetAlarm(EVENT_TYPE__THREAD_BREAK,false);
    AlarmHandlingSetAlarm(EVENT_TYPE__THREAD_TENSION_CONTROL_FAILURE_PULLER_DANCER,false);
    AlarmHandlingSetAlarm(EVENT_TYPE__THREAD_TENSION_CONTROL_FAILURE_FEEDER_DANCER,false);
    AlarmHandlingSetAlarm(EVENT_TYPE__THREAD_TENSION_CONTROL_FAILURE_WINDER_DANCER,false);
    AlarmHandlingSetAlarm(EVENT_TYPE__WINDER_CONE_DOES_NOT_EXIST,false);
    AlarmHandlingSetAlarm(EVENT_TYPE__FPGA_WATCHDOG_ACTIVATED,false);

//    status |= MCU_E2PromProgram(EEPROM_STORAGE_DANCER_0,DancersCfg[0].zeropoint);
//      double feedertension = 0;
//    double pullertension = 0;
//    double windertension = 0;

    EnableLubrication = JobTicket->enablelubrication;
    EnableIntersegment = JobTicket->enableintersegment;
    IntersegmentLength = JobTicket->intersegmentlength;

    PrepareWaitCount = 0;
#ifndef FOUR_WINDERS
    status = ThreadPrepare_Tension (HARDWARE_DANCER_TYPE__LeftDancer, windertension);
    ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_Tension Winder",__FILE__,HARDWARE_DANCER_TYPE__LeftDancer,PrepareWaitCount,RpWarning,(int)windertension,0);
    status = ThreadPrepare_Tension (HARDWARE_DANCER_TYPE__MiddleDancer, pullertension);
    ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_Tension Puller",__FILE__,HARDWARE_DANCER_TYPE__MiddleDancer,PrepareWaitCount,RpWarning,(int)pullertension,0);
    status = ThreadPrepare_Tension (HARDWARE_DANCER_TYPE__RightDancer, feedertension);
    ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_Tension Feeder",__FILE__,HARDWARE_DANCER_TYPE__RightDancer,PrepareWaitCount,RpWarning,(int)feedertension,0);
#endif
    FirstCalcInJob = true;
    if(MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].speedmaster == false)
    {
        ThreadMotorIdToMotorId[FEEDER_MOTOR] = HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING;
        ThreadMotorIdToMotorId[DRYER_MOTOR] = HARDWARE_MOTOR_TYPE__MOTO_RDRIVING;
    }
    else
    {
        ThreadMotorIdToMotorId[FEEDER_MOTOR] = HARDWARE_MOTOR_TYPE__MOTO_RDRIVING ;
        ThreadMotorIdToMotorId[DRYER_MOTOR] = HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING;
    }
    MotorStop(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, Hard_Stop);

    /*if (FPGA_Read_limit_Switches(Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_DH_LID]) != LIMIT)
    {
        ReportWithPackageFilter(ThreadFilter,"Dyeing head is open!!!",__FILE__,__LINE__,HARDWARE_MOTOR_TYPE__MOTO_DH_LID,RpWarning,LIMIT,0);
        //JobEndReason = JOB_LIDS_OPEN;
        //PrepareReady(Module_Thread,ModuleFail);
        //return ERROR;
    }
    if (FPGA_Read_limit_Switches(Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID]) != LIMIT)
    {
        ReportWithPackageFilter(ThreadFilter,"Dryer lid is open!!!",__FILE__,__LINE__,HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID,RpWarning,LIMIT,0);
        //JobEndReason = JOB_LIDS_OPEN;
        //PrepareReady(Module_Thread,ModuleFail);
        //return ERROR;
    }*/

//    if ((FPGA_Read_limit_Switches(Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_DH_LID]) == LIMIT)&&(JoggingJobActive == false))
    if ((FPGA_Read_limit_Switches(Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_DH_LID]) != LIMIT)&&(JoggingJobActive == false))
    {
        if(Head_Type != HEAD_TYPE_ARC)
        {
            ReportWithPackageFilter(ThreadFilter,"Dyeing head is wide open!!!",__FILE__,__LINE__,HARDWARE_MOTOR_TYPE__MOTO_DH_LID,RpError,LIMIT,0);
            if (SkipOpenLids == false)
            {
                JobEndReason = JOB_LIDS_OPEN;
                PrepareReady(Module_Thread,ModuleFail);
                return ERROR;
            }
        }
    }
//    if ((FPGA_Read_limit_Switches(Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID]) == LIMIT)&&(JoggingJobActive == false))
    if ((FPGA_Read_limit_Switches(Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID]) != LIMIT)&&(JoggingJobActive == false))
    {
        ReportWithPackageFilter(ThreadFilter,"Dryer lid is wide open!!!",__FILE__,__LINE__,HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID,RpError,LIMIT,0);
        if (SkipOpenLids == false)
        {
            JobEndReason = JOB_LIDS_OPEN;
            PrepareReady(Module_Thread,ModuleFail);
            return ERROR;
        }
    }

    //start thread control for all motors
    for (Motor_i = 0;Motor_i < MAX_THREAD_MOTORS_NUM;Motor_i++)
    {
        HW_Motor_Id = ThreadMotorIdToMotorId[Motor_i];
        Pid_Id = Motor_i;/*ThreadMotorIdToControlId[Motor_i];*/
#ifdef FOUR_WINDERS
        if (Motor_i == WINDER_2_MOTOR) Pid_Id = WINDER_MOTOR;
        if (Motor_i == WINDER_3_MOTOR) Pid_Id = WINDER_MOTOR;
        if (Motor_i == WINDER_4_MOTOR) Pid_Id = WINDER_MOTOR;
#endif
        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!!!

        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;
                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);

#ifndef FOUR_WINDERS
        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)
            {
                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);
        }
#endif
        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)
            {
                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);
        }
#ifndef FOUR_WINDERS
            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)
                {
                    if(RemoveControlCallback(ControlIdtoMotorId[Motor_i],ThreadControlCBFunction)!=OK)
                        ReportWithPackageFilter(ThreadFilter,"Remove Control Failed.",__FILE__,__LINE__,(int)Motor_i,RpError,(int)PoolerSpeedControlId,0);
                    ControlIdtoMotorId[Motor_i] = 0xFF;
                    CurrentControlledSpeed[Motor_i] = 0;
                }
#ifndef TEST_PID_THREAD
                ControlIdtoMotorId[Motor_i] = AddControlCallback(NULL,ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i);
                //AddControlCallback(NULL,ThreadControlSpeedReadFunction, eHundredMillisecond,MotorGetSpeedFromFPGA,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],Motor_i);
#endif
            }
#endif
            if (Motor_i == POOLER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will n//ot be controlled
            {
                if (ControlIdtoMotorId[Motor_i] != 0xFF)
                {
                    if(RemoveControlCallback(ControlIdtoMotorId[Motor_i],ThreadControlCBFunction)!=OK)
                        ReportWithPackageFilter(ThreadFilter,"Remove Control Failed.",__FILE__,__LINE__,(int)Motor_i,RpError,(int)PoolerSpeedControlId,0);
                    CurrentControlledSpeed[Motor_i] = 0;
                    ControlIdtoMotorId[Motor_i] = 0xFF;
                }
#ifndef TEST_PID_THREAD
                ControlIdtoMotorId[Motor_i] = AddControlCallback(NULL,ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i);
#endif
            }
            if (Motor_i == WINDER_MOTOR) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will n//ot be controlled
            {
                ReportWithPackageFilter(ThreadFilter,"Winder Control",__FILE__,Motor_i,MotorControlConfig[Motor_i].m_params.Kp,RpWarning,MotorControlConfig[Motor_i].m_params.Ki,0);
                if (ControlIdtoMotorId[Motor_i] != 0xFF)
                {
                    if(RemoveControlCallback(ControlIdtoMotorId[Motor_i],ThreadControlCBFunction)!=OK)
                        ReportWithPackageFilter(ThreadFilter,"Remove Control Failed.",__FILE__,__LINE__,(int)Motor_i,RpError,(int)PoolerSpeedControlId,0);
                    CurrentControlledSpeed[Motor_i] = 0;
                    ControlIdtoMotorId[Motor_i] = 0xFF;
                }
#ifndef TEST_PID_THREAD
                ControlIdtoMotorId[Motor_i] = AddControlCallback(NULL,ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i);
#endif
            }
#ifdef FOUR_WINDERS
            if ((Motor_i == WINDER_2_MOTOR)||(Motor_i == WINDER_3_MOTOR)||(Motor_i == WINDER_4_MOTOR)) // dryer motor is speed controlled. later a speed sensor will be utilized, but for now it will n//ot be controlled
            {
                ReportWithPackageFilter(ThreadFilter,"Winder 2/3/4 Control",__FILE__,Motor_i,MotorControlConfig[WINDER_MOTOR].m_params.Kp,RpWarning,MotorControlConfig[WINDER_MOTOR].m_params.Ki,0);
                if (ControlIdtoMotorId[Motor_i] != 0xFF)
                {
                    if(RemoveControlCallback(ControlIdtoMotorId[Motor_i],ThreadControlCBFunction)!=OK)
                        ReportWithPackageFilter(ThreadFilter,"Remove Control Failed.",__FILE__,__LINE__,(int)Motor_i,RpError,(int)PoolerSpeedControlId,0);
                    CurrentControlledSpeed[Motor_i] = 0;
                    ControlIdtoMotorId[Motor_i] = 0xFF;
                }
#ifndef TEST_PID_THREAD
                ControlIdtoMotorId[Motor_i] = AddControlCallback(NULL,ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i);
#endif
            }
#endif
    }

#ifdef TEST_PID_THREAD
    testDancersControl();
#endif
    if (PrepareWaitCount == 0)
        PrepareReady(Module_Thread,ModuleDone);
    //set 3 dancers to the profile positions
    return OK;
}
uint32_t UpdatePidDuringRun(HardwarePidControl *request)
{
    int Motor_i = MAX_THREAD_MOTORS_NUM,i;
    double temp_dt;

    for (i=0;i<MAX_THREAD_MOTORS_NUM;i++)
    {
        if (ThreadMotorIdToControlId[i] == request->hardwarepidcontroltype)
        {
            Motor_i = i;
            break;
        }
    }
    if (Motor_i == MAX_THREAD_MOTORS_NUM)
        return ERROR;


    if (request->derivativetime == true)
    {
        MotorControlConfig[Motor_i].m_params.Kd = request->derivativetime;
        ReportWithPackageFilter(ThreadFilter,"UpdatePidDuringRun Kd",__FILE__,Motor_i,(int)(request->derivativetime),RpWarning,0,0);
    }
    if (request->proportionalgain == true)
    {
        MotorControlConfig[Motor_i].m_params.Kp = request->proportionalgain;
        ReportWithPackageFilter(ThreadFilter,"UpdatePidDuringRun Kp",__FILE__,Motor_i,(int)(request->proportionalgain),RpWarning,0,0);
    }
    if (request->integraltime == true)
    {
        MotorControlConfig[Motor_i].m_params.Ki = request->integraltime;
        ReportWithPackageFilter(ThreadFilter,"UpdatePidDuringRun Ki",__FILE__,Motor_i,(int)(request->integraltime),RpWarning,0,0);
    }
    if (request->epsilon == true)
    {
        MotorControlConfig[Motor_i].m_params.epsilon = request->epsilon;
        ReportWithPackageFilter(ThreadFilter,"UpdatePidDuringRun epsilon",__FILE__,Motor_i,(int)(request->epsilon*10000),RpWarning,0,0);
    }
    if (request->has_controloutputtype == true)
    {
        MotorControlConfig[Motor_i].m_params.dt = request->controloutputtype;
        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,"UpdatePidDuringRun dt",__FILE__,Motor_i,(int)(request->controloutputtype*1000),RpWarning,temp_dt,0);
    }
//////////////////////////////////////////////////
    return OK;
}
void SetOriginMotorSpeed(float process_speed)
{
    int Motor_i, HW_Motor_Id;
    for (Motor_i = 0; Motor_i < SCREW_MOTOR; Motor_i++)
    {
        HW_Motor_Id = ThreadMotorIdToMotorId[Motor_i];
        //(Speed*uStep*PPR)/((2*PI*motor_Radius)
        //        double motor_speed = (process_speed *  MotorsCfg[HW_Motor_Id].pulseperround *  MotorsCfg[HW_Motor_Id].microstep)/(2*PI* MotorsCfg[HW_Motor_Id].pulleyradius);
        double motor_speed = (process_speed
                * MotorsCfg[HW_Motor_Id].pulseperround)
                / (2 * PI * MotorsCfg[HW_Motor_Id].pulleyradius);
        //MotorControlConfig[Motor_i].m_SetParam = motor_speed;
        OriginalMotorSpd_2PPS[Motor_i] = (int) motor_speed;
        InitialDryerSpeed = 0.0;
        CurrentControlledSpeed[Motor_i] = (int) motor_speed;
        if (process_speed > 1)
            ReportWithPackageFilter(ThreadFilter,"Original Speed",__FILE__,Motor_i,(int)motor_speed,RpWarning,process_speed,0);

       // for (i = 0; i <= MAX_CONTROL_SAMPLES; i++)
       //     MotorSpeedSamples[Motor_i][i] = motor_speed;
    }
}
void ThreadPreSegmentEnded(void)
{
    InitialProcess = false;
    REPORT_MSG (0,"First ThreadPreSegmentEnded");
    PreSegmentReady(Module_Thread,ModuleDone);
}
int DrierDivider = 20;
uint32_t ThreadDryerRampUp(uint32_t IfIndex, uint32_t BusyFlag)
{
    InitialDryerSpeed += (OriginalMotorSpd_2PPS[DRYER_MOTOR]/DrierDivider);
    if (InitialDryerSpeed >= OriginalMotorSpd_2PPS[DRYER_MOTOR])
    {
        InitialDryerSpeed = OriginalMotorSpd_2PPS[DRYER_MOTOR];
        SafeRemoveControlCallback(ControlIdtoMotorId[DRYER_MOTOR], ThreadDryerRampUp );
        ControlIdtoMotorId[DRYER_MOTOR] = 0xFF;
        //ReportWithPackageFilter(ThreadFilter,"ThreadDryerRampUp end",__FILE__,ControlIdtoMotorId[DRYER_MOTOR],(int)InitialDryerSpeed,RpWarning,(int)OriginalMotorSpd_2PPS[DRYER_MOTOR],0);
    }
    if (InitialDryerSpeed == 0)
    {
        //ReportWithPackageFilter(ThreadFilter,"ThreadDryerRampUp Stopped",__FILE__,ControlIdtoMotorId[DRYER_MOTOR],(int)InitialDryerSpeed,RpWarning,(int)OriginalMotorSpd_2PPS[DRYER_MOTOR],0);
        return OK;
    }
    MotorSetSpeed(ThreadMotorIdToMotorId[DRYER_MOTOR],InitialDryerSpeed );
    //ReportWithPackageFilter(ThreadFilter,"ThreadDryerRampUp",__FILE__,ControlIdtoMotorId[DRYER_MOTOR],(int)InitialDryerSpeed,RpWarning,(int)OriginalMotorSpd_2PPS[DRYER_MOTOR],0);


    return OK;
}
bool Set_Thread_Rockers_Bypass (int value)
{
    if (value == 0)
        Thread_Rockers_Bypass = false;
    else
        Thread_Rockers_Bypass = true;
    return Thread_Rockers_Bypass;
}

//********************************************************************************************************************
uint32_t ThreadPreSegmentState(void *SegmentDetails, uint32_t SegmentId)
{
//set the speed only before the first segment, speed is constant across all job segments and intersegments
    //JobSegment* Segment = SegmentDetails;

    float process_speed = dyeingspeed;
    if (dyeingspeed == 0)
    {
        ReportWithPackageFilter(ThreadFilter,"job speed zero.",__FILE__,__LINE__,(int)dyeingspeed,RpError,(int)SegmentId,0);
        return ERROR;
    }
    ReportWithPackageFilter(ThreadFilter,"ThreadPreSegmentState",__FILE__,__LINE__,(int)dyeingspeed,RpWarning,(int)SegmentId,0);
    if (SegmentId == 0) // do all this only in the beginning of the job. do not touch after that (assuming spool does not change mid job)
    {
        SetOriginMotorSpeed(process_speed);
        ThreadControlActive = true;
        if (PrepareWaitCount == 0)
            PrepareState = false;
        PullerSpeedIndex = 0;
        FeederSpeedIndex = 0;

#ifndef TEST_PID_THREAD
        // set the new speed in the dryer motor to the speed of the new segment
        if(MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].speedmaster == false)
        {
            DrierDivider = dyeingspeed/3;  //ramp up drier in 5 cm/sec steps
        }
        else
        {
            DrierDivider = dyeingspeed/5;  //ramp up drier in 5 cm/sec steps
        }
        ReportWithPackageFilter(ThreadFilter,"Drier ramp up",__FILE__,__LINE__,(int)dyeingspeed,RpWarning,(int)DrierDivider,0);
        InitialDryerSpeed = OriginalMotorSpd_2PPS[DRYER_MOTOR]/DrierDivider;
        MotorSetSpeed(ThreadMotorIdToMotorId[DRYER_MOTOR],InitialDryerSpeed );
        ControlIdtoMotorId[DRYER_MOTOR] = AddControlCallback("DryerRampUp",ThreadDryerRampUp, 200,TemplateDataReadCBFunction,0,0,0);
#endif
#ifdef HUNDRED_MICROSECONDS_DANCER_READ
     MillisecLogInit();
#endif
         if (Thread_Rockers_Bypass == false)
         {
            if (MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].maxfrequency > 0)
            {
                MotorSetDirection((TimerMotors_t)HARDWARE_MOTOR_TYPE__MOTO_RLOADING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].directionthreadwize);
                MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_RLOADING, 1);
            }
            if (MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LLOADING].maxfrequency > 0)
            {
                MotorSetDirection((TimerMotors_t)HARDWARE_MOTOR_TYPE__MOTO_LLOADING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LLOADING].directionthreadwize);
                MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_LLOADING, 1);
            }
         }
        if (EnableLubrication == true)
        {
            IDS_StartLubrication();
        }
    }
    // activate control fr all motors
    //set speed for both rocker motors
    //wait for all motors to get to the required speed (set the target speed for the control to check)
    //call the job state machine when the thread system is ready
    if ((InitialProcess==false) && (EnableIntersegment == true)) //&& (IntersegmentLength >= 1.0)) //fix - avoid intersegment length 0
    {//add initial presegment and cleaning before first segment
        ThreadUpdateProcessLength (IntersegmentLength,(void *)ThreadInterSegmentEnded);
        REPORT_MSG (IntersegmentLength," ThreadPreSegmentState IntersegmentLength");
        SegmentState = false;
        PreSegmentState = true;
        DTSState = false;
    }
    /*else if (InitialProcess==true)
    {
        ThreadUpdateProcessLength (dryerbufferMeters,(void *)ThreadPreSegmentEnded);
        REPORT_MSG (dryerbufferCentimeters," ThreadPreSegmentState DTS length (sample)");
        SegmentState = false;
        PreSegmentState = true;
        DTSState = false;
    }*/
    else
    {
        ThreadUpdateProcessLength (0,(void *)NULL);
        PreSegmentReady(Module_Thread,ModuleDone);
        JobCounter = 0;
        InitialProcess = false;
    }

    return OK;
}
int REPSegmentId = 0;
void SendSegmentFail(void)
{
    if (SegmentState == true)
        SegmentReady(Module_Thread,ModuleFail);
    else if (PreSegmentState == true)
        PreSegmentReady(Module_Thread,ModuleFail);
    else if (DTSState == true)
        DistanceToSpoolReady(Module_Thread,ModuleFail);

}

void ThreadInterSegmentEnded(void)
{
    REPORT_MSG (REPSegmentId,"ThreadInterSegmentEnded");
    //ThreadUpdateProcessLength (0,(void *)NULL);
    PreSegmentReady(Module_Thread,ModuleDone);
}
void ThreadSegmentEnded(void)
{
    REPORT_MSG (REPSegmentId," ThreadSegmentEnded");
    SegmentReady(Module_Thread,ModuleDone);
}
void ThreadDistanceToSpoolEnded(void)
{
    REPORT_MSG (REPSegmentId," ThreadDistanceToSpoolEnded");
    DistanceToSpoolReady(Module_Thread,ModuleDone);
}
double seglength = 0.0;
//********************************************************************************************************************
uint32_t ThreadSegmentState(void *SegmentDetails, int SegmentId)
{
    JobSegment* Segment = SegmentDetails;
    REPSegmentId = SegmentId;
    seglength = Segment->length;
    CurrentSegmentId = SegmentId;
    REPORT_MSG (seglength," ThreadSegmentState");
    ThreadUpdateProcessLength (seglength,(void *)ThreadSegmentEnded);
    SegmentState = true;
    PreSegmentState = false;
    DTSState = false;
    return OK;
}

//********************************************************************************************************************
uint32_t ThreadDistanceToSpoolState(void )
{
    seglength = dryerbufferMeters;
    REPORT_MSG (seglength,"ThreadDistanceToSpoolState");
//#ifdef FEEDER_LENGTH_CALCULATION
    ThreadUpdateProcessLength (seglength,(void *)ThreadDistanceToSpoolEnded);
/*#else
    ThreadUpdateProcessLength (0,(void *)NULL); //move DTS to job start
    DistanceToSpoolReady(Module_Thread,ModuleDone);
#endif*/
    SegmentState = false;
    PreSegmentState = false;
    DTSState = true;
    return OK;
}

char Endstr[150];
//********************************************************************************************************************
 uint32_t ThreadEndState(void )
{
     int Motor_i;
     ThreadControlActive = false;
     uint32_t status = OK;
     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);

     ThreadUpdateProcessLength (0.0,(void *)NULL);
     //TotalProcessedLength = 0.0;
     SetOriginMotorSpeed(0);
#ifdef HUNDRED_MICROSECONDS_DANCER_READ
     MillisecLogClose();
#endif
     if (SpeedControlId != 0xFF)
     {
         if(RemoveControlCallback(SpeedControlId,ThreadLengthCBFunction)!=OK)
             ReportWithPackageFilter(ThreadFilter,"Remove Control Failed.",__FILE__,__LINE__,(int)ThreadLengthCBFunction,RpError,(int)SpeedControlId,0);
         SpeedControlId = 0xFF;
     }
     if (PoolerSpeedControlId != 0xFF)
     {
         if(RemoveControlCallback(PoolerSpeedControlId,PoolerThreadLengthCBFunction)!=OK)
             ReportWithPackageFilter(ThreadFilter,"Remove Control Failed.",__FILE__,__LINE__,(int)PoolerThreadLengthCBFunction,RpError,(int)PoolerSpeedControlId,0);
         PoolerSpeedControlId = 0xFF;
     }

     for ( Motor_i = 0;Motor_i < SCREW_MOTOR;Motor_i++)
     {
         if (ControlIdtoMotorId[Motor_i] != 0xFF)
         {
             status = RemoveControlCallback(ControlIdtoMotorId[Motor_i],ThreadControlCBFunction);
             if(status == OK)
                 ControlIdtoMotorId[Motor_i] = 0xFF;
             else
                 ReportWithPackageFilter(ThreadFilter,"Remove Control Failed.",__FILE__,__LINE__,(int)Motor_i,RpError,(int)ControlIdtoMotorId[Motor_i],0);
         }

     }
	 Task_sleep(100);
     for ( Motor_i = 0;Motor_i < SCREW_MOTOR;Motor_i++)
     {
        MotorStop(ThreadMotorIdToMotorId[Motor_i],Hard_Hiz);

     }
     MotorStop(HARDWARE_MOTOR_TYPE__MOTO_RLOADING,Hard_Hiz);
     MotorStop(HARDWARE_MOTOR_TYPE__MOTO_LLOADING,Hard_Hiz);
     Release_Right_TFU_Tension();
     IDS_StopLubrication();
    return OK;
}



//********************************************************************************************************************

void ThreadStartPrinting(void)
{
    //PrintingIterate();
}

//********************************************************************************************************************
//********************************************************************************************************************

void ThreadStopPrinting(void)
{
    //PrintingIterate();
}
generated by cgit v1.3.1 (git 2.54.0) at 2026-07-15 17:30:56 +0000