aboutsummaryrefslogtreecommitdiffstats
path: root/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
blob: ab5075ab92c0c7d0c1ac0eb7f3df7d79f14e30df (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
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
/* Generated by the protocol buffer compiler.  DO NOT EDIT! */
/* Generated from: StopThreadLoadingResponse.proto */

/* Do not generate deprecated warnings for self */
#ifndef PROTOBUF_C__NO_DEPRECATED
#define PROTOBUF_C__NO_DEPRECATED
#endif

#include "StopThreadLoadingResponse.pb-c.h"
void   stop_thread_loading_response__init
                     (StopThreadLoadingResponse         *message)
{
  static const StopThreadLoadingResponse init_value = STOP_THREAD_LOADING_RESPONSE__INIT;
  *message = init_value;
}
size_t stop_thread_loading_response__get_packed_size
                     (const StopThreadLoadingResponse *message)
{
  assert(message->base.descriptor == &stop_thread_loading_response__descriptor);
  return protobuf_c_message_get_packed_size ((const ProtobufCMessage*)(message));
}
size_t stop_thread_loading_response__pack
                     (const StopThreadLoadingResponse *message,
                      uint8_t       *out)
{
  assert(message->base.descriptor == &stop_thread_loading_response__descriptor);
  return protobuf_c_message_pack ((const ProtobufCMessage*)message, out);
}
size_t stop_thread_loading_response__pack_to_buffer
                     (const StopThreadLoadingResponse *message,
                      ProtobufCBuffer *buffer)
{
  assert(message->base.descriptor == &stop_thread_loading_response__descriptor);
  return protobuf_c_message_pack_to_buffer ((const ProtobufCMessage*)message, buffer);
}
StopThreadLoadingResponse *
       stop_thread_loading_response__unpack
                     (ProtobufCAllocator  *allocator,
                      size_t               len,
                      const uint8_t       *data)
{
  return (StopThreadLoadingResponse *)
     protobuf_c_message_unpack (&stop_thread_loading_response__descriptor,
                                allocator, len, data);
}
void   stop_thread_loading_response__free_unpacked
                     (StopThreadLoadingResponse *message,
                      ProtobufCAllocator *allocator)
{
  if(!message)
    return;
  assert(message->base.descriptor == &stop_thread_loading_response__descriptor);
  protobuf_c_message_free_unpacked ((ProtobufCMessage*)message, allocator);
}
#define stop_thread_loading_response__field_descriptors NULL
#define stop_thread_loading_response__field_indices_by_name NULL
#define stop_thread_loading_response__number_ranges NULL
const ProtobufCMessageDescriptor stop_thread_loading_response__descriptor =
{
  PROTOBUF_C__MESSAGE_DESCRIPTOR_MAGIC,
  "",
  "",
  "",
  "",
  sizeof(StopThreadLoadingResponse),
  0,
  stop_thread_loading_response__field_descriptors,
  stop_thread_loading_response__field_indices_by_name,
  0,  stop_thread_loading_response__number_ranges,
  (ProtobufCMessageInit) stop_thread_loading_response__init,
  NULL,NULL,NULL    /* reserved[123] */
};
a> 583 584 585 586 587 588 589 590 591 592 593 594 595 596 597 598 599 600 601 602 603 604 605 606 607 608 609 610 611 612 613 614 615 616 617 618 619 620 621 622 623 624 625 626 627 628 629 630 631 632 633 634 635 636 637 638 639 640 641 642 643 644 645 646 647 648 649 650 651 652 653 654 655 656 657 658 659 660 661 662
/*
 * ThreadLoad.c
 *
 *  Created on: Jan 16, 2019
 *      Author: shlomo
 */
#include <DataDef.h>
#include "include.h"
#include "thread_ex.h"

#include "Common/SWUpdate/FileSystem.h"

#include "StateMachines/Printing/PrintingSTM.h"

#include "Modules/thread/thread.h"
#include "Modules/Control/control.h"
#include "Modules/General/process.h"
#include "Modules/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/ProcessParameters.pb-c.h"

#include "drivers/Flash_Memory/fatfs/ff.h"
#include "drivers/I2C_Communication/DAC/Blower.h"
#include "drivers/SSI_Comm/Dancer/Dancer.h"
#include "drivers/motors/motor.h"

#include "PMR/Stubs/StubHeatingTestRequest.pb-c.h"
#include "PMR/Stubs/StubHeatingTestResponse.pb-c.h"
#include "PMR/Stubs/StubHeatingTestPollRequest.pb-c.h"
#include "PMR/Stubs/StubHeatingTestPollResponse.pb-c.h"

//#include <stdint.h>
//#include <stdbool.h>

/*    typedef enum
    {
        0 THREAD_LOAD_INIT,
        1 THREAD_LOAD_REDUCE_HEAT,                           //HEATERS OFF, DRYER BLOWER OFF, BLOWER LOW,
        2 THREAD_LOAD_SET_LOAD_ARM_TO_START_POSITION,        //USE NOTATION HOW MANY ROTATIONS IN THE DRYER, OR CHECK AGAINST STOPPER. MOVE SLOWLY
        3 THREAD_LOAD_CENTER_HEAD_ROCKERS,                   //puthead cleaning rockers to middle position
        4 THREAD_LOAD_OPEN_COVERS,                           //OPEN DYEING HEAD COVER AND DRYER LID
        5 THREAD_LOAD_LIFT_DANCERS,
        6 THREAD_LOAD_LIFT_ROCKERS,                          //MACHINE IS READY. SEND MESSAGE, START TIMER TO CLOSE LIDS, WAIT FOR OPERATOR RESPONSE
        7 THREAD_LOAD_INITIAL_TENSION,                       //CHECK SPOOL PRESENCERUN WINDER UNTIL BREAK SENSOR IS IDENTIFIEING MOVEMENT FOR A SECOND
        8 THREAD_LOAD_CLOSE_ROCKERS,
        9 THREAD_LOAD_CLOSE_DANCERS,                         //SEND DANCER MOTORS TO PRESET LOCATION, CHECK THAT THE DANCERS ARE ON THE THREAD
        10 THREAD_LOAD_CLOSE_LIDS,
        11 THREAD_LOAD_RESUME_HEATING,
        12 THREAD_LOAD_JOG_FEEDER_TO_MIDDLE_POINT,           //JOG THE FEEDER MOTOR UNTIL THE FEEDER DANCER IS AT MIDDLE POSITION
        13 THREAD_LOAD_DRYER_LOADING,                        //START FEEDER PID, ROTATE LOADING ARM COUNTER THREAD DIRECTION X CIRCLES ACCORDING TO RML. FEEDER SPEED IS 40
                                                             //KEEP NOTATION HOW MANY ROTATIONS IN THE DRYER
        14 THREAD_LOAD_JOG_THREAD,                           //JOG THREAD SHORTLY TO MAKE SURE SPOOL IS RUNNING. REPORT END OF LOADING
        15 THREAD_LOAD_END
    }THREAD_LOAD_STAGES_ENUM;*/
    THREAD_LOAD_STAGES_ENUM LoadStages = THREAD_LOAD_INIT;
    ProcessParameters ProcessParametersClear,ProcessParametersRecover;
    uint8_t NumberOfDrierLoaderCycles = 0;
    uint32_t status = OK;
    uint32_t ControlId = 0xFF;
    uint32_t ThreadLoadStateMachine( THREAD_LOAD_STAGES_ENUM LoadStages);
    uint32_t ThreadLoadControlCBFunction(uint32_t index, uint32_t ReadValue);

    typedef struct
    {
        uint32_t LoadArmRounds;
        uint32_t LoadArmBackLash;
    }LoadArmInfoStruc;
    LoadArmInfoStruc LoadArmInfo;
    char LoadArmPath[50] = "0://SysInfo//LoadArm.cfg";

    //RUN MOTOR TO SWITCH WITH TIMEOUT
    //RUN MOTOR TO BREAK SENSOR
    //RUN MOTOR TO DANCER LOCATION (POOLER GOES TO THE OTHER DIRECTION)

    //RUN MOTOR A FULL CYCLE
    //RUN A MOTOR NUMBER OF STEPS
    //RUN CONTROL FOR A SINGLE DANCER
    bool ThreadLoadingActive(void)
    {
        if ((LoadStages > THREAD_LOAD_INIT)&&(LoadStages < THREAD_LOAD_END))
            return true;
        else
            return false;
    }
    uint32_t Thread_Load_Init(void)
    {
        void* buffer = NULL;
        uint32_t Bytes = 0;
        FRESULT Fresult = FR_OK;
        REPORT_MSG(LoadStages, "Thread Load State Machine step");

        LoadArmInfo.LoadArmBackLash = 10;
        LoadArmInfo.LoadArmRounds = 0xFF;
        Fresult = f_mkdir(SW_INFO_DIR);
        if ((Fresult == FR_OK)||(Fresult == FR_EXIST))
        {

            Fresult = FileRead(LoadArmPath, &Bytes, &buffer);
            if (Fresult == FR_OK)
            {
               if (Bytes>=sizeof(LoadArmInfo))
                   memcpy (&LoadArmInfo,buffer,sizeof(LoadArmInfo));
               free (buffer);
            }
        }

        Report("Thread_Load_Init",__FILE__,__LINE__,LoadArmInfo.LoadArmBackLash,RpMessage,LoadArmInfo.LoadArmRounds,0);
        memcpy (&ProcessParametersRecover,&ProcessParametersKeep,sizeof(ProcessParameters));
        //NumberOfDrierLoaderCycles = loadLoadArmParameters();
        LoadStages++;
        ThreadLoadStateMachine(LoadStages);

        return OK;
    }
    uint32_t Thread_Load_Reduce_Heat(void)
    {

        //Heaters Off, Dryer Blower Off, Blower Low,
        REPORT_MSG(LoadStages, "Thread Load State Machine step ");
        memcpy (&ProcessParametersClear,&ProcessParametersKeep,sizeof(ProcessParameters));
        ProcessParametersClear.dryerzone1temp = 0;
        ProcessParametersClear.dryerzone2temp = 0;
        ProcessParametersClear.dryerzone3temp = 0;
        ProcessParametersClear.mixertemp      = 0;
        ProcessParametersClear.headzone1temp  = 0;
        ProcessParametersClear.headzone2temp  = 0;
        ProcessParametersClear.headzone3temp  = 0;
        ProcessParametersClear.headzone4temp  = 0;
        ProcessParametersClear.headzone5temp  = 0;
        ProcessParametersClear.headzone6temp  = 0;
        ProcessParametersClear.dyeingspeed = 40;
        if (HandleProcessParameters(&ProcessParametersClear)!= OK)
        {
            LOG_ERROR (LoadStages, "Thread_Load_Init failed");
        }
        else
        {
            Turn_the_Blower_On();//Turn on with the Default_Voltage
            if (BlowerCfg.heatingvoltage)
                Control_Voltage_To_Blower(BlowerCfg.heatingvoltage);
            else
                Control_Voltage_To_Blower(BlowerCfg.voltage-500);
            Control_Dryer_Fan(STOP,75);//use START or STOP,  0 - 100%
            LoadStages++;
            //ThreadLoadStateMachine(LoadStages);
        }
        return OK;
    }
    uint32_t Thread_Load_Set_Load_Arm_To_Start_Position_Callback(uint32_t deviceID, uint32_t BusyFlag)
    {
       // Report("Thread Load State Machine Callback.",__FILE__,__LINE__,LoadStages,RpMessage,NumberOfDrierLoaderCycles,0);

        /*NumberOfDrierLoaderCycles--;
        if (NumberOfDrierLoaderCycles)
        {
            Report("Thread_Load_Set_Load_Arm_To_Start_Position_Callback",__FILE__,__LINE__,LoadStages,RpMessage,NumberOfDrierLoaderCycles,0);
            status |= MotorMoveWithCallback(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, (1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize),
                                            MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround, Thread_Load_Set_Load_Arm_To_Start_Position_Callback,1000);
        }
        else*/
        {
            Report("Thread_Load_Set_Load_Arm_To_Start_Position_Callback",__FILE__,__LINE__,LoadStages,RpMessage,NumberOfDrierLoaderCycles,0);
            //storeLoadArmParameters();
            SetMotHome(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM);  //set this point as the spool home
            LoadStages++;
            //ThreadLoadStateMachine(LoadStages);
        }
        return OK;
    }
    uint32_t Thread_Load_Set_Load_Arm_To_Stopper_Callback(uint32_t deviceID, uint32_t BusyFlag)
    {
//        Report("Thread Load State Machine Callback.",__FILE__,__LINE__,LoadStages,RpMessage,NumberOfDrierLoaderCycles,0);
        Report("Thread_Load_Set_Load_Arm_To_Stopper_Callback",__FILE__,__LINE__,LoadStages,RpMessage,NumberOfDrierLoaderCycles,0);
        NumberOfDrierLoaderCycles=0;
        //storeLoadArmParameters();
        LoadStages++;
        SetMotHome(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM);  //set this point as the spool home

        //ThreadLoadStateMachine(LoadStages);
        return OK;
    }
    uint32_t Thread_Load_Set_Load_Arm_To_Start_Position(void)
    {
        uint32_t numberOfSteps = 0;
        REPORT_MSG(LoadStages, "Thread Load State Machine step");
        if (LoadArmInfo.LoadArmRounds != 0xFF)
        {
            //Start Feeder Pid, Rotate Loading Arm Counter Thread Direction X Circles According To Rml. Feeder Speed Is 40
            numberOfSteps = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround*LoadArmInfo.LoadArmRounds*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].microstep*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius;
            Report("Thread_Load_Set_Load_Arm_To_Start_Position",__FILE__,__LINE__,numberOfSteps,RpMessage,NumberOfDrierLoaderCycles,0);
            NumberOfDrierLoaderCycles = LoadArmInfo.LoadArmRounds;
            status |= MotorMoveWithCallback(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, (1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize),
                                            numberOfSteps, Thread_Load_Set_Load_Arm_To_Start_Position_Callback,1000);
        }
        else //number of circles is not known - compare to position of the motor to verify location at the stopper
        {
            Report("Thread_Load_Set_Load_Arm_To_Start_Position",__FILE__,__LINE__,LoadStages,RpMessage,NumberOfDrierLoaderCycles,0);
            status |= MotorMoveToStopper(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, (1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize),
                                            MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround/2, Thread_Load_Set_Load_Arm_To_Stopper_Callback,LoadArmInfo.LoadArmBackLash,1000);

        }
        //Use Notation How Many Rotations In The Drier, Or Check Against Stopper. Move Slowly
        return OK;
    }
    uint8_t CallbackCounter = 0;
    uint8_t TimeoutsCounter = 0;

    uint32_t Thread_Load_HomingCallback(uint32_t MotorId, uint32_t ReadValue)
    {
        Report("Thread Load State Machine Callback.",__FILE__,__LINE__,LoadStages,RpMessage,NumberOfDrierLoaderCycles,0);
        REPORT_MSG(MotorId, "Thread_Load_HomingCallback Motor Id");
        if (CallbackCounter)
        {
            CallbackCounter--;
            if (ReadValue != LIMIT)
            {
                //returned with a timeout
                TimeoutsCounter++;
            }
            if (CallbackCounter == 0)
            {
                if (TimeoutsCounter)
                {
                    LOG_ERROR(LoadStages,"Load sequence timeout");
                    TimeoutsCounter = 0;
                }
                else
                {
                    LoadStages++;
                    if (LoadStages == THREAD_LOAD_LIFT_ROCKERS)
                    {
                        MotorSetMicroStep(HARDWARE_MOTOR_TYPE__MOTO_RLOADING,  MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].microstep);
                        MotorSetMicroStep(HARDWARE_MOTOR_TYPE__MOTO_LLOADING,  MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LLOADING].microstep);
                        MotorSetKvalRun(HARDWARE_MOTOR_TYPE__MOTO_RLOADING, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].microstep);
                        MotorSetKvalRun(HARDWARE_MOTOR_TYPE__MOTO_LLOADING, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LLOADING].microstep);
                    }
                    if (LoadStages != THREAD_LOAD_INITIAL_TENSION) //on this stage we should wait for user call
                    {
                        //ThreadLoadStateMachine(LoadStages);
                    }
                }
            }
        }
        else
            LOG_ERROR(MotorId, "Thread_Load_HomingCallback Callback is already 0");
        return OK;
    }
    uint32_t Thread_Load_Center_Head_Rockers(void)
    {
        REPORT_MSG(LoadStages, "Thread Load State Machine step");
        CallbackCounter++;
        MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANHEAD,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANHEAD].directionthreadwize, 80,  Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANHEAD], Thread_Load_HomingCallback,10000);
        CallbackCounter++;
        MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANMECH,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANMECH].directionthreadwize, 80,  Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANMECH], Thread_Load_HomingCallback,10000);
        return OK;
    }
    uint32_t Thread_Load_Open_Covers(void)
    {
        REPORT_MSG(LoadStages, "Thread Load State Machine step");
        //Open Dyeing Head Cover And Dryer Lid
        //HARDWARE_MOTOR_TYPE__MOTO_DH_LID = 2,
        //HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID = 4,
        DeActivateHeadMagnet();
        CallbackCounter++;
        MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_DH_LID,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DH_LID].directionthreadwize, 200,  Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_DH_LID], Thread_Load_HomingCallback,10000);
        CallbackCounter++;
        //MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID].directionthreadwize, 200,  Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID], Thread_Load_HomingCallback,10000);
        MotorGotoWithCallback(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID, 0, Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID], Thread_Load_HomingCallback,4000);
        return OK;
    }
    uint32_t Thread_Load_Lift_Dancers(void)
    {
        REPORT_MSG(LoadStages, "Thread Load State Machine step");
        CallbackCounter++;
        MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RDANCER,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].directionthreadwize, 15,  Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_RDANCER], Thread_Load_HomingCallback,15000);
        CallbackCounter++;
        MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_LDANCER1,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDANCER1].directionthreadwize, 500,  Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_LDANCER1], Thread_Load_HomingCallback,20000);

        return OK;
    }
    uint32_t Thread_Load_Lift_Rockers(void)
    //Machine Is Ready. Send Message, Start Timer To Close Lids, Wait For Operator Response
    {
        REPORT_MSG(LoadStages, "Thread Load State Machine step");
        MotorSetMicroStep(HARDWARE_MOTOR_TYPE__MOTO_RLOADING, 1);
        MotorSetMicroStep(HARDWARE_MOTOR_TYPE__MOTO_LLOADING, 1);
        MotorSetKvalRun(HARDWARE_MOTOR_TYPE__MOTO_RLOADING, 35);
        MotorSetKvalRun(HARDWARE_MOTOR_TYPE__MOTO_LLOADING, 35);
        Task_sleep(10);
        CallbackCounter++;
        MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RLOADING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].directionthreadwize, 50,  Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_RLOADING], Thread_Load_HomingCallback,25000);
        CallbackCounter++;
        MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_LLOADING,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LLOADING].directionthreadwize, 50,  Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_LLOADING], Thread_Load_HomingCallback,25000);

        return OK;
    }
    uint32_t Thread_Load_Initial_Tension(void) //Check Spool Presence, run Winder Until Break Sensor Is Identifieing Movement For A Second
    {
        REPORT_MSG(LoadStages, "Thread Load State Machine step");
        if (FPGA_Read_limit_Switches(GPI_SW_SPOOL_EXISTS)==LIMIT)
        {
            REPORT_MSG(LIMIT, "No cone in winder");
            return ERROR;
        }
        CallbackCounter++;
        MotorMovetoBreakSensor (HARDWARE_MOTOR_TYPE__MOTO_WINDER,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_WINDER].directionthreadwize, 500,  Thread_Load_HomingCallback,10000);

        return OK;

    }
    uint32_t Thread_Load_Close_Rockers(void)
    {
        REPORT_MSG(LoadStages, "Thread Load State Machine step");
        CallbackCounter++;
        MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RLOADING,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].directionthreadwize, 300,  Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_RLOADING], Thread_Load_HomingCallback,12000);
        CallbackCounter++;
        MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_LLOADING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LLOADING].directionthreadwize, 300,  Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_LLOADING], Thread_Load_HomingCallback,12000);

        return OK;
    }

    uint32_t Thread_Load_Close_Dancers(void)
    //Send Dancer Motors To Preset Location, Check That The Dancers Are On The Thread
    {
        REPORT_MSG(LoadStages, "Thread Load State Machine step");
        CallbackCounter++;
        MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RDANCER,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].directionthreadwize, 15,  Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_RDANCER], Thread_Load_HomingCallback,4000);
        CallbackCounter++;
        MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_LDANCER1,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDANCER1].directionthreadwize, 500,  Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_LDANCER1], Thread_Load_HomingCallback,10000);

        return OK;
    }
    uint32_t Thread_Load_Close_Lids(void)
    {
        REPORT_MSG(LoadStages, "Thread Load State Machine step");
        //Close Dyeing Head Cover And Dryer Lid
        //HARDWARE_MOTOR_TYPE__MOTO_DH_LID = 2,
        //HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID = 4,
        CallbackCounter++;
        MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_DH_LID,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DH_LID].directionthreadwize, 200,  Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_DH_LID], Thread_Load_HomingCallback,10000);
        CallbackCounter++;
