merge conflicts

13 files changed, 171 insertions, 41 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.c b/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.c
index d1a7b0145..fc2fa7625 100644
--- a/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.c
+++ b/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.c
@@ -26,6 +26,7 @@
 #include "PMR/Hardware/HardwareDancerType.pb-c.h"
 
 #include "modules/thread/thread_ex.h"
+#include "modules/heaters/heaters_ex.h"
 #include "modules/ids/ids_ex.h"
 #include  <PMR/Diagnostics/Event.pb-c.h>
 #include  <PMR/Diagnostics/EventType.pb-c.h>
@@ -269,6 +270,9 @@ uint32_t AlarmHandlingConsequentActions(uint32_t AlarmId, DebugLogCategory Sever
 
         //raise flag fr next job
         break;
+    case DEBUG_LOG_CATEGORY__Critical:
+        watchdogCriticalAlarm = true;
+        //intentional fall through
     case DEBUG_LOG_CATEGORY__Error:
         if (JobIsActive())
         {
@@ -278,17 +282,6 @@ uint32_t AlarmHandlingConsequentActions(uint32_t AlarmId, DebugLogCategory Sever
         }
 //Stop Job
         break;
-    case DEBUG_LOG_CATEGORY__Critical:
-        if (JobIsActive())
-        {
-            JobEndReason = JOB_OTHER_ALARM;
-            AbortJob(AlarmItem[AlarmId].EventName);
-            Report(AlarmItem[AlarmId].EventName, __FILE__,__LINE__,AlarmId, RpMessage, DEBUG_LOG_CATEGORY__Critical, 0);
-        }
-        watchdogCriticalAlarm = true;
-        //stop job
-        //turn machine off
-        break;
     case DEBUG_LOG_CATEGORY__Debug:
     case DEBUG_LOG_CATEGORY__Info:
     default:
@@ -298,6 +291,83 @@ uint32_t AlarmHandlingConsequentActions(uint32_t AlarmId, DebugLogCategory Sever
     }
     return OK;
 }
+uint32_t AlarmHandlingPrepareJob(void *CurrentJob)
+{
+    JobTicket* JobTicket = CurrentJob;
+    bool DispenserInUse[MAX_SYSTEM_DISPENSERS] = {false,false,false,false,false,false,false,false};
+    EventType HeaterEventType[MAX_HEATERS_NUM] = {EVENT_TYPE__DryerOverTemperature,EVENT_TYPE__DryerOverTemperature,EVENT_TYPE__DryerOverTemperature,EVENT_TYPE__DyeingHead1OverTemperature,EVENT_TYPE__DyeingHead2OverTemperature,
+                                                  EVENT_TYPE__DyeingHead3OverTemperature,EVENT_TYPE__DyeingHead4OverTemperature,EVENT_TYPE__DyeingHead5OverTemperature,EVENT_TYPE__DyeingHead6OverTemperature,
+                                                  EVENT_TYPE__GeneralInternalOverTemperature};
+    int Segment_i,Brush_i,Dispenser_i,DispenserId,Alarm_i,Heater_i,AlarmId=0;
+    HeaterState HeaterState;
+
+    uint32_t status = OK;
+    if (JobTicket->n_segments == 0)
+        return OK;
+    for (Segment_i=0;Segment_i<JobTicket->n_segments;Segment_i++)
+        {
+        for (Brush_i=0;Brush_i<JobTicket->segments[Segment_i]->n_brushstops;Brush_i++)
+            {
+                if (JobTicket->segments[Segment_i]->brushstops[Brush_i]->n_dispensers)
+                {
+                    for (Dispenser_i = 0;Dispenser_i < JobTicket->segments[Segment_i]->brushstops[Brush_i]->n_dispensers;Dispenser_i++)
+                    {
+                        //prepare the SW structures
+                        DispenserId = JobTicket->segments[Segment_i]->brushstops[Brush_i]->dispensers[Dispenser_i]->index;
+                        if (JobTicket->segments[Segment_i]->brushstops[Brush_i]->dispensers[Dispenser_i]->nanolitterpersecond>0.0)
+                        {
+                            DispenserInUse[DispenserId] = true;
+                        }
+                    }//for dispenser
+                }//if dispensers
+            }//for brush
+        }//for segments
+    for (Dispenser_i=0;Dispenser_i<MAX_SYSTEM_DISPENSERS;Dispenser_i++)
+    {
+        if (DispenserInUse[Dispenser_i] == true)
+        {
+            for (Alarm_i = 0;Alarm_i<MAX_SYSTEM_ALARMS;Alarm_i++)
+            {
+                if (AlarmItem[Alarm_i].EventType == (EVENT_TYPE__Dispenser1Empty+Dispenser_i))
+                {
+                    if (AlarmItem[Alarm_i].Status == true)
+                    {
+                        status = ERROR;
+                        AlarmId = Alarm_i;
+                        break;
+                    }
+                }
+            }
+        }
+    }
+    for (Heater_i = 0;Heater_i<MAX_HEATERS_NUM;Heater_i++)
+    {
+        LoadHeaterState((HeaterType)Heater_i,&HeaterState);
+        if (HeaterState.setpoint)//temperature is set - heater active
+        {
+            for (Alarm_i = 0;Alarm_i<MAX_SYSTEM_ALARMS;Alarm_i++)
+            {
+                if (AlarmItem[Alarm_i].EventType == HeaterEventType[Heater_i])
+                {
+                    if (AlarmItem[Alarm_i].Status == true)
+                    {
+                        status = ERROR;
+                        AlarmId = Alarm_i;
+                        break;
+                    }
+                }
+            }
+        }
+    }
+    if (status == ERROR)
+    {
+        JobEndReason = getEndReason(AlarmId);
+        AbortJob(AlarmItem[AlarmId].EventName);
+        Report(AlarmItem[AlarmId].EventName, __FILE__,__LINE__,AlarmId, RpMessage, DEBUG_LOG_CATEGORY__Error, 0);
+    }
+    return status;
+}
+
 /*
 uint32_t AlarmHandling_CalculateTemperatures(uint32_t SensorId, uint32_t Data)
 {
@@ -404,7 +474,7 @@ uint32_t AlarmHandlingLoop(uint32_t tick)
             switch (AlarmItem[Alarm_i].AlarmSource)
             {
             case TemperatureAlarm:
-                ivalue = TemperatureSensorRead(AlarmItem[Alarm_i].DeviceId);
+                ivalue = TemperatureSensorRead((TEMPERATURE_SENSOR_ID_ENUM)AlarmItem[Alarm_i].DeviceId);
                 if (AlarmItem[Alarm_i].AlarmDirection == true)
                 {
                     if (ivalue/100 >= AlarmItem[Alarm_i].AlarmValue)
@@ -423,7 +493,7 @@ uint32_t AlarmHandlingLoop(uint32_t tick)
                 }
                 break;
             case LimitSwitchAlarm:
-                value = IDS_CheckDispenserLimitSwitch(AlarmItem[Alarm_i].DeviceId);
+                value = IDS_CheckDispenserLimitSwitch((LimitSwitchAlarms)AlarmItem[Alarm_i].DeviceId);
                 if (value == AlarmItem[Alarm_i].AlarmValue)
                 {
                     Status = true;
@@ -449,12 +519,12 @@ uint32_t AlarmHandlingLoop(uint32_t tick)
             case CurrentAlarm:
                 break;
             case MotorAlarm:
-                if (isMotorConfigured(AlarmItem[Alarm_i].DeviceId) == false)
+                if (isMotorConfigured((TimerMotors_t)AlarmItem[Alarm_i].DeviceId) == false)
                 {
                     Status = false;
                     break;
                 }
-                value = MotorGetStatus(AlarmItem[Alarm_i].DeviceId);
+                value = MotorGetStatus((TimerMotors_t)AlarmItem[Alarm_i].DeviceId);
                 if (AlarmItem[Alarm_i].AlarmDirection == true)
                 {
                     if (value && AlarmItem[Alarm_i].AlarmValue)
@@ -481,25 +551,31 @@ uint32_t AlarmHandlingLoop(uint32_t tick)
                 if (Status == true) //increase counter
                 {
                     AlarmItem[Alarm_i].DebounceCounter++;
-                    if (AlarmItem[Alarm_i].DebounceCounter < AlarmItem[Alarm_i].DebounceValue)
+                    if (AlarmItem[Alarm_i].Status == false) // alarm is not set yet
                     {
-                        Status = false;
+                        if (AlarmItem[Alarm_i].DebounceCounter < AlarmItem[Alarm_i].DebounceValue) //had not reached the debounce value
+                        {
+                            Status = false; //do not set the alarm
+                        } //else alarm will be set
                     }
-                    else
+                    else // alarm is already set
                     {
-                        AlarmItem[Alarm_i].DebounceCounter = AlarmItem[Alarm_i].DebounceValue;
+                        AlarmItem[Alarm_i].DebounceCounter = AlarmItem[Alarm_i].DebounceValue; // do not go over the debounce value
                     }
                 }
                 else //status  == false - decrease counter
                 {
                     AlarmItem[Alarm_i].DebounceCounter--;
-                    if (AlarmItem[Alarm_i].DebounceCounter > 0)
+                    if (AlarmItem[Alarm_i].Status == true) // alarm is set
                     {
-                        Status = true;
+                        if (AlarmItem[Alarm_i].DebounceCounter > 0) // had not reached zero yet
+                        {
+                            Status = true; // do not reset the alarm yet
+                        }   // else reset the alarm
                     }
-                    else
+                    else // if the alarm is off
                     {
-                        AlarmItem[Alarm_i].DebounceCounter = 0;
+                        AlarmItem[Alarm_i].DebounceCounter = 0; //do not go below 0
                     }
                 }
             }
diff --git a/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.h b/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.h
index 4a11276a1..edce0d9cb 100644
--- a/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.h
+++ b/Software/Embedded_SW/Embedded/Modules/AlarmHandling/AlarmHandling.h
@@ -15,6 +15,7 @@ uint32_t AlarmHandlingStart(void);
 uint32_t AlarmHandlingStop(void);
 void AlarmHandlingSetAlarm(uint32_t AlarmId, bool Value);
 
+uint32_t AlarmHandlingPrepareJob(void *CurrentJob);
 uint32_t StartEventsNotificationRequestFunc(MessageContainer* requestContainer);
 uint32_t StopEventsNotificationRequestFunc(MessageContainer* requestContainer);
 uint32_t ResolveEventRequestFunc(MessageContainer* requestContainer);
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
index bee00c049..68bd680a8 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
@@ -523,7 +523,7 @@ uint32_t MillisecLoop(uint32_t tick)
     }
     //ROM_IntMasterEnable();
 
-    //loop_Run_Read_Speed(); - just for testing
+
     return OK;
 }
 /******************************************************************************
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.c b/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.c
index f37759943..72774461a 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.c
@@ -36,4 +36,40 @@ float PIDAlgorithmCalculation(float _setPoint,float _mesuredParam , PID_Config_P
 	return output;
 }
 
+float AdvancedPIDAlgorithmCalculation(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(abs(error) > params->epsilon)
+    {
+        *_integral = *_integral + error*params->dt;
+    }
+
+    derivative = (error - *_pre_error)/params->dt;
+    float IntegralErrorMultiplier;
+    float ProportionalErrorMultiplier;
+
+    output = params->Kp*error/params->ProportionalErrorMultiplier + params->Ki**_integral/params->IntegralErrorMultiplier + 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;
+}
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.h b/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.h
index 03c363131..4792be9ec 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.h
+++ b/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.h
@@ -12,7 +12,11 @@ typedef struct
     float Kp;
     float Kd;
     float Ki;
+    float IntegralErrorMultiplier;
+    float ProportionalErrorMultiplier;
+
 }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/General/GeneralHardware.c b/Software/Embedded_SW/Embedded/Modules/General/GeneralHardware.c
index a0ac11d1f..4ad9af903 100644
--- a/Software/Embedded_SW/Embedded/Modules/General/GeneralHardware.c
+++ b/Software/Embedded_SW/Embedded/Modules/General/GeneralHardware.c
@@ -108,6 +108,7 @@ uint32_t HWConfiguration(UploadHardwareConfigurationRequest* UploadRequest)
     {
         BlowerCfg.enabled = true;
         BlowerCfg.voltage = request->blowers[0]->voltage;
+        BlowerCfg.heatingvoltage = request->blowers[0]->heatingvoltage;
         /*Turn_the_Blower_On();//Turn on with the Default_Voltage
         if (request->blowers[0]->voltage)
             Control_Voltage_To_Blower(request->blowers[0]->voltage);
diff --git a/Software/Embedded_SW/Embedded/Modules/General/process.c b/Software/Embedded_SW/Embedded/Modules/General/process.c
index 1ab1365d0..96248ab8b 100644
--- a/Software/Embedded_SW/Embedded/Modules/General/process.c
+++ b/Software/Embedded_SW/Embedded/Modules/General/process.c
@@ -28,7 +28,7 @@ int32_t tableindex = 0;
 
 #define MAX_ALLOWED_TEMPERATURE 280
 
-ProcessParameters* ProcessParametersKeep;
+ProcessParameters ProcessParametersKeep;
 
 uint32_t HandleProcessParameters(ProcessParameters* ProcessParams)
 {
@@ -47,7 +47,7 @@ uint32_t HandleProcessParameters(ProcessParameters* ProcessParams)
         return status;
     if (ProcessParams)
     {
-        ProcessParametersKeep = ProcessParams;
+        memcpy (&ProcessParametersKeep,ProcessParams,sizeof(ProcessParameters));
     }
     if (ProcessParams->mixertemp)
         status |= HeaterCommandRequestMessage(
diff --git a/Software/Embedded_SW/Embedded/Modules/General/process.h b/Software/Embedded_SW/Embedded/Modules/General/process.h
index 520b5e626..12f1f8461 100644
--- a/Software/Embedded_SW/Embedded/Modules/General/process.h
+++ b/Software/Embedded_SW/Embedded/Modules/General/process.h
@@ -19,7 +19,7 @@ extern double headairflow;
 extern double dryerairflow;
 extern int32_t tableindex;
 
-extern ProcessParameters* ProcessParametersKeep;
+extern ProcessParameters ProcessParametersKeep;
 
 extern void ProcessRequestFunc(MessageContainer* requestContainer);
 extern uint32_t HandleProcessParameters(ProcessParameters* ProcessParams);
diff --git a/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_init.c b/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_init.c
index d1febd3db..169ae463e 100644
--- a/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_init.c
+++ b/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_init.c
@@ -284,10 +284,11 @@ uint32_t HeaterRecalculateHeaterParams(uint32_t deviceId, uint32_t new_outputpro
    }
 
    // all numbers are rounded down. better to have carefully calculated numbers
-   HeaterControl[deviceId].outputproportionalpowerlimit = new_outputproportionalpowerlimit;
+   if (new_outputproportionalpowerlimit > HeaterControl[deviceId].outputproportionalpowerlimit)
+       new_outputproportionalpowerlimit = HeaterControl[deviceId].outputproportionalpowerlimit;
 
    //mark the time slices  for heaters operation as empty / Heater1000 / Heater 200
-   DCTimeSliceAllocation[deviceId] = (HeaterControl[deviceId].outputproportionalpowerlimit/*/100*/ * NumberOFSlicesInUse) / 100;
+   DCTimeSliceAllocation[deviceId] = (new_outputproportionalpowerlimit/*/100*/ * NumberOFSlicesInUse) / 100;
 
 
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c b/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c
index aa3d695de..4f8edde02 100644
--- a/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Heaters/Heaters_print.c
@@ -145,7 +145,9 @@ void LoadHeaterState(HeaterType HeaterType,HeaterState *HeaterState)
     HeaterState->has_heatertype = true;
     HeaterState->heatertype = HeaterType;
     HeaterState->has_setpoint = true;
