aboutsummaryrefslogtreecommitdiffstats
path: root/Software/Visual_Studio/MachineStudio/Modules/Tango.MachineStudio.Statistics/Converters/MidTankLevelToElementHeightConverter.cs
blob: de002046eb8558228d69e1070a3cb4cbbadbdf0a (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
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
using System;
using System.Globalization;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Windows.Data;

namespace Tango.MachineStudio.Statistics.Converters
{
    public class MidTankLevelToElementHeightConverter : IMultiValueConverter
    {
        public const double MAX_QUANTITY = 130000000;
        public object Convert(object[] values, Type targetType, object parameter, CultureInfo culture)
        {
            try
            {
                double parentActualHeight;
                Double.TryParse(values[0].ToString(), out parentActualHeight);
                double quantity;
                Double.TryParse(values[1].ToString(), out quantity);
                
                double  midTankLevel = (double)Math.Min(quantity, MAX_QUANTITY);
                double delta = ((midTankLevel / MAX_QUANTITY) * parentActualHeight);
                if (quantity > 0 && midTankLevel < (MAX_QUANTITY/10))// if quantity < 10|% set 2 pixel 
                    delta = 2.0;
                var test = delta;
                return parentActualHeight - delta;
            }
            catch
            {
                return 0d;
            }
        }

        public object[] ConvertBack(object value, Type[] targetTypes, object parameter, CultureInfo culture)
        {
            throw new NotImplementedException();
        }
    }
}
/************************************************************************************************************************
 * 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 "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/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 "modules/heaters/heaters.h"
#include "modules/General/process.h"
#include "Modules/AlarmHandling/AlarmHandling.h"
#include "Control/MillisecTask.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
///////////////////////////////////////////////////////////////////////////////////////////

uint32_t CurrentControlledSpeed[MAX_THREAD_MOTORS_NUM] = {0};

TimerMotors_t ThreadMotorIdToMotorId[MAX_THREAD_MOTORS_NUM] = {HARDWARE_MOTOR_TYPE__MOTO_RDRIVING,HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING,HARDWARE_MOTOR_TYPE__MOTO_LDRIVING,HARDWARE_MOTOR_TYPE__MOTO_WINDER,HARDWARE_MOTOR_TYPE__MOTO_SCREW};
HardwareDancerType ThreadMotorIdToDancerId[MAX_THREAD_MOTORS_NUM] = {FEEDER_DANCER,NUM_OF_DANCERS,POOLER_DANCER,WINDER_DANCER,NUM_OF_DANCERS};
uint32_t    ControlIdtoMotorId [MAX_THREAD_MOTORS_NUM] = {0xFF};
uint32_t    SpeedControlId=0xFF;

double DancerError[NUM_OF_DANCERS] = {0.0};
int OriginalMotorSpd_2PPS[MAX_THREAD_MOTORS_NUM] = {0};
uint32_t JobCounter = 0;
typedef struct
{
    bool                m_isEnabled;
    int32_t             m_SetParam;
    float               m_mesuredParam;
    float               m_preError;
    float               m_integral;
    float               m_calculatedError;
    bool                m_isReady;
    PID_Config_Params   m_params;
}MotorControlConfig_t;

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

uint32_t PreviousPosition = 0, CurrentPosition = 0;
double totalLength = 0.0;
double CurrentRequestedLength = 0.0;
double CurrentProcessedLength = 0.0;
double TotalProcessedLength = 0.0;
double LengthCalculationMultiplier;
bool PrepareState = 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);
void SetOriginMotorSpeed(float process_speed);

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;
  else
    Time_Pass = Current_Read - Previous_Read;

  return (Time_Pass);
}
/*****************************************************************************************
 *
 *
 *
 *
 *
 *
 * **************************************************************************************/
uint32_t initialpos = 0xFFFF;

void ThreadUpdateProcessLength (double length, void *Funcptr)
{
    CurrentRequestedLength = length*100;//Centimetres
    CurrentProcessedLength = 0;
    //PreviousPosition = 0;
    //CurrentPosition = 0;
    totalLength = 0;
    ProcessedLengthFuncPtr = (ProcessedLengthFunc)Funcptr;
    initialpos = 0xFFFF;
}
double MotorSentData[100] = {0};
uint32_t PosDif[100] = {0};


int MotorDataIndex = 0;

uint32_t ThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue)
{
    uint32_t positionDiff = 0;
    double length = 0.0;
    char str[150];
    int index = MAX_THREAD_MOTORS_NUM;
    if (IfIndex>>8 != IfTypeThread)
    {
        LOG_ERROR (IfIndex, "Wrong  Interface type");
        return 0xFFFFFFFF;
    }
    index = IfIndex&0xFF;
//    if (CurrentRequestedLength == 0.0)
//        return OK;
    if (index != FEEDER_MOTOR)
    {
        LOG_ERROR (IfIndex, "Wrong Motor");
        return 0xFFFFFFFF;
    }
    CurrentPosition = MotorGetPosition(ThreadMotorIdToMotorId[index]);
//    if (CurrentPosition == 0)
//        return OK; //unusable data
    if (initialpos == 0xFFFF)
    {
        PreviousPosition = CurrentPosition;
        initialpos = 0;
    }
    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 > 0.1)
    {
        totalLength+=length;
    }

