merge

3 files changed, 82 insertions, 39 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
index 10286853e..4f78d2134 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
@@ -83,6 +83,8 @@ bool watchdogCriticalAlarm = false;
 
 uint32_t msec_millisecondCounter = 0;
 
+extern bool Machine_Idle_Mode;
+
 MillisecMotorDataStruc ScrewSetMaxSpeedPending = {0};
 MillisecMotorDataStruc ScrewMovePending = {0};
 MillisecMotorDataStruc MotorData[NUM_OF_MOTORS] = {0};
@@ -446,7 +448,8 @@ uint32_t MillisecLowLoop(uint32_t tick)
         //Speed_Data = Calculate_Speed_Sensor_Velocity();
         //MillisecReadFromTempSensor(Sensor_Read, NULL);
         //if (Sensor_Read++ >= MAX_TEMPERATURE_SENSOR_ID) Sensor_Read = 0;
-        //Control_LED1_PWM();
+        if(Machine_Idle_Mode == true)
+            Machine_Idle_Breathing_Led();
     }
     if (Hundred_msTick)
     {
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.c b/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.c
index f9aee7929..a443b4785 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.c
@@ -71,3 +71,51 @@ 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;
+    float derivative;
+    float output;
+
+    //double error = *mySetpoint - input;
+    error = _setPoint - _mesuredParam;
+    //ITerm+= (ki * error);
+    *_integral = *_integral + (error*params->Ki);
+    /*  if(ITerm > outMax)
+      {
+          ITerm= outMax;
+      }
+      else if(ITerm < outMin)
+      {
+          ITerm= outMin;
+      } */
+    if(*_integral > params->MAX)
+    {
+        *_integral = params->MAX;
+    }
+    else if(*_integral < params->MIN)
+    {
+        *_integral = params->MIN;
+    }
+    //  double dInput = (input - lastInput);
+    derivative = error - *_pre_error;
+
+      /*Compute PID Output*/
+    //  double output = kp * error + ITerm- kd * dInput;
+    output = params->Kp*error/params->ProportionalErrorMultiplier + *_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/control.c b/Software/Embedded_SW/Embedded/Modules/Control/control.c
index 538e251ef..69820e306 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/control.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/control.c
@@ -51,6 +51,7 @@
 #include <inc/hw_ints.h>
 
 #include "drivers/adc_sampling/adc.h"
+#include "Modules/General/buttons.h"
 
 #include "control.h"
 #include "MillisecTask.h"
@@ -98,7 +99,7 @@ uint32_t                ControlDatalog[MAX_TANGO_CONTROL_DEVICES];
 uint16_t ControlBacklog[MAX_BACKLOG_SIZE]={0};
 uint16_t backlogindex = 0;
 uint32_t        Control_timerBase = TIMER0_BASE;        //Timer handle
-uint32_t MaxHighDevices = 0xFF;
+uint32_t MaxHighDevices = 0;
 /******************** Functions  ********************************************/
 void OneMilliSecondFunction(UArg arg0);
 
@@ -124,7 +125,7 @@ void ControlInit(void)
     ControlRestart = false;
 
     memset(ControlDatalog,0,sizeof(uint32_t)*MAX_TANGO_CONTROL_DEVICES);
-    MaxHighDevices = 0xFF;
+    MaxHighDevices = 0;
     for (Device_i = 0; Device_i < MAX_TANGO_CONTROL_DEVICES; Device_i++)
     {
         ControlArray[Device_i].ControlActive = false;
@@ -157,10 +158,20 @@ void ControlStop(void)
 uint32_t ControlActivityLed( uint32_t Parameter1)
 {
     static bool flag = false;
+    static uint8_t counter;
+    const uint8_t Blink_Freq = 15;//odd number
+
     if (flag==true)
     {
         COMM_RED_LED_ON;
         ACTIVITY_RED_LED_OFF; // Heaters indication - all the Heaters OFF
+        if(power.color == fastBILNK)
+            Pannel_Leds(POWER_ON_OFF,MODE_OFF);
+
+        if((power.color == BLINK) && (counter % Blink_Freq == 0) )
+        {
+            Pannel_Leds(POWER_ON_OFF,MODE_OFF);
+        }
         flag = false;
     }
     else
@@ -168,8 +179,22 @@ uint32_t ControlActivityLed( uint32_t Parameter1)
         COMM_RED_LED_OFF;
         if (HeaterActive > 0)// Blink the led on heating
             ACTIVITY_RED_LED_ON;// Heaters indication - at least one of  the Heaters is ON
+
+        if(power.color == fastBILNK)
+            Pannel_Leds(POWER_ON_OFF,MODE_ON);
+
+        if((power.color == BLINK) && (counter % Blink_Freq == 0) )
+        {
+            Pannel_Leds(POWER_ON_OFF,MODE_ON);
+        }
+
         flag = true;
     }
+
+    if (counter < 0xFF)
+        counter++;
+    else
+        counter = Blink_Freq + 1;
 return OK;
 }
 uint32_t ControlEmptyCBFunction(uint32_t IfIndex, uint32_t ReadValue)
@@ -218,13 +243,8 @@ uint32_t  AddControlCallback( ControlCBFunction Callback, CTRL_TIMING_ENUM CtrlF
                 break;
             }
         }