-    HeaterState->setpoint = HeaterCmd[HeaterId].targettemperatue/100;
+  //  HeaterState->setpoint = HeaterCmd[HeaterId].targettemperatue/100;
+  //  if (HeaterType >= HEATER_TYPE__HeaterZone1)
+        HeaterState->setpoint = DCTimeSliceAllocation[HeaterId]*100/NumberOFSlicesInUse;
     HeaterState->has_currentvalue = true;
     HeaterState->currentvalue = TemperatureSensorRead(HeaterId2PT100Id[HeaterId])/100;
     HeaterState->has_isactive = true;
@@ -359,7 +361,9 @@ uint32_t PrepareHeater(int HeaterId, uint32_t SetTemperatue)
         if (BlowerCfg.enabled == true)
         {
             Turn_the_Blower_On();//Turn on with the Default_Voltage
-            if (BlowerCfg.voltage)
+            if (BlowerCfg.heatingvoltage)
+                Control_Voltage_To_Blower(BlowerCfg.heatingvoltage);
+            else
                 Control_Voltage_To_Blower(BlowerCfg.voltage-500);
 
         }
@@ -653,7 +657,7 @@ uint32_t DCHeaterControlCBFunction(uint32_t IfIndex, uint32_t readValue)
             //        Report(logmsg[index],__FILE__,__LINE__,index,RpWarning,index,  Counter[index]);
             //        #warning PID is now only proportional (above)
             if (printindex==index)
