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

5 files changed, 58 insertions, 10 deletions

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/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/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
index 5390ed0a7..15ea5157b 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 = 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
         {
@@ -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;