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

5 files changed, 142 insertions, 28 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
index fab0831fe..689219583 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
@@ -43,10 +43,10 @@
 #include "modules/thread/thread_ex.h"
 #include "modules/ifs/ifs.h"
 #include "modules/ids/ids_ex.h"
+#include "modules/waste/waste_ex.h"
 #include "modules/Diagnostics/Diagnostics.h"
 #include "Modules/General/MachineStatus.h"
 
-#include "drivers/Flash_Memory/Flash_Memory.h"
 #include "drivers/Flash_Memory/fatfs/ff.h"
 #include "drivers/Flash_ram/FlashProgram.h"
 
@@ -62,6 +62,16 @@
 #include "Modules/Heaters/Heaters_ex.h"
 #include <Drivers/I2C_Communication/WHS_Card/D_EMC2302_Fan/WHS_Fan.h>
 #include <Drivers/I2C_Communication/WHS_Card/D_MAX11614_ADC/WHS_MAX11614_A2D.h>
+#include <Drivers/I2C_Communication/I2C_Task.h>
+
+#include "drivers/Uart_Comm/WHS_Controller_Comm/WHS_UART/WHS_Uart.h"
+#include "Drivers/Uart_Comm/WHS_Controller_Comm/Shinko/ACS-13AC5E3.h"
+
+#include <Drivers/I2C_Communication/RFID_NFC/NFC_MainBaord.h>
+#include <Drivers/I2C_Communication/RFID_NFC/logi-tag/LT_RFID.h>
+#include <Drivers/I2C_Communication/RFID_NFC/RFIDTagInfo.h>
+#include <Drivers/I2C_Communication/RFID_NFC/I2C_IFS_Mux.h>
+#include <Drivers/I2C_Communication/RFID_NFC/NFC.h>
 
 Task_Handle Millisecond_Task_Handle;
 /******************** Definitions  ********************************************/
@@ -490,9 +500,23 @@ uint32_t MillisecLoop(uint32_t tick)
 #ifdef HUNDRED_MICROSECONDS_DANCER_READ
    SaveLogData();
 #else
-    Dancer_Data[FEEDER_DANCER] = Read_Dancer_Position(FEEDER_DANCER);
-    Dancer_Data[POOLER_DANCER] = Read_Dancer_Position(POOLER_DANCER);
-    Dancer_Data[WINDER_DANCER] = Read_Dancer_Position(WINDER_DANCER);
+
+    #ifdef test_RTFU_dancer
+       test_dancer_responce_RTFU();
+    #else
+#ifdef FOUR_WINDERS
+        Dancer_Data[HARDWARE_DANCER_0] = Read_Dancer_Position(HARDWARE_DANCER_0);
+        Dancer_Data[HARDWARE_DANCER_1] = Read_Dancer_Position(HARDWARE_DANCER_1);
+        Dancer_Data[HARDWARE_DANCER_2] = Read_Dancer_Position(HARDWARE_DANCER_2);
+        Dancer_Data[HARDWARE_DANCER_3] = Read_Dancer_Position(HARDWARE_DANCER_3);
+        Dancer_Data[HARDWARE_DANCER_4] = Read_Dancer_Position(HARDWARE_DANCER_4);
+#else
+        Dancer_Data[FEEDER_DANCER] = Read_Dancer_Position(FEEDER_DANCER);
+        Dancer_Data[POOLER_DANCER] = Read_Dancer_Position(POOLER_DANCER);
+        Dancer_Data[WINDER_DANCER] = Read_Dancer_Position(WINDER_DANCER);
+#endif
+    #endif
+
 #endif
     return OK;
 }
@@ -564,25 +588,29 @@ float MillisecGetPressures (int SensorId)
     return PressureCalc[SensorId];
 }
 
+bool getRapidPressureRead(void)
+{
+    return RapidPressureRead;
+}
 void setRapidPressureRead(bool value)
 {
     RapidPressureRead = value;
     if (GetDiagnosticMode() == Diagnostic_Extreme_Mode)
         RapidPressureRead = true;
 }