-                Report(heatstr,__FILE__,__LINE__,index,RpWarning,readValue,  HeaterPIDConfig[index].m_calculatedError);
+                Report(heatstr,__FILE__,__LINE__,index,RpWarning,HeaterPIDConfig[index].m_calculatedError,0);
             HeaterRecalculateHeaterParams(index, (int)(HeaterPIDConfig[index].m_calculatedError/100));
 
         }
diff --git a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c
index f9c4c1bc3..958f481bf 100644
--- a/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/IDS/IDS_print.c
@@ -379,6 +379,7 @@ uint32_t IDSPreSegmentState(void *JobDetails, int SegmentId)
      int Dispenser_i;
      TimerMotors_t HW_Motor_Id;
 
+     Valve_Set(VALVE_MIXCHIP_WASTECH, Mixer_Waste);  //#bug 323
          for (Dispenser_i = 0;Dispenser_i < MAX_SYSTEM_DISPENSERS;Dispenser_i++)
          {
              HW_Motor_Id = DispenserIdToMotorId[Dispenser_i];
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
index b6392249a..c7d303f9a 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_Winder.c
@@ -219,7 +219,9 @@ uint32_t Screw100msecDirectionChange(uint32_t deviceID, uint32_t BusyFlag)
             winderspeed+=WinderMotorSpeed[i];
         }
         winderspeed/=MAX_WINDER_SPEED_CALCULATION;