//        MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID].directionthreadwize, 200,  Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID], Thread_Load_HomingCallback,10000);
        MotorGotoWithCallback(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID, 2, Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID], Thread_Load_HomingCallback,4000);

        return OK;
    }
    uint32_t Thread_Load_Resume_Heating(void)
    {
        REPORT_MSG(LoadStages, "Thread Load State Machine step");
        ActivateHeadMagnet();

        if (HandleProcessParameters(&ProcessParametersRecover)!= OK)
        {
            LOG_ERROR (LoadStages, "Thread_Load_Resume_Heating failed");
        }
        else
        {
            Control_Dryer_Fan(START,75);//use START or STOP,  0 - 100%
            LoadStages++;
            //ThreadLoadStateMachine(LoadStages);
        }
        return OK;
    }
    uint32_t Thread_Load_Jog_Feeder_To_Middle_Point(void)
    //Jog The Feeder Motor Until The Feeder Dancer Is At Middle Position
    {
        REPORT_MSG(LoadStages, "Thread Load State Machine step");
        CallbackCounter++;
        MotorMovetoDancerPosition (HARDWARE_MOTOR_TYPE__MOTO_RDRIVING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].directionthreadwize, 200, FEEDER_DANCER ,true, Thread_Load_HomingCallback,10000);
        return OK;
    }

    uint32_t Thread_Load_Dryer_Loading_Callback(uint32_t MotorId, uint32_t ReadValue)
    {
        Report("Thread Load State Machine Callback.",__FILE__,__LINE__,LoadStages,RpMessage,NumberOfDrierLoaderCycles,0);
        REPORT_MSG(MotorId, "Thread_Load_Dryer_Loading_Callback Motor Id");
        MotorMovetoEncoderPosition(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,0,0);
        if(ControlId != 0xFF)
        {
            MotorStop(ThreadMotorIdToMotorId[FEEDER_MOTOR],Hard_Hiz);
            RemoveControlCallback(ControlId, ThreadLoadControlCBFunction );
            ControlId = 0xFF;
        }
        if (CallbackCounter)
        {
            CallbackCounter--;
            if (CallbackCounter == 0)
            {
                LoadStages++;
                if (LoadStages != THREAD_LOAD_INITIAL_TENSION) //on this satge we should wait for user call
                {
                    //ThreadLoadStateMachine(LoadStages);
                }
            }
        }
        else
            LOG_ERROR(MotorId, "Thread_Load_HomingCallback Callback is already 0");
        return OK;
    }

    uint32_t Thread_Load_Dryer_Loading(void)
    {
        REPORT_MSG(LoadStages, "Thread Load State Machine step");
        LoadArmInfo.LoadArmRounds = 20;
        uint32_t numberOfSteps = 0;
        //Start Feeder Pid, Rotate Loading Arm Counter Thread Direction X Circles According To Rml. Feeder Speed Is 40
        SetOriginMotorSpeed(19);
//        OriginalMotorSpd_2PPS[FEEDER_MOTOR] = 1000;
//        CurrentControlledSpeed[FEEDER_MOTOR] = 1000;

        numberOfSteps = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround*LoadArmInfo.LoadArmRounds*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].microstep*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius;
        MotorControlConfig[FEEDER_MOTOR].m_params.MAX = 1;
        MotorControlConfig[FEEDER_MOTOR].m_params.MIN = MotorsControl[FEEDER_MOTOR].outputproportionalpowerlimit*-1;
        MotorControlConfig[FEEDER_MOTOR].m_params.Kd = MotorsControl[FEEDER_MOTOR].derivativetime;
        MotorControlConfig[FEEDER_MOTOR].m_params.Kp = MotorsControl[FEEDER_MOTOR].proportionalgain;
        MotorControlConfig[FEEDER_MOTOR].m_params.Ki = MotorsControl[FEEDER_MOTOR].integraltime;
        MotorControlConfig[FEEDER_MOTOR].m_params.IntegralErrorMultiplier = MotorsControl[FEEDER_MOTOR].setpointramprateorsoftstartramp;
        MotorControlConfig[FEEDER_MOTOR].m_params.ProportionalErrorMultiplier = MotorsControl[FEEDER_MOTOR].outputonoffhysteresisvalue;
        MotorControlConfig[FEEDER_MOTOR].m_params.epsilon = MotorsControl[FEEDER_MOTOR].epsilon;
        MotorControlConfig[FEEDER_MOTOR].m_params.dt = MotorsControl[FEEDER_MOTOR].controloutputtype;
        MotorControlConfig[FEEDER_MOTOR].m_ingnoreValue =  MotorsControl[FEEDER_MOTOR].sensorcorrectionadjustment; // the minimal change required to change the motor speed in pulses
        MotorControlConfig[FEEDER_MOTOR].m_calculatedError = 0;
        MotorControlConfig[FEEDER_MOTOR].m_integral = 0;
        MotorControlConfig[FEEDER_MOTOR].m_isEnabled = true;
        MotorControlConfig[FEEDER_MOTOR].m_isReady = true;
        MotorControlConfig[FEEDER_MOTOR].m_mesuredParam = 0;
        MotorControlConfig[FEEDER_MOTOR].m_preError = 0;
        MotorControlConfig[FEEDER_MOTOR].m_SetParam = 0;//need to update SetParams on presegment stage
        MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_RDRIVING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].directionthreadwize);

        ControlId = AddControlCallback(ThreadLoadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+FEEDER_MOTOR),FEEDER_DANCER,FEEDER_MOTOR);
        CallbackCounter++;
        MotorMoveWithCallback (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize,
                               numberOfSteps,    Thread_Load_Dryer_Loading_Callback, 100000);

        //Keep Notation How Many Rotations In The Dryer
        //LoadArmInfo.LoadArmBackLash = 0;
        LoadArmInfo.LoadArmRounds = 0xFF;
        FileWrite(&LoadArmInfo, sizeof(LoadArmInfo),LoadArmPath);
        return OK;
    }
    uint32_t Thread_Load_Jog_ThreadStop(uint32_t index, uint32_t ReadValue)
    {
        REPORT_MSG(LoadStages, "Thread Load State Machine step");
        SafeRemoveControlCallback(ControlId, Thread_Load_Jog_ThreadStop );
        ControlId = 0xFF;
        ThreadAbortJoggingFunc();
        LoadStages++;
        //ThreadLoadStateMachine(LoadStages);
        return OK;
    }
    uint32_t Thread_Load_Jog_Thread(void)
    //Jog Thread Shortly To Make Sure Spool Is Running. Report End Of Loading
    {
        REPORT_MSG(LoadStages, "Thread Load State Machine step");
        ThreadJoggingFunc(20);
        ControlId = AddControlCallback(Thread_Load_Jog_ThreadStop, eOneSecond*5,Control_Read_Dancer_Position,(IfTypeThread*0x100+FEEDER_MOTOR),FEEDER_DANCER,FEEDER_MOTOR);
        return OK;
    }
    uint32_t Thread_Load_End(void)
    {
        REPORT_MSG(LoadStages,"Loading Ended");
        return OK;
    }

    uint32_t ThreadLoadStateMachine( THREAD_LOAD_STAGES_ENUM ReadValue)
    {
        REPORT_MSG(ReadValue,"ThreadLoadStateMachine");
        switch (ReadValue)
        {
        case THREAD_LOAD_INIT:
            //LoadStages++;
            Thread_Load_Init();
            break;
        case THREAD_LOAD_REDUCE_HEAT:  //HEATERS OFF, DRYER BLOWER OFF, BLOWER LOW,
            //LoadStages++;
            Thread_Load_Reduce_Heat();
            break;
        case THREAD_LOAD_SET_LOAD_ARM_TO_START_POSITION://USE NOTATION HOW MANY ROTATIONS IN THE DRYER, OR CHECK AGAINS STOPPER. MOVE SLOWLY
            //LoadStages++;
            Thread_Load_Set_Load_Arm_To_Start_Position();
            break;
        case THREAD_LOAD_CENTER_HEAD_ROCKERS:
            //LoadStages++;
            Thread_Load_Center_Head_Rockers();
            break;
        case THREAD_LOAD_OPEN_COVERS:  //OPEN DYEING HEAD COVER AND DRYER LID
            //LoadStages++;
            Thread_Load_Open_Covers();
            break;
        case THREAD_LOAD_LIFT_DANCERS:
            //LoadStages++;
            Thread_Load_Lift_Dancers();
            break;
        case THREAD_LOAD_LIFT_ROCKERS: //MACHINE IS READY. SEND MESSAGE, START TIMER TO CLOSE LIDS, WAIT FOR OPERATOR RESPONSE
            //LoadStages++;
            Thread_Load_Lift_Rockers();
            break;
        case THREAD_LOAD_INITIAL_TENSION://CHECK SPOOL PRESENCERUN WINDER UNTIL BREAK SENSOR IS IDENTIFIEING MOVEMENT FOR A SECOND
            //LoadStages++;
            Thread_Load_Initial_Tension();
            break;
        case THREAD_LOAD_CLOSE_ROCKERS:
            //LoadStages++;
            Thread_Load_Close_Rockers();
            break;
        case THREAD_LOAD_CLOSE_DANCERS: //SEND DANCER MOTORS TO PRESET LOCATION, CHECK THAT THE DANCERS ARE ON THE THREAD
            //LoadStages++;
            Thread_Load_Close_Dancers();
            break;
        case THREAD_LOAD_CLOSE_LIDS:
            //LoadStages++;
            Thread_Load_Close_Lids();
            break;
        case THREAD_LOAD_RESUME_HEATING:
            //LoadStages++;
            Thread_Load_Resume_Heating();
            break;
        case THREAD_LOAD_JOG_FEEDER_TO_MIDDLE_POINT://JOG THE FEEDER MOTOR UNTIL THE FEEDER DANCER IS AT MIDDLE POSITION
            //LoadStages++;
            Thread_Load_Jog_Feeder_To_Middle_Point();
            break;
        case THREAD_LOAD_DRYER_LOADING: //START FEEDER PID, ROTATE LOADING ARM COUNTER THREAD DIRECTION X CIRCLES ACCORDING TO RML. FEEDER SPEED IS 40
                                    //KEEP NOTATION HOW MANY ROTATIONS IN THE DRYER
            //LoadStages++;
            Thread_Load_Dryer_Loading();
            break;
        case THREAD_LOAD_JOG_THREAD: //JOG THREAD SHORTLY TO MAKE SURE SPOOL IS RUNNING. REPORT END OF LOADING
            //LoadStages++;
            Thread_Load_Jog_Thread();
            break;
        case THREAD_LOAD_END:
            Thread_Load_End();
            break;
        default:
            LOG_ERROR(LoadStages,"ERROR IN LOAD STATE MACHINE");
            break;
    }
    return OK;
}