//#warning control disabled
        CurrentProcessedLength+=length;

    PosDif[MotorDataIndex] = CurrentPosition;
    //PosDif[MotorDataIndex] = positionDiff;
    MotorSentData[MotorDataIndex] = length;
    MotorDataIndex+=1;
    if (MotorDataIndex == 99) MotorDataIndex = 0;
    static int pooler_counter = 0;
    pooler_counter++;
    TotalProcessedLength+= (length/100);
    if (pooler_counter%10 == 0)
    {
        if (PrepareState == true)
        {
            //later - add temperatures
             TemperatureListString(str);

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

    }
    if (CurrentProcessedLength>=CurrentRequestedLength )
    {
        // segment/intersegment/distance to spool finished
        if (ProcessedLengthFuncPtr)
            ProcessedLengthFuncPtr();
    }
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;
    uint32_t calculated_speed;
    float speed = getSensorSpeedData();
    if (IfIndex>>8 != IfTypeThread)
    {
        LOG_ERROR (IfIndex, "Wrong  Interface type");
        return 0xFFFFFFFF;
    }
    index = IfIndex&0xFF;
    SpeedSamples[MotorSamplePointer[index]] = speed;//(-1 * TranslatedReadValue);
    MotorSamplePointer[index]++;
    if (MotorSamplePointer[index] >= MotorsControl[index].pvinputfilterfactormode)
        MotorSamplePointer[index] = 0;
    for (i=0;i<MotorsControl[index].pvinputfilterfactormode;i++)
        avreageSampleValue += SpeedSamples[i];
    avreageSampleValue = avreageSampleValue / 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 (abs(calculated_speed-CurrentControlledSpeed[index])>2)
        {
            CurrentControlledSpeed[index] = calculated_speed;
            MotorSetSpeed(ThreadMotorIdToMotorId[index], calculated_speed);
        }
    }
 return OK;
}
uint32_t _speed;
uint32_t ThreadControlSpeedReadFunction(uint32_t IfIndex, uint32_t ReadValue)
{
    int index;
    if (IfIndex>>8 != IfTypeThread)
    {
        LOG_ERROR (IfIndex, "Wrong  Interface type");
        return 0xFFFFFFFF;
    }
    index = IfIndex&0xFF;

    if(MotorControlConfig[index].m_isEnabled )
    {
        int MotorId = ThreadMotorIdToMotorId[index];
        _speed = MotorGetSpeedFromFPGA_Res ((TimerMotors_t)MotorId);
    }
    return OK;
}
/*double calculatedError[100];
double eNormalizedError[100];
int    readValue[100];
int    TranslatedreadValue[100];
int    AveragereadValue[100];
int    calculatedspeed[100];
int controlIndex = 0;
int32_t KeepReadValue = 0;
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;
    ThreadControlActive = true;
    SetOriginMotorSpeed(30.0);
    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;
}*/
bool dancerinvalid = false;
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 DancerId;
    int32_t TranslatedReadValue, avreageSampleValue = 0;
    //double tempcalcspeed = 0;
    uint32_t calculated_speed;
    double NormalizedError;
    char Message[60];

    if (ThreadControlActive == false)
        return OK;
    if (IfIndex>>8 != IfTypeThread)
    {
        LOG_ERROR (IfIndex, "Wrong  Interface type");
        return 0xFFFFFFFF;
    }
    index = IfIndex&0xFF;

    /*for (i=0;i<MAX_THREAD_MOTORS_NUM;i++)
        if (ControlIdtoMotorId[i] == MotorId)
        {
            index = i;
            break;
        }
    if (index==MAX_THREAD_MOTORS_NUM)
    {
        LOG_ERROR (MotorId, "No motor  for device");
        return 0xFFFFFFFF;
    }*/

    if(MotorControlConfig[index].m_isEnabled )
    {
        DancerId = ThreadMotorIdToDancerId[index];
        if (ReadValue < 10)
        {
            REPORT_MSG(ReadValue, "Dancer value read too small.");
            return OK;
        }
        if (ReadValue == 0x3FFF)
        {
            if (dancerinvalid == false)
            {
                dancerinvalid = true;
                LOG_ERROR(index, "Dancer value invalid.");
            }
            return OK;
        }
        TranslatedReadValue = ReadValue - DancersCfg[DancerId].zeropoint;
        if (index == POOLER_MOTOR)
        {
			//pooler dancer is right sided: data is opposite
            TranslatedReadValue = (-1*TranslatedReadValue);
            JobCounter++;
        }
        //TranslatedReadValue = 0;//test
        MotorSamples[index][MotorSamplePointer[index]] = TranslatedReadValue;//(-1 * TranslatedReadValue);
        MotorSamplePointer[index]++;
        if (MotorSamplePointer[index] >= MotorsControl[index].pvinputfilterfactormode)
            MotorSamplePointer[index] = 0;
        for (i=0;i<MotorsControl[index].pvinputfilterfactormode;i++)
            avreageSampleValue += MotorSamples[index][i];
        avreageSampleValue = avreageSampleValue / MotorsControl[index].pvinputfilterfactormode;

        if (BreakSensorenabled == true)
        {
            if (index == POOLER_MOTOR)
            {
                if (JobCounter > eOneSecond)
                {
                    if (ReadBreakSensor()==ERROR)
                    {
                        //consider applying the debouce parameters later
                        //BreakSensordebouncetimemilli
                        JobEndReason = JOB_THREAD_BREAK;
                        ThreadControlActive = false;
                        SendJobProgress(0.0,0,false, "ReadBreakSensor Error");
                        SegmentReady(Module_Thread,ModuleFail);
                        AlarmHandlingSetAlarm(EVENT_TYPE__ThreadBreak,true);
                        //EndState(CurrentJob,"ReadBreakSensor Error" );
                        LOG_ERROR(index, "ReadBreakSensor Error");
                        return OK;
                    }
                }
            }
        }

        //Stop Execution if the dancer moves too much
        if ((abs(avreageSampleValue)> DancerStopActivityLimit[index])&&(JobCounter > eOneSecond))
        {
            usnprintf(Message, 60, "Dancer %d limit %d value %d Zero %d",DancerId,DancerStopActivityLimit[index],avreageSampleValue,DancersCfg[DancerId].zeropoint);
            //JobAbortedByUser = true;
            ThreadControlActive = false;
            JobEndReason = JOB_DANCER_FAIL;
            SendJobProgress(0.0,0,false, Message);
            //EndState(CurrentJob,Message );
            SegmentReady(Module_Thread,ModuleFail);
            AlarmHandlingSetAlarm(EVENT_TYPE__ThreadTensionControlFailure,true);
            LOG_ERROR (index, "Dancer Failure");
            return OK;
        }
        NormalizedError = avreageSampleValue*NormalizedErrorCoEfficient[index];
        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);
        if (index != FEEDER_MOTOR) //feeder unit handles errors opposite to left unit
        {
            MotorControlConfig[index].m_calculatedError = (-1*MotorControlConfig[index].m_calculatedError);
        }
        else
        {
            //KeepNormalizedError = NormalizedError;
        }
        calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*OriginalMotorSpd_2PPS[index];
        /*if (index == FEEDER_MOTOR)
        {
            if (KeepReadValue != TranslatedReadValue)
            {
                eNormalizedError[controlIndex] = NormalizedError;
                calculatedError[controlIndex] = MotorControlConfig[index].m_calculatedError;
                readValue[controlIndex] = ReadValue;
                TranslatedreadValue[controlIndex] = TranslatedReadValue;
                AveragereadValue[controlIndex] = avreageSampleValue;
                calculatedspeed[controlIndex] = calculated_speed;
                controlIndex++;
                if (controlIndex >= 99) controlIndex = 0;
                KeepReadValue = TranslatedReadValue;
            }
        }*/
        if (abs(calculated_speed-CurrentControlledSpeed[index])>2)
        {
            CurrentControlledSpeed[index] = calculated_speed;
            MotorSetSpeed(ThreadMotorIdToMotorId[index], calculated_speed);
        }
    }

 return OK;
}