-        LOG_ERROR(winderspeed, "WinderSpeedUpdated");
+        //LOG_ERROR(winderspeed, "WinderSpeedUpdated");
+        Report("WinderSpeedUpdated",__FILE__,__LINE__,winderspeed,RpWarning,ScrewNumberOfSteps,0);
+
         WinderReferenceSpeed = winderspeed;
     }
     screw_horizontal_speed = ScrewNumberOfSteps /  InternalWinderCfg.NumberOfRotationPerPassage;
@@ -231,7 +233,8 @@ uint32_t Screw100msecDirectionChange(uint32_t deviceID, uint32_t BusyFlag)
     temp /= ScrewSpeed;
     if (ScrewRunningTime != temp)
     {
-        LOG_ERROR(temp , "new winder speed");
+        //LOG_ERROR(temp , "new winder speed");
+        Report("new winder speed",__FILE__,__LINE__,temp,RpWarning,ScrewSpeed,0);
     }
     ScrewRunningTime = temp;//(SYS_CLK_FREQ*Steps)/ScrewSpeed;
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
index 5390ed0a7..6db7f616e 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
@@ -319,7 +319,7 @@ uint32_t ThreadControlSpeedReadFunction(uint32_t IfIndex, uint32_t ReadValue)
 #define MAX_THREAD_CONTROL_LOG 300
 double calculatedError[MAX_THREAD_CONTROL_LOG+1];
 double NormError[MAX_THREAD_CONTROL_LOG+1];