-/*        if (MaxHighDevices == 0xFF)
-            MaxHighDevices = deviceId;
-        else
-        {
-      	  	if ((deviceId!=0xFF )&&(deviceId> MaxHighDevices))
-     	       	MaxHighDevices = deviceId;
-        }
+    /*    if ((deviceId!=0xFF )&&(deviceId> MaxHighDevices))
+    //        MaxHighDevices = deviceId;
     }
     else
     {
@@ -339,23 +359,7 @@ uint32_t GetControlLowDevice_i(void)
 }
 uint32_t ControlLoop(uint32_t tick)
 {
-    //call all modules control functions
-    //test dancers and speed encoders
-    //check all callback units (state machine waiting for completion of a change)
-    //uint32_t ControlDevice_i;
-    /*bool Ten_msTick, Hundred_msTick, Onesecond_Tick,Tick98,Tick998;
-    Ten_msTick      =   (tick%eTenMillisecond == 0)      ?true:false;
-    Hundred_msTick  =   (tick%eHundredMillisecond == 0)  ?true:false;
-//    Hundred_msTick  =   (tick%200 == 0)  ?true:false;
-    Onesecond_Tick  =   (tick%eOneSecond == 0)           ?true:false;
-    Tick98          =   (tick%eHundredMillisecond == 98) ?true:false;
-//    Tick98          =   (tick%200 == 199) ?true:false;
-    Tick998         =   (tick%eOneSecond == 996)         ?true:false;
-*/
-    //ROM_IntMasterDisable();
-    //if (MaxHighDevices == 0xFF)
-    //    return OK;
-    //for (ControlDevice_i = 0; ControlDevice_i <= MaxHighDevices;ControlDevice_i++)
+    //for (ControlDevice_i = 0; ControlDevice_i < MaxHighDevices;ControlDevice_i++)
     for (ControlDevice_i = 0; ControlDevice_i < MAX_TANGO_CONTROL_DEVICES;ControlDevice_i++)
     {
         if (ControlArray[ControlDevice_i].ControlActive)
@@ -386,18 +390,6 @@ uint32_t ControlLoop(uint32_t tick)
 }
 uint32_t ControlLowLoop(uint32_t tick)
 {
-    //call all modules control functions
-    //test dancers and speed encoders
-    //check all callback units (state machine waiting for completion of a change)
-    //uint32_t Device_i;
-    //bool Ten_msTick, Hundred_msTick, Onesecond_Tick,Tick98,Tick998;
-    //Ten_msTick      =   (tick%eTenMillisecond == 0)      ?true:false;
-    //Hundred_msTick  =   (tick%eHundredMillisecond == 0)  ?true:false;
-    //Onesecond_Tick  =   (tick%eOneSecond == 0)           ?true:false;
-    //Tick98          =   (tick%eHundredMillisecond == 98) ?true:false;
-    //Tick998         =   (tick%eOneSecond == 996)         ?true:false;
-
-    //ROM_IntMasterDisable();
     for (ControlLowDevice_i = 0; ControlLowDevice_i < MAX_TANGO_CONTROL_DEVICES;ControlLowDevice_i++)
     {
         if (ControlArray[ControlLowDevice_i].ControlActive)