uint32_t ThreadLoadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
{
    //read value is the dancer angle
    int i;
    int DancerId;
    int32_t TranslatedReadValue, avreageSampleValue = 0;
    int index=MAX_THREAD_MOTORS_NUM;
    //double tempcalcspeed = 0;
    uint32_t calculated_speed;
    double NormalizedError;

    DancerId = FEEDER_DANCER;//ThreadMotorIdToDancerId[index];
    if (ReadValue < 10)
    {
        Report("Dancer value read too small.",__FILE__,__LINE__,DancerId,RpError,ReadValue,0);
        return OK;
    }
    if (ReadValue == 0x3FFF)
    {
        return OK;
    }
    TranslatedReadValue = ReadValue - DancersCfg[DancerId].zeropoint;
    if (IfIndex>>8 != IfTypeThread)
    {
        LOG_ERROR (IfIndex, "Wrong  Interface type");
        return 0xFFFFFFFF;
    }
    index = IfIndex&0xFF;
    /*if (index == POOLER_MOTOR)
    {
        //pooler dancer is right sided: data is opposite
        TranslatedReadValue = (-1*TranslatedReadValue);
    }*/
    //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;


    NormalizedError = avreageSampleValue*NormalizedErrorCoEfficient[index];
    MotorControlConfig[index].m_mesuredParam = NormalizedError;
    MotorControlConfig[index].m_calculatedError = AdvancedPIDAlgorithmCalculation((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);
    }*/
    calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*OriginalMotorSpd_2PPS[index];
    if (abs(calculated_speed-CurrentControlledSpeed[index])> MotorControlConfig[index].m_ingnoreValue)
    {
        CurrentControlledSpeed[index] = calculated_speed;
        MotorSetSpeed(ThreadMotorIdToMotorId[index], calculated_speed);
    }
 return OK;
}
void ThreadLoadRequest(MessageContainer* requestContainer)
{
//#ifdef DEBUG_TEST_FUNCTIONS
    MessageContainer responseContainer;
    uint8_t* container_buffer;
    //uint32_t status = OK;

    StubHeatingTestRequest* request = stub_heating_test_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);
    StubHeatingTestResponse response = STUB_HEATING_TEST_RESPONSE__INIT;

    responseContainer = createContainer(MESSAGE_TYPE__StubHeatingTestResponse, requestContainer->token, false, &response, &stub_heating_test_response__pack, &stub_heating_test_response__get_packed_size);
    container_buffer = my_malloc(message_container__get_packed_size(&responseContainer));


    int LoadStage = (int)request->dryerzone1temp;
    LoadStages = LoadStage;
    ThreadLoadStateMachine(LoadStage);

    /*if (status)
    {
        responseContainer.has_error = true;
        responseContainer.error = ERROR_CODE__JOB_UNSPECIFIED_ERROR;
    }*/
    size_t container_size = message_container__pack(&responseContainer, container_buffer);
    my_free(responseContainer.data.data);
    //USBCDCD_sendData(container_buffer, container_size,10);
    SendChars(container_buffer, container_size);
    stub_heating_test_request__free_unpacked(request,NULL);

//#else
//    LOG_ERROR (-1, "Heating Control not on debug");
//    return ERROR;
//#endif
}
void ThreadLoadPollRequest(MessageContainer* requestContainer)
{
    uint8_t* container_buffer;

    StubHeatingTestPollRequest* request = stub_heating_test_poll_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);

    StubHeatingTestPollResponse response = STUB_HEATING_TEST_POLL_RESPONSE__INIT;
    MessageContainer responseContainer;

    response.has_heater1percentage = true;
    response.heater1percentage = LoadStages;
    responseContainer = createContainer(MESSAGE_TYPE__StubHeatingTestPollResponse, requestContainer->token, false, &response, &stub_heating_test_poll_response__pack, &stub_heating_test_poll_response__get_packed_size);
    container_buffer = my_malloc(message_container__get_packed_size(&responseContainer));


/*
    {
        responseContainer.has_error = true;
        responseContainer.error = ERROR_CODE__JOB_UNSPECIFIED_ERROR;
    }*/
    size_t container_size = message_container__pack(&responseContainer, container_buffer);
    my_free(responseContainer.data.data);
    SendChars(container_buffer, container_size);
    stub_heating_test_poll_request__free_unpacked(request,NULL);
}