//********************************************************************************************************************
uint32_t ThreadGetMotorSpeed(threadMotorsEnum MotorId)
{
    return  CurrentControlledSpeed[MotorId];
}

//********************************************************************************************************************
uint32_t ThreadInitialTestStub(HardwareMotor * request)
{


    //MotorsConfigMessage(request);
     ThreadPrepareState(request);
     ThreadPreSegmentState(request);
    return OK;
}
bool InitialProcess = false;
uint32_t ThreadEmptyCBFunction(uint32_t IfIndex, uint32_t ReadValue)
{
    return OK;
}

//********************************************************************************************************************
 uint32_t ThreadPrepareState(void *JobDetails)
{
    int Motor_i, HW_Motor_Id, Pid_Id;
    CurrentSegmentId = 0;

    JobCounter = 0;
    TotalProcessedLength = 0.0;
    PrepareState = true;
    //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];*/
            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.epsilon = 0.1;
            MotorControlConfig[Motor_i].m_params.dt = 1000;
            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

            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 (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(ThreadLengthCBFunction, 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)
                {
                    RemoveControlCallback(ControlIdtoMotorId[Motor_i],ThreadControlCBFunction);
                    ControlIdtoMotorId[Motor_i] = 0xFF;
                    CurrentControlledSpeed[Motor_i] = 0;
                }
                ControlIdtoMotorId[Motor_i] = AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i);
                //AddControlCallback(ThreadControlSpeedReadFunction, eHundredMillisecond,MotorGetSpeedFromFPGA,(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 n//ot be controlled
            {
                if (ControlIdtoMotorId[Motor_i] != 0xFF)
                {
                    RemoveControlCallback(ControlIdtoMotorId[Motor_i],ThreadControlCBFunction);
                    CurrentControlledSpeed[Motor_i] = 0;
                    ControlIdtoMotorId[Motor_i] = 0xFF;
                }
                ControlIdtoMotorId[Motor_i] = AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i);
            }
            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
            {
                if (ControlIdtoMotorId[Motor_i] != 0xFF)
                {
                    RemoveControlCallback(ControlIdtoMotorId[Motor_i],ThreadControlCBFunction);
                    CurrentControlledSpeed[Motor_i] = 0;
                    ControlIdtoMotorId[Motor_i] = 0xFF;
                }
                ControlIdtoMotorId[Motor_i] = AddControlCallback(ThreadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToDancerId[Motor_i],Motor_i);
            }