-
 uint16_t PumpCounter = 0;
 uint16_t realtimetest[101];
 uint32_t MillisecLowLoop(uint32_t tick)
 {
     uint8_t Motor_i,Disp_i,temp;
     TEMPERATURE_SENSOR_ID_ENUM Sensor_i;
+    RFID_READER_ID readerID;
 
     //static int temp=0;
     //call all modules Millisec functions
     //test dancers and speed encoders
     //check all callback units (state machine waiting for completion of a change)
-    bool Ten_msTick, Fifty_msTick, Hundred_msTick , m20msecTick,m70msecTick,m90msecTick, Onesecond_Tick,Tensecond_Tick,OneMinute_Tick,OneHourTick,Gradient_Tick;
+    bool Ten_msTick, Fifty_msTick, Hundred_msTick , m20msecTick,m70msecTick,m90msecTick, Onesecond_Tick,Tensecond_Tick,OneMinute_Tick,TenMinutes_Tick,OneHourTick,Gradient_Tick;
     bool O700Millisecond_Tick,O200Millisecond_Tick,O400Millisecond_Tick,O500Millisecond_Tick,O600Millisecond_Tick;
     //bool O100Millisecond_Tick,O200Millisecond_Tick,O400Millisecond_Tick,O500Millisecond_Tick,O600Millisecond_Tick,O800Millisecond_Tick,O900Millisecond_Tick;
     Ten_msTick      =   (tick%eTenMillisecond == 0)      ?true:false;
@@ -602,23 +630,29 @@ uint32_t MillisecLowLoop(uint32_t tick)
     Onesecond_Tick  =   (tick%eOneSecond == 0)           ?true:false;
     Tensecond_Tick  =   (tick%10000 == 0)           ?true:false;
     OneMinute_Tick  =   (tick%eOneMinute == 0)           ?true:false;
+    TenMinutes_Tick  =   (tick%eTenMinutes == 0)           ?true:false;
     OneHourTick  =   (tick%eOneHour == 0)           ?true:false;
     realtimetest[(tick%1000)/10]++;
     //gather Motor data from FPGA
     //ROM_IntMasterDisable();
     int StartPT100 = 0;
     //Screw_ENC_Velocity_to_DAC(); - for testing the screw enc
-    if (Head_Type > HEAD_TYPE_SYLKO_WITHOUT_CARD)
+    if (Head_Type > HEAD_TYPE_FLAT_WITHOUT_CARD)
         StartPT100 = TEMP_SENSE_ANALOG_DRYER_TEMP1;
+
     if (Ten_msTick)
     {
-        Speed_Data = Read_Speed_Sensor_TypeII();
+        //Speed_Data = Read_Speed_Sensor_TypeII();
         //MillisecReadFromTempSensor(Sensor_Read, NULL);
         //if (Sensor_Read++ >= MAX_MAIN_CARD_TEMPERATURE_SENSOR_ID) Sensor_Read = 0;
         if(Machine_Idle_Mode == true)
             Machine_Idle_Breathing_Led();
         Trigger_HeaterWriting();
-
+        Read_Dryer_ENC_Position();
+    }
+    if (m20msecTick)
+    {
+        ADC0SS0Handler();
     }
     if(Fifty_msTick)
     {
@@ -643,7 +677,6 @@ uint32_t MillisecLowLoop(uint32_t tick)
 #ifndef EVALUATION_BOARD
         Read_Buttons_Reg();
 #endif
-        //Ink_Cart_Led();
         ///////////////////////////////////////////////////////////////////
 
 
@@ -669,6 +702,18 @@ uint32_t MillisecLowLoop(uint32_t tick)
         }
 
         Trigger_InputsReading();
+
+        #ifdef USE_RFID
+            //every 100 m sec
+            for(readerID = READER_1; readerID < Max_Readers ; readerID++)
+            {
+                if(DiscoverRFIDTagEvery100mSec[readerID] == true)
+                {
+                    RFIDCallEvery100mSec(readerID);
+                    break;
+                }
+            }
+        #endif
     }
     if (Gradient_Tick)
         DispensersCollectionCall();
