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

14 files changed, 241 insertions, 138 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
index aa8f29451..5cccc5b45 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
@@ -54,6 +54,7 @@
 
 #include "Communication/Connection.h"
 #include "Modules/General/buttons.h"
+#include "Drivers/I2C_Communication/Head_Card/PT100/Head_PT100_ADC.h"
 
 Task_Handle Millisecond_Task_Handle;
 /******************** Definitions  ********************************************/
@@ -527,7 +528,7 @@ int MillisecGetTemperatures (TEMPERATURE_SENSOR_ID_ENUM SensorId)
         return 0;
     return TemperatureCalc[SensorId];
 }
-bool RapidPressureRead = false;
+bool RapidPressureRead = true;
 float PressureSum[MAX_SYSTEM_DISPENSERS];
 float PressureMin[MAX_SYSTEM_DISPENSERS];
 float PressureMax[MAX_SYSTEM_DISPENSERS];
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.c b/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.c
index 6c1b647be..3d1d353c2 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.c
@@ -3,41 +3,6 @@
 
 float PIDAlgorithmCalculation(float _setPoint,float _mesuredParam , PID_Config_Params *params, float *_pre_error, float *_integral)
 {
-	float error;
-	float derivative;
-	float output;
-
-	//Calculate P,I,D
-	error = _setPoint - _mesuredParam;
-
-	//In case of error too small then stop integration
-	if(fabs(error) > params->epsilon)
-	{
-		*_integral = *_integral + error*params->dt;
-	}
-
-	derivative = (error - *_pre_error)/params->dt;
-
-	output = params->Kp*error + params->Ki**_integral + params->Kd*derivative;
-
-	//Saturation Filter
-	if(output > params->MAX)
-	{
-		output = params->MAX;
-	}
-	else if(output < params->MIN)
-	{
-		output = params->MIN;
-	}
-
-	//Update error
-	*_pre_error = error;
-
-	return output;
-}
-
-float AdvancedPIDAlgorithmCalculation(float _setPoint,float _mesuredParam , PID_Config_Params *params, float *_pre_error, float *_integral)
-{
     float error;
     float derivative;
     float output;
@@ -70,7 +35,7 @@ float AdvancedPIDAlgorithmCalculation(float _setPoint,float _mesuredParam , PID_
 
     return output;
 }
-
+/*
 float TestPIDAlgorithmCalculation(float _setPoint,float _mesuredParam , PID_Config_Params *params, float *_pre_error, float *_integral)
 {
     float error;
@@ -81,14 +46,14 @@ float TestPIDAlgorithmCalculation(float _setPoint,float _mesuredParam , PID_Conf
     error = _setPoint - _mesuredParam;
     //ITerm+= (ki * error);
     *_integral = *_integral + (error*params->Ki);
-    /*  if(ITerm > outMax)
+    /`*  if(ITerm > outMax)
       {
           ITerm= outMax;
       }
       else if(ITerm < outMin)
       {
           ITerm= outMin;
-      } */
+      } *`/
     if(*_integral > params->MAX)
     {
         *_integral = params->MAX;
@@ -100,7 +65,7 @@ float TestPIDAlgorithmCalculation(float _setPoint,float _mesuredParam , PID_Conf
     //  double dInput = (input - lastInput);
     derivative = error - *_pre_error;
 
-      /*Compute PID Output*/
+      /`*Compute PID Output*`/
     //  double output = kp * error + ITerm- kd * dInput;
     output = params->Kp*error/params->ProportionalErrorMultiplier + *_integral/params->IntegralErrorMultiplier + params->Kd*derivative;
 
@@ -119,3 +84,4 @@ float TestPIDAlgorithmCalculation(float _setPoint,float _mesuredParam , PID_Conf
 
     return output;
 }
+*/
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.h b/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.h
index 4792be9ec..4f94f3612 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.h
+++ b/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.h
@@ -17,6 +17,5 @@ typedef struct
 
 }PID_Config_Params;
 float PIDAlgorithmCalculation(float _setPoint,float _mesuredParam , PID_Config_Params *params, float *_pre_error, float *_integral);
-float AdvancedPIDAlgorithmCalculation(float _setPoint,float _mesuredParam , PID_Config_Params *params, float *_pre_error, float *_integral);
 
 #endif /* MODULES_PIDALGO_H_ */
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/control.c b/Software/Embedded_SW/Embedded/Modules/Control/control.c
index b75e44001..0d73c9c93 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/control.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/control.c
@@ -181,9 +181,12 @@ uint32_t ControlActivityLed( uint32_t Parameter1)
         {
             FPGA_WD_Occurred = false;
             AlarmHandlingSetAlarm(EVENT_TYPE__FPGA_WATCHDOG_ACTIVATED,true);
-            JobEndReason = JOB_MOTOR_ALARM;
-            SendJobProgress(0.0,0,false, "Hardware Failure Error");
-            AbortJob("FPGA Watchdog Error");
+            if (JobIsActive())
+            {
+                JobEndReason = JOB_MOTOR_ALARM;
+                SendJobProgress(0.0,0,false, "Hardware Failure Error");
+                AbortJob("FPGA Watchdog Error");
+            }
             ReportWithPackageFilter(FPGAFilter, "FPGA Watchdog Error",__FILE__,__LINE__,0,RpError, 0,0);
 
             ACTIVITY_GREEN_LED_ON;
diff --git a/Software/Embedded_SW/Embedded/Modules/Diagnostics/DiagnosticsHoming.c b/Software/Embedded_SW/Embedded/Modules/Diagnostics/DiagnosticsHoming.c
index 033f9daf1..c58d54a8c 100644
--- a/Software/Embedded_SW/Embedded/Modules/Diagnostics/DiagnosticsHoming.c
+++ b/Software/Embedded_SW/Embedded/Modules/Diagnostics/DiagnosticsHoming.c
@@ -45,7 +45,7 @@ uint32_t MotorHomingRequestCallback(uint32_t deviceID, uint32_t ReadValue)
     MotorHomingResponse response = MOTOR_HOMING_RESPONSE__INIT;
 
     responseContainer = createContainer(MESSAGE_TYPE__MotorHomingResponse, HomingToken[deviceID], true, &response, &motor_homing_response__pack, &motor_homing_response__get_packed_size);
-    responseContainer.continuous = false;
+    responseContainer.continuous = true;
     uint8_t* container_buffer = my_malloc(message_container__get_packed_size(&responseContainer));
     size_t container_size = message_container__pack(&responseContainer, container_buffer);
     my_free(responseContainer.data.data);
@@ -141,7 +141,7 @@ uint32_t MotorHomingRequestFunc(MessageContainer* requestContainer)
     responseContainer.error = ERROR_CODE__INVALID_PROCESS_ID;
     responseContainer.errormessage = "Job Active or incorrect motor ID";
     responseContainer = createContainer(MESSAGE_TYPE__MotorHomingResponse,  requestContainer->token, true, &response, &motor_homing_response__pack, &motor_homing_response__get_packed_size);
-    responseContainer.continuous = false;
+    responseContainer.continuous = true;
     uint8_t* container_buffer = my_malloc(message_container__get_packed_size(&responseContainer));
     size_t container_size = message_container__pack(&responseContainer, container_buffer);
     my_free(responseContainer.data.data);
diff --git a/Software/Embedded_SW/Embedded/Modules/General/buttons.c b/Software/Embedded_SW/Embedded/Modules/General/buttons.c
index 22af2b07e..d8c68b345 100644
--- a/Software/Embedded_SW/Embedded/Modules/General/buttons.c
+++ b/Software/Embedded_SW/Embedded/Modules/General/buttons.c
@@ -226,7 +226,8 @@ uint32_t Buttons_Init(void)
     strcpy(load.bttn_name, "load");
     load.bttn_status = RELEASE;    // 0=RELEASE 1=PRESS
     load.Action = OFFPB ;   //OFFPB,short,long,count,replong
-    load.color = colorOFF;   //colorOFF, BLUE, BLINK,
+    Pannel_Leds(THREAD_LOAD, MODE_ON);//AVI+
+    load.color = colorON;   //colorOFF, BLUE, BLINK,
     load.state = sttRDY; //
     load.count = 0;
 
@@ -338,6 +339,13 @@ uint32_t ButtonLoadCBFunction(uint32_t IfIndex, uint32_t ReadValue)
            LoadLongPress( &load );
            break;
        default:
+           if (ThreadLoadingActive() == false)
+           {
+               if (JobIsActive())
+                   load.color =  colorOFF;
+               else
+                   load.color =  colorON;
+           }
            break;
    }
 return OK;
@@ -528,29 +536,29 @@ uint32_t LoadStatMachine( button *pBtn)
     switch (pBtn->state)
     {
         case sttRDY :
-            ReportWithPackageFilter(GeneralFilter,"------------ load.state = sttRDY -----------------", __FILE__,__LINE__,0, RpMessage, 0, 0);
-/*            switch (pBtn->Action)
-            {
-                case (SHORTPB):
-                case (LONGPB):
-                case (REPLONGPB):
-                    pBtn->state = sttPRELOAD;
-                    pBtn->color =  BLINK;*/
-                    if (ThreadLoadButton( THREAD_LOAD_INIT)== OK)
-                    {
-                        pBtn->state = sttPRELOAD ;    // to do
-                        pBtn->color =  colorON;
-                        Pannel_Leds(THREAD_LOAD, MODE_ON);//AVI+
-                    }
-                    else
-                    {
-                       // pBtn->state = sttDISABLE ;
-                        Pannel_Leds(THREAD_LOAD, MODE_OFF);//AVI+
-                    }
-                    /*break;
-                default :
-                    break;
-            }*/
+//            ReportWithPackageFilter(GeneralFilter,"------------ load.state = sttRDY -----------------", __FILE__,__LINE__,0, RpMessage, 0, 0);
+///*            switch (pBtn->Action)
+//            {
+//                case (SHORTPB):
+//                case (LONGPB):
+//                case (REPLONGPB):
+//                    pBtn->state = sttPRELOAD;
+//                    pBtn->color =  BLINK;*/
+//                    if (ThreadLoadButton( THREAD_LOAD_INIT)== OK)
+//                    {
+//                        pBtn->state = sttPRELOAD ;    // to do
+//                        pBtn->color =  colorON;
+//                        Pannel_Leds(THREAD_LOAD, MODE_ON);//AVI+
+//                    }
+//                    else
+//                    {
+//                       // pBtn->state = sttDISABLE ;
+//                        Pannel_Leds(THREAD_LOAD, MODE_OFF);//AVI+
+//                    }
+//                    /*break;
+//                default :
+//                    break;
+//            }*/
             break;
         case sttPRELOAD:
             ReportWithPackageFilter(GeneralFilter,"------------ load.state = sttPRELOAD -----------------", __FILE__,__LINE__,0, RpMessage, 0, 0);
@@ -650,7 +658,25 @@ void test_avi()
 uint32_t LoadLongPress( button *pBtn)
 {
     ReportWithPackageFilter(GeneralFilter,"------------ load.state = LoadLongPress -----------------", __FILE__,__LINE__,0, RpMessage, 0, 0);
-    Thread_Load_End();
+    switch (pBtn->state)
+    {
+        case sttRDY :
+            ReportWithPackageFilter(GeneralFilter,"------------ load.state = sttRDY -----------------", __FILE__,__LINE__,0, RpMessage, 0, 0);
+            if (ThreadLoadButton( THREAD_LOAD_INIT)== OK)
+            {
+                pBtn->state = sttPRELOAD ;    // to do
+                pBtn->color =  colorON;
+                Pannel_Leds(THREAD_LOAD, MODE_ON);//AVI+
+            }
+            else
+            {
+               // pBtn->state = sttDISABLE ;
+                Pannel_Leds(THREAD_LOAD, MODE_OFF);//AVI+
+            }
+            break;
+        default:
+            Thread_Load_End();
+    }
     return OK;
 }
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_init.c b/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_init.c
index d7fc0703a..78ae37eef 100644
--- a/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_init.c
+++ b/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_init.c
@@ -45,7 +45,7 @@ typedef enum {
 }HEATERS_EVENTS_ENUM;
 */
 /******************** GLOBAL PARAMETERS  ********************************************/
-HeaterPIDControlConfig HeaterControl[HEATER_TYPE_MAX_HEATERS] = {0,0,0,0,0,0,0,0,0,0,0,0};
+HeaterPIDControlConfig HeaterControl[HEATER_TYPE_MAX_HEATERS] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
 bool AcHeaterConfigured[MAX_AC_HEATERS] = {0,0,0};
 
 int      NumberOFSlicesInUse = 0;
diff --git a/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c b/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c
index 71fd9a428..50796d3c9 100644
--- a/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c
@@ -1110,16 +1110,8 @@ uint32_t HeaterControlCBFunction(uint32_t IfIndex, uint32_t readValue)
             }
             else
             {
-                if (HeaterPIDConfig[index].m_params.IntegralErrorMultiplier == 0)
-                {
-                    HeaterPIDConfig[index].m_calculatedError = PIDAlgorithmCalculation(HeaterPIDConfig[index].m_SetParam , HeaterPIDConfig[index].m_mesuredParam,
-                                                          &HeaterPIDConfig[index].m_params,   &HeaterPIDConfig[index].m_preError, &HeaterPIDConfig[index].m_integral);
-                }
-                else
-                {
-                    HeaterPIDConfig[index].m_calculatedError = AdvancedPIDAlgorithmCalculation(HeaterPIDConfig[index].m_SetParam , HeaterPIDConfig[index].m_mesuredParam,
-                                                          &HeaterPIDConfig[index].m_params,   &HeaterPIDConfig[index].m_preError, &HeaterPIDConfig[index].m_integral);
-                }
+               HeaterPIDConfig[index].m_calculatedError = PIDAlgorithmCalculation(HeaterPIDConfig[index].m_SetParam , HeaterPIDConfig[index].m_mesuredParam,
+                                     &HeaterPIDConfig[index].m_params,   &HeaterPIDConfig[index].m_preError, &HeaterPIDConfig[index].m_integral);
             //}
 //            len = usnprintf(ACheatstr, 254, "ACD Id, Temp , Integral, Output{ %d, %d ,%d, %d} ",index,(int)HeaterPIDConfig[index].m_mesuredParam ,(int)HeaterPIDConfig[index].m_integral,(int)HeaterPIDConfig[index].m_calculatedError);
             //        ReportWithPackageFilter(HeatersFilter,logmsg[index],__FILE__,__LINE__,index,RpWarning,index,  Counter[index]);
@@ -1272,16 +1264,8 @@ uint32_t DCHeaterControlCBFunction(uint32_t IfIndex, uint32_t readValue)
             }
             else
             {
-                if (HeaterPIDConfig[index].m_params.IntegralErrorMultiplier == 0)
-                {
-                    HeaterPIDConfig[index].m_calculatedError = PIDAlgorithmCalculation(HeaterPIDConfig[index].m_SetParam , HeaterPIDConfig[index].m_mesuredParam,
-                                                          &HeaterPIDConfig[index].m_params,   &HeaterPIDConfig[index].m_preError, &HeaterPIDConfig[index].m_integral);
-                }
-                else
-                {
-                    HeaterPIDConfig[index].m_calculatedError = AdvancedPIDAlgorithmCalculation(HeaterPIDConfig[index].m_SetParam , HeaterPIDConfig[index].m_mesuredParam,
-                                                          &HeaterPIDConfig[index].m_params,   &HeaterPIDConfig[index].m_preError, &HeaterPIDConfig[index].m_integral);
-                }
+                HeaterPIDConfig[index].m_calculatedError = PIDAlgorithmCalculation(HeaterPIDConfig[index].m_SetParam , HeaterPIDConfig[index].m_mesuredParam,
+                                      &HeaterPIDConfig[index].m_params,   &HeaterPIDConfig[index].m_preError, &HeaterPIDConfig[index].m_integral);
 
             }
             //        error[index][Counter[index]]    = HeaterPIDConfig[index].m_preError;
@@ -1469,7 +1453,7 @@ uint32_t HeatersControlLoop(uint32_t tick)
     SliceCounter++;
     if (SliceCounter >= NumberOFSlicesInUse)
         SliceCounter = 0;
-    //Trigger_HeaterWriting();
+
     return OK;
 }
 /******************************************************************************
diff --git a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c
index 28bb191fe..00d740c7e 100644
--- a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c
@@ -159,7 +159,7 @@ uint32_t IDS_DispenserPidRequestMessage(HardwarePidControl* request)
     DispenserControlConfig[Dispenser_i].m_isReady = true;
     DispenserControlConfig[Dispenser_i].m_mesuredParam = 0;
     DispenserControlConfig[Dispenser_i].m_preError = 0;
-    DispenserControlConfig[Dispenser_i].m_SetParam = 1.65;//need to update SetParams on presegment stage
+    DispenserControlConfig[Dispenser_i].m_SetParam = DispensersCtrl[Dispenser_i].outputproportionalcycletime;//need to update SetParams on presegment stage
 
 
     return OK;
@@ -169,7 +169,7 @@ uint32_t IDS_Pid_Testing_Func(uint32_t DispenserId, uint32_t ReadValue)
 {
     float calculated_speed;
     DispenserControlConfig[DispenserId].m_mesuredParam = MillisecGetPressures(DispenserId);
-    DispenserControlConfig[DispenserId].m_calculatedError = AdvancedPIDAlgorithmCalculation((float)DispenserControlConfig[DispenserId].m_SetParam , (float)DispenserControlConfig[DispenserId].m_mesuredParam,
+    DispenserControlConfig[DispenserId].m_calculatedError = PIDAlgorithmCalculation((float)DispenserControlConfig[DispenserId].m_SetParam , (float)DispenserControlConfig[DispenserId].m_mesuredParam,
                                                       &DispenserControlConfig[DispenserId].m_params,   &DispenserControlConfig[DispenserId].m_preError, &DispenserControlConfig[DispenserId].m_integral);
     DispenserControlConfig[DispenserId].m_calculatedError = (-1*DispenserControlConfig[DispenserId].m_calculatedError);
     calculated_speed = (1-DispenserControlConfig[DispenserId].m_calculatedError)*CurrentDispenserSpeed[DispenserId];
diff --git a/Software/Embedded_SW/Embedded/Modules/IFS/ifs.c b/Software/Embedded_SW/Embedded/Modules/IFS/ifs.c
index c6128b0cb..15117e616 100644
--- a/Software/Embedded_SW/Embedded/Modules/IFS/ifs.c
+++ b/Software/Embedded_SW/Embedded/Modules/IFS/ifs.c
@@ -32,6 +32,10 @@ bool IFS_TimeOutAlarm(bool status);
 bool ColorMatch();
 bool MidTankValvesAction(bool action);
 
+bool IFS_MidTankFilling(void)
+{
+    return (IFS_info.Ink.time_out > 0);
+}
 
 //RfidTagContent TagContent[MAX_CARTRIDGES];
 NFC_Tag NFCTag[MAX_CARTRIDGES];
@@ -230,8 +234,8 @@ bool MidTankValvesAction(bool action) //Cartridge_MidTank_ON of Cartridge_MidTan
     {
         Disable_MidTank_Pressure_Reading(IFS_info.Ink.cart_color);
     }
-#warning check that the dispenser is not in homing before CLOSING the air valve
-    Valve_Set(IDS_Id_to_AirValve[IFS_info.Ink.cart_color], action );   //Atm_MidTank_OFF/ON
+    if (IDS_IsHomingActive(IFS_info.Ink.cart_color)==false)
+        Valve_Set(IDS_Id_to_AirValve[IFS_info.Ink.cart_color], action );   //Atm_MidTank_OFF/ON
     Valve_Set(IDS_Id_to_CartrideValve[IFS_info.Ink.cart_color], action );   //Atm_MidTank_OFF/ON
 
     ret = true;
@@ -396,7 +400,9 @@ uint32_t MidTankCallBackFunction(uint32_t IfIndex, uint32_t BusyFlag)
 uint32_t MidTankReading(void)
 {
     MidTankCounter = 0;
-    MidTankReadControlId = AddControlCallback("MidTankRead", MidTankCallBackFunction, 300/*eHundredMillisecond*/, TemplateDataReadCBFunction,0,0, 0 );
+    if (IFS_info.Ink.time_out > 0)
+        return ERROR;
+    MidTankReadControlId = AddControlCallback( MidTankCallBackFunction, 300/*eHundredMillisecond*/, TemplateDataReadCBFunction,0,0, 0 );
 
     return OK;
 }
diff --git a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Progress.c b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Progress.c
index 8d0a23f23..013d22968 100644
--- a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Progress.c
+++ b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Progress.c
@@ -24,7 +24,7 @@
 #include "Modules/IFS/ifs.h"
 #include "Modules/IDS/ids_ex.h"
 #include "Modules/Control/MillisecTask.h"
-
+#include "modules/thread/thread_ex.h"
 #include <Drivers/I2C_Communication/I2C.h>
 #include <Drivers/I2C_Communication/Head_Card/I2C_Head_Mux.h>
 #include <Drivers/I2C_Communication/Head_Card/IO_Ports/Head_IO.h>
@@ -276,11 +276,26 @@ void Stub_ProgressRequest(MessageContainer* requestContainer)
 
     }
     else
-    if((request->amount == 0xDF) && (request->delay == 0xDF)) //Power off
+    if(request->amount == 0xB2)  //fast refresh for pressure
+    {
+        LOG_ERROR(request->delay,"Set loading arm cycles");
+
+        response.progress = (double)MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,request->delay);
+        response.has_progress = true;
+
+    }
+    else     
+	if((request->amount == 0xDF) && (request->delay == 0xDF)) //Power off
     {
         PowerOffInit();
     }
     else
+    if(request->amount == 0xF1)
+    {
+        LOG_ERROR(request->delay,"NumberOfRotationPerPassage");
+        NumberOfRotationPerPassage = (float)(request->delay)/1000;
+    }
+    else
     {
         response.has_progress = true;
         int i = 0;
diff --git a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_FPGARWReg.c b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_FPGARWReg.c
index 2fd85db09..a058d54f9 100644
--- a/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_FPGARWReg.c
+++ b/Software/Embedded_SW/Embedded/Modules/Stubs_Handler/Stub_FPGARWReg.c
@@ -24,6 +24,8 @@
 #include "drivers/FPGA/FPGA.h"
 #include "drivers/FPGA/FPGA_Comm.h"
 
+#include "control/control.h"
+
 
 void Stub_FpgaReadRegRequest(MessageContainer* requestContainer)
 {
@@ -67,10 +69,19 @@ void Stub_FpgaReadRegRequest(MessageContainer* requestContainer)
     //free(requestContainer);
 
 }
+uint32_t value = 0,ctlId;
+uint32_t ResetCallBackFunction(uint32_t IfIndex, uint32_t BusyFlag)
+{
+    SafeRemoveControlCallback(ctlId, ResetCallBackFunction );
+    volatile short *ptr = (volatile short *)(FPGA3_BASE | 0x3D0);
+    *ptr = (value & 0xFFFF);
+    return OK;
+}
 
 void Stub_FpgaWriteRegRequest(MessageContainer* requestContainer)
 {
     uint32_t status = PASSED;
+    volatile short *ptr;
 
     MessageContainer responseContainer;
 
@@ -82,9 +93,14 @@ void Stub_FpgaWriteRegRequest(MessageContainer* requestContainer)
     }
     else
     {
-        volatile short *ptr;
+        value = request->value;
         ptr = (volatile short *) (request->address);
-        *ptr = (request->value & 0xFFFF);
+        if (request->address == (FPGA3_BASE | 0x3D0))
+        {
+            ctlId = AddControlCallback( ResetCallBackFunction, 2* eOneSecond, TemplateDataReadCBFunction,ptr,ptr, 0 );
+        }
+        else
+            *ptr = (request->value & 0xFFFF);
     }
     StubFpgaWriteRegResponse response =   STUB_FPGA_WRITE_REG_RESPONSE__INIT;
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
index fb5de9011..40000b8bb 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/ThreadLoad.c
@@ -74,6 +74,8 @@
     int32_t keepkvalrun,keepkvalhold;
     uint8_t CallbackCounter = 0;
     uint8_t TimeoutsCounter = 0;
+    bool SecondTry = false,TryAgain = false;
+    int MessageState = 0; //0 - none, 1 = start, 2 = continue
 
     uint32_t UnloadingStart = 0;
     uint8_t NumberOfDrierLoaderCycles = 0;
@@ -112,7 +114,8 @@
         else
             return false;
     }
-    uint32_t Thread_Load_Init(void)
+    bool InitCalled = false;
+    uint32_t Thread_Load_Init(THREAD_LOAD_STAGES_ENUM Stage)
     {
         //void* buffer = NULL;
         //uint32_t Bytes = 0;
@@ -128,7 +131,8 @@
         //EEPROM_STORAGE_DRYER_CYCLES
         MCU_E2PromRead(EEPROM_STORAGE_DRYER_CYCLES,&LoadArmInfo.LoadArmRounds);
 
-        Read_Dryer_ENC_Position();
+
+        Report("Read_Dryer_ENC_Position()",__FILE__,__LINE__,0,RpWarning,Read_Dryer_ENC_Position(),0);
 
         //if (LoadArmInfo.LoadArmRounds <= 2)
         //        LoadArmInfo.LoadArmRounds = 20;
@@ -151,8 +155,12 @@
         StopInitSequence();
         memcpy (&ProcessParametersRecover,&ProcessParametersKeep,sizeof(ProcessParameters));
         //NumberOfDrierLoaderCycles = loadLoadArmParameters();
-        LoadStages++;
-        ThreadLoadStateMachine(LoadStages);
+        if (Stage == THREAD_LOAD_INIT)
+        {
+            LoadStages++;
+            ThreadLoadStateMachine(LoadStages);
+        }
+        InitCalled = true;
 
         return OK;
     }
@@ -179,6 +187,7 @@
             LOG_ERROR (LoadStages, "Thread_Load_Init failed");
             strcpy(LoadErrorMsg,"Thread_Load_Init failed");
             LoadStatus = ERROR;
+            TryAgain = true;
         }
         else
         {
@@ -273,13 +282,37 @@
         //Use Notation How Many Rotations In The Drier, Or Check Against Stopper. Move Slowly
         return OK;
     }*/
-    uint32_t Thread_Load_Jog_Feeder_To_Middle_Point_Callback(uint32_t deviceID, uint32_t BusyFlag)
+    uint32_t Thread_Load_Jog_Feeder_To_Middle_Point_Callback(uint32_t deviceID, uint32_t Condition)
     {
-        CallbackCounter--;
-        Report("Thread_Load_Jog_Feeder_To_Middle_Point_Callback",__FILE__,__LINE__,LoadStages,RpMessage,CallbackCounter,0);
-        //storeLoadArmParameters();
-        LoadStages++;
-        ThreadLoadStateMachine(LoadStages);
+        if (Condition == true)
+        {
+            CallbackCounter--;
+            Report("Thread_Load_Jog_Feeder_To_Middle_Point_Callback",__FILE__,__LINE__,LoadStages,RpMessage,CallbackCounter,0);
+            //storeLoadArmParameters();
+            LoadStages++;
+            ThreadLoadStateMachine(LoadStages);
+        }
+        else
+        {
+            CallbackCounter--;
+            Report("Thread_Load_Jog_Feeder_To_Middle_Point TimeOut!",__FILE__,__LINE__,LoadStages,RpMessage,CallbackCounter,0);
+            //storeLoadArmParameters();
+            if (SecondTry == true)
+            {
+                LoadStages++;
+                ThreadLoadStateMachine(LoadStages);
+                LoadStatus = OK;
+            }
+            else
+            {
+                TimeoutsCounter = 0;
+                load.color = fastBILNK;
+                usnprintf(LoadErrorMsg, 100, "Thread_Load_Jog_Feeder_To_Middle_Point TimeOut!");
+                LoadStatus = ERROR;
+                ThreadLoadingReport();
+                TryAgain = true;
+            }
+        }
 
         return OK;
     }
@@ -321,6 +354,7 @@
                     usnprintf(LoadErrorMsg, 100, "Load sequence timeout %d motor %d",LoadStages, MotorId);
                     LoadStatus = ERROR;
                     ThreadLoadingReport();
+                    TryAgain = true;
                 }
                 else
                 {
@@ -371,10 +405,10 @@
     uint32_t Thread_Load_Lift_Dancers(void)
     {
         REPORT_MSG(LoadStages, "Thread Load State Machine step");
-        keepkvalhold = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].kvalhold;
+        /*keepkvalhold = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].kvalhold;
         MotorSetKvalHold(HARDWARE_MOTOR_TYPE__MOTO_RDANCER, 40);
         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);
+        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,25000);
 
@@ -387,8 +421,17 @@
         keepkvalrun = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].kvalrun;
         if (keepkvalrun>=25)
             keepkvalrun = 25;
+
+        Report("Read_Dryer_ENC_Position()",__FILE__,__LINE__,0,RpWarning,Read_Dryer_ENC_Position(),0);
+
         Report("Thread_Load_Lift_Rockers",__FILE__,__LINE__,keepkvalrun,RpMessage,keepmicrostep,0);
 
+        Report("Thread_Load_Lift_right dancer",__FILE__,__LINE__,0,RpMessage,0,0);
+        keepkvalhold = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].kvalhold;
+        MotorSetKvalHold(HARDWARE_MOTOR_TYPE__MOTO_RDANCER, 40);
+        CallbackCounter++;
+        MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RDANCER,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].directionthreadwize, 22,  Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_RDANCER], Thread_Load_HomingCallback,15000);
+
         REPORT_MSG(LoadStages, "Thread Load State Machine step");
         MotorSetMicroStep(HARDWARE_MOTOR_TYPE__MOTO_RLOADING, 8);
         MotorSetMicroStep(HARDWARE_MOTOR_TYPE__MOTO_LLOADING, 8);
@@ -489,7 +532,7 @@
     {
         REPORT_MSG(LoadStages, "Thread Load State Machine step");
         CallbackCounter++;
-        MotorMovetoDancerPosition (HARDWARE_MOTOR_TYPE__MOTO_RDRIVING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].directionthreadwize, 100, FEEDER_DANCER ,true, Thread_Load_Jog_Feeder_To_Middle_Point_Callback,2000);
+        MotorMovetoDancerPosition (HARDWARE_MOTOR_TYPE__MOTO_RDRIVING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].directionthreadwize, 100, FEEDER_DANCER ,true, Thread_Load_Jog_Feeder_To_Middle_Point_Callback,4000);
         return OK;
     }
 
@@ -529,14 +572,16 @@
     }
     float numberOfSteps = 0;
     float numberOfCycles = 0;
-    uint32_t DrierPrevLocation = 0;
+    double DrierPrevLocation = 0;
     uint32_t Thread_Load_Dryer_Loading_Callback(uint32_t MotorId, uint32_t ReadValue)
     {
         numberOfCycles++;
-        uint32_t temp = Control_Read_Dryer_Position(0,0);
-        if ((abs (temp -DrierPrevLocation)>20  )&&(ReadValue == NOTBUSY))
+
+        Report("Thread_Load_Dryer_Loading_Callback",__FILE__,(int)TotalLoadedLen,(int)DrierPrevLocation,RpMessage,ReadValue,0);
+
+        if ((fabs (TotalLoadedLen  -DrierPrevLocation)>50  )&&(ReadValue == NOTBUSY))
         {
-            DrierPrevLocation = temp;
+            DrierPrevLocation = TotalLoadedLen ;
             //Report("Thread_Load_Dryer_Loading_Callback",__FILE__,__LINE__,LoadStages,RpMessage,NumberOfDrierLoaderCycles,0);
             //Report("Thread_Load_Dryer_Loading_Callback details",__FILE__,(int)(TotalLoadedLen),numberOfCycles,RpMessage,CallbackCounter,0);
             if (LoadStages != THREAD_LOAD_DRYER_LOADING)
@@ -558,6 +603,8 @@
         else
         {
             LOG_ERROR(LoadStages,"Load sequence timeout - drier loading");
+            TryAgain = true;
+            SecondTry = false;
             Report("Store Number of cycles in drier - halted",__FILE__,__LINE__,numberOfCycles,RpMessage,LoadArmInfo.LoadArmRounds,0);
             MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,numberOfCycles);
             TimeoutsCounter = 0;
@@ -727,6 +774,8 @@
     {
         numberOfCycles++;
         uint32_t temp = Control_Read_Dryer_Position(0,0);
+        Report("Thread_Load_Dryer_Loading_Callback",__FILE__,ReadValue,temp,RpMessage,DrierPrevLocation,0);
+        //Report("Thread_Load_Dryer_Loading_Callback details",__FILE__,(int)(TotalLoadedLen),numberOfCycles,RpMessage,CallbackCounter,0);
         if ((abs (temp -DrierPrevLocation)>20  )&&(ReadValue == NOTBUSY))
         {
             DrierPrevLocation = temp;
@@ -754,6 +803,7 @@
             Report("Store Number of cycles in drier - halted",__FILE__,__LINE__,numberOfCycles,RpMessage,LoadArmInfo.LoadArmRounds,0);
             MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,LoadArmInfo.LoadArmRounds-numberOfCycles);
             TimeoutsCounter = 0;
+            CallbackCounter = 0;
             if(PullerControlId != 0xFF)
             {
                 MotorStop(ThreadMotorIdToMotorId[POOLER_MOTOR],Hard_Hiz);
@@ -851,15 +901,15 @@
 
         MCU_E2PromRead(EEPROM_STORAGE_DRYER_CENTER,&DrierPrevLocation);
 
-        Report("Thread_Load_Set_Load_Arm_To_Start_Position",__FILE__,UnloadingStart,LoadStages,RpMessage,NumberOfDrierLoaderCycles,0);
+        Report("Thread_Load_Set_Load_Arm_To_Start_Position",__FILE__,UnloadingStart,LoadStages,RpMessage, LoadArmInfo.LoadArmRounds,0);
         MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround/4*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius);
 //        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/4, Thread_Load_Set_Load_Arm_To_Stopper_Callback,LoadArmInfo.LoadArmBackLash,1000);
         MotorMoveWithCallback (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, 1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize,
                                numberOfSteps-400,    Thread_Load_Dryer_UnLoading_Callback, 10000);
 
-        Screw_Dir = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize;
-        status = MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_SCREW,Screw_Dir, 80, screw[Screw_Dir], Thread_Load_switchCallback,30000);
+        Screw_Dir = 1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize;
+        status = MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_SCREW,Screw_Dir, 80, screw[Screw_Dir], Thread_Load_switchCallback,20000);
 
         //Keep Notation How Many Rotations In The Dryer
         //LoadArmInfo.LoadArmRounds = (int)dryerbufferlength;
@@ -877,11 +927,24 @@
         {
         case THREAD_LOAD_INIT:
             //LoadStages++;
-            Thread_Load_Init();
+            MessageState = 1;
+
+            Thread_Load_Init(ReadValue);
             break;
         case THREAD_LOAD_REDUCE_HEAT:  //HEATERS OFF, DRYER BLOWER OFF, BLOWER LOW,
             //LoadStages++;
+            MessageState = 1;
+            if (InitCalled == false)
+            {
+                REPORT_MSG(ReadValue,"Thread_Load_Init called from 1");
+                Thread_Load_Init(ReadValue);
+            }
+            else
+            {
+                REPORT_MSG(ReadValue,"Thread_Load_Init called from 0");
+            }
             Thread_Load_Reduce_Heat();
+            InitCalled = false;
             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++;
@@ -939,6 +1002,7 @@
             ThreadLoadingReport();
             break;
         case THREAD_LOAD_END:
+            MessageState = 0;
             LoadStages = THREAD_LOAD_END;
             Thread_Load_End();
             break;
@@ -961,9 +1025,19 @@ uint32_t ThreadLoadButton(THREAD_LOAD_STAGES_ENUM ReadValue)
         return ERROR;
     }
     Report("ThreadLoadButton",__FILE__,__LINE__,LoadStages,RpMessage,ReadValue,0);
-    if (LoadStages > ReadValue)
+    if (LoadStatus == ERROR)
     {
-        ThreadLoadStateMachine(LoadStages+1);
+        if ((SecondTry == false)&&(TryAgain == true))
+        {
+            LoadStatus = OK;
+            SecondTry = true;
+            ThreadLoadStateMachine(LoadStages);
+        }
+        else
+        {
+            SecondTry = false;
+            ThreadLoadStateMachine(LoadStages+1);
+        }
     }
     else
     {
@@ -1071,7 +1145,7 @@ uint32_t ThreadLoadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
 
     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_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
     {
@@ -1209,7 +1283,7 @@ void ThreadLoadPollRequest(MessageContainer* requestContainer)
     stub_heating_test_poll_request__free_unpacked(request,NULL);
 }
 char ThreadLoadingToken[36+1] = {0};
-int MessageState = 0; //0 - none, 1 = start, 2 = continue
+char DefaultErrSrt[] = "OK";
 uint32_t ThreadLoadingReport(void)
 {
     MessageContainer responseContainer;
@@ -1219,7 +1293,14 @@ uint32_t ThreadLoadingReport(void)
     if (ThreadLoadingToken[0] == 0)
         return OK;
 
-    if (MessageState == 1)
+    if (MessageState == 0)
+    {
+        response.has_state = true;
+        response.state = THREAD_LOADING_STATE__None;
+        response.errorreason = DefaultErrSrt;
+
+    }
+    else if (MessageState == 1)
     {
         response.has_state = true;
         if (LoadStatus == OK)
@@ -1228,6 +1309,7 @@ uint32_t ThreadLoadingReport(void)
             if (LoadStages == THREAD_LOAD_INITIAL_TENSION)
             {
                 response.state = THREAD_LOADING_STATE__ReadyForLoading;
+                response.errorreason = DefaultErrSrt;
             }
         }
         else
@@ -1242,10 +1324,11 @@ uint32_t ThreadLoadingReport(void)
         if (LoadStatus == OK)
         {
             response.state = THREAD_LOADING_STATE__Finalizing;
-            if (LoadStages >= THREAD_LOAD_JOG_THREAD)
+            if (LoadStages > THREAD_LOAD_JOG_THREAD)
             {
                 response.state = THREAD_LOADING_STATE__Completed;
-                last = true;
+                response.errorreason = DefaultErrSrt;
+                //last = true;
             }
         }
         else
@@ -1273,7 +1356,7 @@ uint32_t StartThreadLoadingFunc(MessageContainer* requestContainer)
 {
     StartThreadLoadingRequest *request = start_thread_loading_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);
     ustrncpy (ThreadLoadingToken, requestContainer->token,36);
-    MessageState = 1;
+    //MessageState = 1;
     Report("StartThreadLoadingFunc",__FILE__,__LINE__,0,RpWarning,(int)LoadStages,0);
     ThreadLoadingReport();
     return OK;
@@ -1301,6 +1384,10 @@ uint32_t ContinueThreadLoadingFunc(MessageContainer* requestContainer)
     size_t container_size = message_container__pack(&responseContainer, container_buffer);
     my_free(responseContainer.data.data);
     SendChars((char*)container_buffer, container_size);
+    if (LoadStages == THREAD_LOAD_INITIAL_TENSION) //on this satge we should wait for user call
+    {
+        ThreadLoadStateMachine(LoadStages);
+    }
     return OK;
 
 }
@@ -1311,7 +1398,7 @@ uint32_t StopThreadLoadingFunc(MessageContainer* requestContainer)
     MessageContainer responseContainer;
     MessageState = 2;
     Thread_Load_End();
-    Report("StopThreadLoadingFunc",__FILE__,__LINE__,(int)11,RpWarning,(int)LoadStages,0);
+    Report("StopThreadLoadingFunc",__FILE__,__LINE__,0xFF,RpWarning,(int)LoadStages,0);
 
     responseContainer = createContainer(MESSAGE_TYPE__StopThreadLoadingResponse, requestContainer->token, true, &Cresponse, &stop_thread_loading_response__pack, &stop_thread_loading_response__get_packed_size);
     responseContainer.continuous = false;
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
index dd31e9cda..0a3a9e149 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
@@ -143,7 +143,7 @@ void ThreadUpdateProcessLength (double length, void *Funcptr)
     CurrentProcessedLength = 0;
     ProcessedLengthFuncPtr = (ProcessedLengthFunc)Funcptr;
 }
-char Lenstr[150];
+char Lenstr[160];
 uint32_t ThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue)
 {
     uint32_t positionDiff = 0,prevprev;
@@ -300,12 +300,12 @@ uint32_t PoolerThreadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue)
     {
         if (PrepareState == true)
         {
-#ifdef Use_Head_Card
-            strcpy(Lenstr,"Heating up");
-#else
+//#ifdef Use_Head_Card
+//            strcpy(Lenstr,"Heating up");
+//#else
             //later - add temperatures
              TemperatureListString(Lenstr);
-#endif
+//#endif
             SendJobProgress(0.0,0,false, Lenstr);
         }
         else
@@ -602,7 +602,7 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
         NormalizedError = avreageSampleValue*NormalizedErrorCoEfficient[index];
         MotorControlConfig[index].m_mesuredParam = NormalizedError;
         DancerError[DancerId] = NormalizedError;
-        MotorControlConfig[index].m_calculatedError = AdvancedPIDAlgorithmCalculation((float)MotorControlConfig[index].m_SetParam , (float)MotorControlConfig[index].m_mesuredParam,
+        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
         {