//            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,ThreadEmptyCBFunction,(IfTypeThread*0x100+Motor_i),ThreadMotorIdToMotorId[Motor_i],0);
            if (Motor_i == 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
                continue;
    }
    //testDancersControl();
    PrepareReady(Module_Thread,ModuleDone);
    //set 3 dancers to the profile positions
    InitialProcess = true;
    return OK;
}

void SetOriginMotorSpeed(float process_speed)
{
    int Motor_i, HW_Motor_Id;
    for (Motor_i = 0; Motor_i <= WINDER_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;
        CurrentControlledSpeed[Motor_i] = (int) motor_speed;
    }
}

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

    float process_speed = dyeingspeed;
    if (dyeingspeed == 0)
    {
        LOG_ERROR (dyeingspeed," job speed zero");
        return ERROR;
    }

    SetOriginMotorSpeed(process_speed);
    ThreadControlActive = true;
    PrepareState = false;
    // set the new speed in the dryer motor to the speed of the new segment
    MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING, OriginalMotorSpd_2PPS[DRYER_MOTOR]);
 //only for testing - when control works, these motors will take their speed from the dryer
    //MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_LDRIVING, OriginalMotorSpd_2PPS[POOLER_MOTOR]);
 //only for testing - when control works, these motors will take their speed from the dryer
    //MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_RDRIVING, OriginalMotorSpd_2PPS[FEEDER_MOTOR]);