@@ -714,9 +759,8 @@ uint32_t MillisecLowLoop(uint32_t tick)
     {
         //char Lenstr[160];
         //static int Counter = 0;
-        MachineUpdateResponseFunc();
-        //KeepAliveOneSecondCall();
-
+        //MachineUpdateResponseFunc();
+        KeepAliveOneSecondCall();
         //TemperatureListString(Lenstr);
         //ReportWithPackageFilter(ThreadFilter,Lenstr,__FILE__,__LINE__,(int)Counter++,RpWarning,(int) msec_millisecondCounter,0);
         for (Sensor_i = StartPT100;Sensor_i < MAX_HEAD_CARD_TEMP_SENS_ID;Sensor_i++)
@@ -734,9 +778,15 @@ uint32_t MillisecLowLoop(uint32_t tick)
         {
             //Trigger_WHS_PT100_Read_All();
             WHS_Blower_Avarege(HEAD_FLOW_METER);
+            WHS_Blower_Avarege(NU_FLOW_METER);
             WHS_Blower_Avarege(DRIER_FLOW_METER);
             WHS_Start_Blower_Control_Closed_Loop ();
 
+            if (ReadingVocEverySec == true)//eOneSecond
+            {
+                Trigger_WHS_MAX11614_Read_Gas_Sensor();
+            }
+
            /* static uint8_t Whs_emptying_cycle = 0;
 
             // #warning TBD need to define the timing
@@ -750,19 +800,32 @@ uint32_t MillisecLowLoop(uint32_t tick)
                 Whs_emptying_cycle++;
             }
             */
+
+            if(Shinko_first_read < 2)//read the two steps once and than in cycle every 10 minutes, also will be used to read after updating the temperature
+            {
+                Shinko_Cycle_Comm();
+            }
+        }
+        if (Head_Type == HEAD_TYPE_ARC) {
+            HeadBlowersControlLoop();
         }
+
+        //call waste state machine
+        Waste_StateMachine_OneSecond_Call();
+
+        //call IFS state machine
+        midTankStateMachine();
+
+        #ifdef USE_RFID
+                RFIDCycleEvery1Sec();
+        #endif
     }
     if (Tensecond_Tick)
     {
-        //Trigger_MidTank_Pressure_Read();
-        for (Disp_i = 0;Disp_i < MAX_SYSTEM_DISPENSERS;Disp_i++)
-        {
-            Read_MidTank_Pressure_Sensor(Disp_i);
-        }
     }
     if (OneMinute_Tick)
     {
-//        MachineUpdateResponseFunc();
+        MachineUpdateResponseFunc();
         /*        for (Motor_i = 0;Motor_i < NUM_OF_MOTORS;Motor_i++)
         {
             if (Motor_i == HARDWARE_MOTOR_TYPE__MOTO_SCREW)
@@ -773,14 +836,35 @@ uint32_t MillisecLowLoop(uint32_t tick)
         midtankDisplay = 1-midtankDisplay;
         /*if (WHS_Type == WHS_TYPE_UNKNOWN)
             Gas_PPM_Info = Calculate_Gas_Power_Consumption();*/
-        if (WHS_Type == WHS_TYPE_NEW)
-            waste_seq_step1();// include 1Sec delay
 //            ReportWithPackageFilter(ThreadFilter,"waste tank calculate level",__FILE__,__LINE__,(int)(GetWHSWasteTankLevelMiliLiter()*1000),RpWarning,(int) msec_millisecondCounter,0);
         //Trigger_WHS_MAX11614_Read_allADC();
+        if ((WHS_Type == WHS_TYPE_NEW) && (ReadingVocEverySec == false))//OneMinute_Tick
+        {
+            Trigger_WHS_MAX11614_Read_Gas_Sensor();
+        }
 #ifdef CONTROL_DEBUG
         ResetControlTime();
 #endif
     }
+    if (TenMinutes_Tick)
+    {
+        /*if (WHS_Type == WHS_TYPE_NEW)
+        {
+            waste_seq_step1();// include 1Sec delay           <- to open !!!!
+        }*/
+        if (WHS_Type == WHS_TYPE_NEW)
+        {
+            Shinko_Cycle_Comm();
+        }
+#ifndef RUN_AS_MAIN_JIG
+        //Trigger_MidTank_Pressure_Read();
+        for (Disp_i = 0;Disp_i < MAX_SYSTEM_DISPENSERS;Disp_i++)
+        {
+            Read_MidTank_Pressure_Sensor(Disp_i);
+        }
+#endif
+
+    }
     if (OneHourTick)
     {
 #define PUMP_LIMIT 4
@@ -791,6 +875,11 @@ uint32_t MillisecLowLoop(uint32_t tick)
             PumpCounter = 0;
         }
         MidTankReading();
+        if (WHS_Type == WHS_TYPE_NEW)
+        {
+            waste_seq_step1();// include 1Sec delay           <- to open !!!!
+        }
+        //Trigger_WHS_MAX11614_Read_Gas_Sensor();
     }
     //ROM_IntMasterEnable();
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.h b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.h
index 9335a6d08..0d7c42ad6 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.h
+++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.h
@@ -36,6 +36,7 @@ uint8_t getGasReading(void);
 int MillisecGetTemperatures (TEMPERATURE_SENSOR_ID_ENUM SensorId);
 float MillisecGetPressures (int SensorId);
 void setRapidPressureRead(bool value);
+bool getRapidPressureRead(void);
 
 
 void MillisecInit(void);
@@ -49,7 +50,6 @@ void MillisecLogClose(void);
 
 #endif
 