-double Integral[MAX_THREAD_CONTROL_LOG+1];
+double mIntegral[MAX_THREAD_CONTROL_LOG+1];
 int    MotorId[MAX_THREAD_CONTROL_LOG+1];
 int    readValue[MAX_THREAD_CONTROL_LOG+1];
 int    AveragereadValue[MAX_THREAD_CONTROL_LOG+1];
@@ -452,13 +452,13 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
             //EndState(CurrentJob,Message );
             SegmentReady(Module_Thread,ModuleFail);
             AlarmHandlingSetAlarm(EVENT_TYPE__ThreadTensionControlFailure,true);
-            LOG_ERROR (index, "Dancer Failure");
+            LOG_ERROR (DancerId, "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_calculatedError = /*Advanced*/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
         {
@@ -478,9 +478,10 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
                     readValue[controlIndex] = ReadValue;
                     AveragereadValue[controlIndex] = avreageSampleValue;
                     calculatedspeed[controlIndex] = calculated_speed;
+                    NormError[controlIndex]
+                                    = MotorControlConfig[index].m_mesuredParam;
+                    mIntegral[controlIndex] = MotorControlConfig[index].m_integral;
                     timestamp[controlIndex] = msec_millisecondCounter;
-                    NormError[controlIndex]= NormalizedError;
-                    Integral[controlIndex] = MotorControlConfig[index].m_integral;
                  if (controlIndex++>=MAX_THREAD_CONTROL_LOG)
                      controlIndex = 0;
             }
@@ -555,8 +556,10 @@ uint32_t ThreadEmptyCBFunction(uint32_t IfIndex, uint32_t ReadValue)
             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_params.IntegralErrorMultiplier = MotorsControl[Pid_Id].setpointramprateorsoftstartramp;
+            MotorControlConfig[Motor_i].m_params.ProportionalErrorMultiplier = MotorsControl[Pid_Id].outputonoffhysteresisvalue;
+            MotorControlConfig[Motor_i].m_params.epsilon = MotorsControl[Pid_Id].epsilon;
+            MotorControlConfig[Motor_i].m_params.dt = MotorsControl[Pid_Id].controloutputtype;
             MotorControlConfig[Motor_i].m_calculatedError = 0;
             MotorControlConfig[Motor_i].m_integral = 0;
             MotorControlConfig[Motor_i].m_isEnabled = true;