//#warning rocker disabled
    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, 2);
    }
    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, 2);
    }
 //#warning rocker disabled

//    MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RDRIVING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].directionthreadwize, 0, GPI_LS_RLOADMOTOR_UP, EndState); //TODO

    // 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) && JobTicket->enableintersegment == true)
    {
        ThreadUpdateProcessLength (JobTicket->intersegmentlength,(void *)ThreadInterSegmentEnded);
    }
    else
    {
        ThreadUpdateProcessLength (0,(void *)NULL);
        PreSegmentReady(Module_Thread,ModuleDone);
        JobCounter = 0;
        InitialProcess = false;
    }

    return OK;
}
void ThreadInterSegmentEnded(void)
{
    PreSegmentReady(Module_Thread,ModuleDone);
}
void ThreadSegmentEnded(void)
{
    SegmentReady(Module_Thread,ModuleDone);
}
void ThreadDistanceToSpoolEnded(void)
{
    DistanceToSpoolReady(Module_Thread,ModuleDone);
}
double seglength = 0.0;
//********************************************************************************************************************
uint32_t ThreadSegmentState(void *JobDetails, int SegmentId)
{
    JobTicket* JobTicket = JobDetails;
    seglength = JobTicket->segments[SegmentId]->length;
    CurrentSegmentId = SegmentId;
    ThreadUpdateProcessLength (seglength,(void *)ThreadSegmentEnded);
    return OK;
}

//********************************************************************************************************************
uint32_t ThreadDistanceToSpoolState(void )
{
    seglength = dryerbufferlength;
    ThreadUpdateProcessLength (seglength,(void *)ThreadDistanceToSpoolEnded);
    return OK;
}

//********************************************************************************************************************
 uint32_t ThreadEndState(void *JobDetails)
{
     int Motor_i;
     ThreadControlActive = false;
     ThreadUpdateProcessLength (0.0,(void *)NULL);
    SetOriginMotorSpeed(0);

     if (SpeedControlId != 0xFF)
     {
         RemoveControlCallback(SpeedControlId,ThreadLengthCBFunction);
         SpeedControlId = 0xFF;
     }
     for ( Motor_i = 0;Motor_i < MAX_THREAD_MOTORS_NUM;Motor_i++)
     {
         if (ControlIdtoMotorId[Motor_i] != 0xFF)
         {
             RemoveControlCallback(ControlIdtoMotorId[Motor_i],ThreadControlCBFunction);
         }
        MotorStop(ThreadMotorIdToMotorId[Motor_i],Hard_Hiz);
     }
     MotorStop(HARDWARE_MOTOR_TYPE__MOTO_RLOADING,Hard_Hiz);
     MotorStop(HARDWARE_MOTOR_TYPE__MOTO_LLOADING,Hard_Hiz);

    return OK;
}



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

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

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

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