-
 extern bool watchdogCriticalAlarm;
 
 extern Task_Handle Millisecond_Task_Handle;
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.c b/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.c
index 3d1d353c2..eac1f605a 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.c
@@ -5,7 +5,7 @@ float PIDAlgorithmCalculation(float _setPoint,float _mesuredParam , PID_Config_P
 {
     float error;
     float derivative;
-    float output;
+    float output = 0;
 
     //Calculate P,I,D
     error = _setPoint - _mesuredParam;
@@ -19,7 +19,7 @@ float PIDAlgorithmCalculation(float _setPoint,float _mesuredParam , PID_Config_P
     derivative = (error - *_pre_error)/params->dt;
 
     output = params->Kp*error/params->ProportionalErrorMultiplier + params->Ki**_integral/params->IntegralErrorMultiplier + params->Kd*derivative;
-
+    //}
     //Saturation Filter
     if(output > params->MAX)
     {
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/control.c b/Software/Embedded_SW/Embedded/Modules/Control/control.c
index 0f9848ba1..0a092821e 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/control.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/control.c
@@ -170,6 +170,7 @@ void ControlStop(void)
     ControlRestart = false;
     ADCAcquireStop();
 }
+int FPGA_ReInit_Count = 0; ///avoid too many reinitializations of motors as happens when FPGA is corrupted
 uint32_t ControlActivityLed( uint32_t Parameter1)
 {
     static bool flag = false;
@@ -189,19 +190,39 @@ uint32_t ControlActivityLed( uint32_t Parameter1)
                 if (JobIsActive())
                 {
                     JobEndReason = JOB_MOTOR_ALARM;
+                    usnprintf(AlarmReasonStr, 100, "Hardware Failure Error");
                     SendJobProgress(0.0,0,false, "Hardware Failure Error");
                     AbortJob("FPGA Watchdog Error");
                 }
-                ReportWithPackageFilter(FPGAFilter, "FPGA Watchdog Error",__FILE__,__LINE__,0,RpError, 0,0);
+                if (FPGA_ReInit_Count++<20)
+                {
+                    ReportWithPackageFilter(FPGAFilter, "FPGA Watchdog Error",__FILE__,__LINE__,0,RpError, 0,0);
+
+                    ACTIVITY_GREEN_LED_ON;
+                    MotorConfiguredTimeout = 100;
 
-                ACTIVITY_GREEN_LED_ON;
-                FPGA_SetMotorsInit();
-                Motor_ReconfigAllMotors();
+                    FPGA_SetMotorsInit();
+                    Motor_ReconfigAllMotors();
+                }
             }
         }
 		else
             ACTIVITY_GREEN_LED_OFF;
 
+        if(power.color == colorOFF) Pannel_Leds(POWER_ON_OFF,MODE_OFF);
+        if(jog.color   == colorOFF) Pannel_Leds(THREAD_JOGGING,MODE_OFF);
+        if(load.color  == colorOFF) Pannel_Leds(THREAD_JOGGING,MODE_OFF);
+        if(cart1.color == colorOFF) Pannel_Leds(CART_1,MODE_OFF);
+        if(cart2.color == colorOFF) Pannel_Leds(CART_2,MODE_OFF);
+        if(cart3.color == colorOFF) Pannel_Leds(CART_3,MODE_OFF);
+
+        if(power.color == colorON) Pannel_Leds(POWER_ON_OFF,MODE_ON);
+        if(jog.color   == colorON) Pannel_Leds(THREAD_JOGGING,MODE_ON);
+        if(load.color  == colorON) Pannel_Leds(THREAD_JOGGING,MODE_ON);
+        if(cart1.color == colorON) Pannel_Leds(CART_1,MODE_ON);
+        if(cart2.color == colorON) Pannel_Leds(CART_2,MODE_ON);
+        if(cart3.color == colorON) Pannel_Leds(CART_3,MODE_ON);
+
         if(power.color == fastBILNK)
             Pannel_Leds(POWER_ON_OFF,MODE_OFF);
         else
@@ -593,6 +614,9 @@ uint32_t ControlLowLoop(uint32_t tick)
                 continue;
             if (ControlArray[ControlLowDevice_i].ControlTiming == eOneMillisecond)
                 continue;
+            if (ControlArray[ControlLowDevice_i].StartTick == tick)
+                continue;
+
             if (((tick - ControlArray[ControlLowDevice_i].StartTick)%ControlArray[ControlLowDevice_i].ControlTiming)<=skipped_ticks) // run the control on exact intervals
             {
                 ControlBacklog[backlogindex]=ControlLowDevice_i;
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/control.h b/Software/Embedded_SW/Embedded/Modules/Control/control.h
index 78e24f495..4b6a80967 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/control.h
+++ b/Software/Embedded_SW/Embedded/Modules/Control/control.h
@@ -21,6 +21,7 @@ typedef enum {
     eHundredMillisecond = 100,
     eOneSecond = 1000,
     eOneMinute = 60000,
+    eTenMinutes = 600000,
     eOneHour = 3600000
 }CTRL_TIMING_ENUM;
 typedef enum