Decrease the delay in I2C communication from 5000 ticks to 100 + add wait until bus is not busy, add I2C task trigger to init the I/O extenders , move to 16 bit in I2C I/O extenders, remove unnecessary delays...

17 files changed, 909 insertions, 112 deletions

diff --git a/Software/Embedded_SW/Embedded/.cproject b/Software/Embedded_SW/Embedded/.cproject
index 26bee6013..33288a248 100644
--- a/Software/Embedded_SW/Embedded/.cproject
+++ b/Software/Embedded_SW/Embedded/.cproject
@@ -170,7 +170,7 @@
 						</toolChain>
 					</folderInfo>
 					<sourceEntries>
-						<entry excluding="Drivers/Uart_Comm/WHS_Controller_Comm/Modbus|Drivers/Uart_Comm/WHS_Controller_Comm/Modbus/tcp|Drivers/I2C_Communication/RFID_NFC/NFC_I2C|Drivers/I2C_Communication/RFID_NFC/PN7150|Drivers/I2C_Communication/Head_Card/ADC/Head_ADC_old.c|src|EmbParam.cfg" flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name=""/>
+						<entry excluding="Drivers/I2C_Communication/ektm4c129_i2c_master_udma_fifo.c|Drivers/I2C_Communication/test_fifo_dma.c|Drivers/Uart_Comm/WHS_Controller_Comm/Modbus|Drivers/Uart_Comm/WHS_Controller_Comm/Modbus/tcp|Drivers/I2C_Communication/RFID_NFC/NFC_I2C|Drivers/I2C_Communication/RFID_NFC/PN7150|Drivers/I2C_Communication/Head_Card/ADC/Head_ADC_old.c|src|EmbParam.cfg" flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name=""/>
 					</sourceEntries>
 				</configuration>
 			</storageModule>
diff --git a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/Head_Card/I2C_Head_Mux.c b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/Head_Card/I2C_Head_Mux.c
index 80411394b..f8a425e36 100644
--- a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/Head_Card/I2C_Head_Mux.c
+++ b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/Head_Card/I2C_Head_Mux.c
@@ -40,7 +40,7 @@ uint32_t Select_Main_Head_Mux_Channel_test()
 {
     uint32_t status= OK;
 
-    uint8_t Write_Buf[1];
+    uint8_t Write_Buf[1] = {0};
 
     Write_Buf[0] = 0x01 << 0;//Channel 0
 
@@ -53,7 +53,9 @@ uint32_t Block_Main_Head_Mux_Channel()
 {
     uint32_t status= OK;
 
-    uint8_t Write_Buf[1];
+    uint8_t Write_Buf[1] = {0};
+
+    Main_Mux_Flag = UNKNOWN;
 
     Write_Buf[0] = 0x00;//Block all
 
diff --git a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/Head_Card/IO_Ports/Head_IO.c b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/Head_Card/IO_Ports/Head_IO.c
index d7b7fc98f..e8f41eaac 100644
--- a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/Head_Card/IO_Ports/Head_IO.c
+++ b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/Head_Card/IO_Ports/Head_IO.c
@@ -31,7 +31,7 @@ uint32_t TCA9555Config(uint8_t TCA9555_ADDRESS)//Set GPIO
     uint32_t status= OK;
     uint8_t Config_Reg_PORT_0;
     uint8_t Config_Reg_PORT_1;
-    uint8_t Write_Buf[2];
+    uint8_t Write_Buf[3];
     TCA9555_union config_bits;
 
     if(TCA9555_ADDRESS == 0x40)
@@ -87,12 +87,8 @@ uint32_t TCA9555Config(uint8_t TCA9555_ADDRESS)//Set GPIO
 
     Write_Buf[0] = TCA9555_CONFIG_PORT_0;
     Write_Buf[1] = Config_Reg_PORT_0;
-
-    status |= I2C_Write(I2C_ID_HEAD_CARD, TCA9555_ADDRESS, Write_Buf, 2);
-
-    Write_Buf[0] = TCA9555_CONFIG_PORT_1;
-    Write_Buf[1] = Config_Reg_PORT_1;
-    status |= I2C_Write(I2C_ID_HEAD_CARD, TCA9555_ADDRESS, Write_Buf, 2);
+    Write_Buf[2] = Config_Reg_PORT_1;
+    status |= I2C_Write(I2C_ID_HEAD_CARD, TCA9555_ADDRESS, Write_Buf, 3);
 
     return OK;
 
@@ -148,19 +144,28 @@ uint32_t Head_IO_Init()
 uint32_t Head_Write_IO_Reg(uint8_t TCA9555_ADDRESS, uint8_t Byte) //LOW/HIGH/LOW_AND_HIGH
 {
     uint32_t status= OK;
-    uint8_t Write_Buf[2];
+    uint8_t Write_Buf[3];
 
     if(TCA9555_ADDRESS == 0x40)
     {
-        if((Byte == LOW) || (Byte == LOW_AND_HIGH))
+        if(Byte == LOW_AND_HIGH)
+        {
+            Write_Buf[0] = TCA9555_OUTPUT_PORT_0;
+            Write_Buf[1] = Head_I2C_EXP1_0x40.uchar[0];
+            Write_Buf[2] = Head_I2C_EXP1_0x40.uchar[1];
+            status |= Select_Main_Head_Mux_Channel();
+            status |= I2C_Write(I2C_ID_HEAD_CARD, TCA9555_ADDRESS, Write_Buf, 3);
+        }
+        else
+        if(Byte == LOW)
         {
             Write_Buf[0] = TCA9555_OUTPUT_PORT_0;
             Write_Buf[1] = Head_I2C_EXP1_0x40.uchar[0];
             status |= Select_Main_Head_Mux_Channel();
             status |= I2C_Write(I2C_ID_HEAD_CARD, TCA9555_ADDRESS, Write_Buf, 2);
         }
-
-        if((Byte == HIGH) || (Byte == LOW_AND_HIGH))
+        else
+        if(Byte == HIGH)
         {
             Write_Buf[0] = TCA9555_OUTPUT_PORT_1;
             Write_Buf[1] = Head_I2C_EXP1_0x40.uchar[1];
@@ -171,14 +176,24 @@ uint32_t Head_Write_IO_Reg(uint8_t TCA9555_ADDRESS, uint8_t Byte) //LOW/HIGH/LOW
     else
     if(TCA9555_ADDRESS == 0x42)
     {
-        if((Byte == LOW) || (Byte == LOW_AND_HIGH))
+        if(Byte == LOW_AND_HIGH)
+        {
+            Write_Buf[0] = TCA9555_OUTPUT_PORT_0;
+            Write_Buf[1] = Head_I2C_EXP2_0x42.uchar[0];
+            Write_Buf[2] = Head_I2C_EXP2_0x42.uchar[1];
+            status |= Select_Main_Head_Mux_Channel();
+            status |= I2C_Write(I2C_ID_HEAD_CARD, TCA9555_ADDRESS, Write_Buf, 3);
+        }
+        else
+        if(Byte == LOW)
         {
             Write_Buf[0] = TCA9555_OUTPUT_PORT_0;
             Write_Buf[1] = Head_I2C_EXP2_0x42.uchar[0];
             status |= Select_Main_Head_Mux_Channel();
             status |= I2C_Write(I2C_ID_HEAD_CARD, TCA9555_ADDRESS, Write_Buf, 2);
         }
-        if((Byte == HIGH) || (Byte == LOW_AND_HIGH))
+        else
+        if(Byte == HIGH)
         {
             Write_Buf[0] = TCA9555_OUTPUT_PORT_1;
             Write_Buf[1] = Head_I2C_EXP2_0x42.uchar[1];
@@ -189,14 +204,24 @@ uint32_t Head_Write_IO_Reg(uint8_t TCA9555_ADDRESS, uint8_t Byte) //LOW/HIGH/LOW
     else
     if(TCA9555_ADDRESS == 0x46)
     {
-        if((Byte == LOW) || (Byte == LOW_AND_HIGH))
+        if(Byte == LOW_AND_HIGH)
+        {
+            Write_Buf[0] = TCA9555_OUTPUT_PORT_0;
+            Write_Buf[1] = Head_I2C_EXP4_0x46.uchar[0];
+            Write_Buf[2] = Head_I2C_EXP4_0x46.uchar[1];
+            status |= Select_Main_Head_Mux_Channel();
+            status |= I2C_Write(I2C_ID_HEAD_CARD, TCA9555_ADDRESS, Write_Buf, 3);
+        }
+        else
+        if(Byte == LOW)
         {
             Write_Buf[0] = TCA9555_OUTPUT_PORT_0;
             Write_Buf[1] = Head_I2C_EXP4_0x46.uchar[0];// & BIT7;
             status |= Select_Main_Head_Mux_Channel();
             status |= I2C_Write(I2C_ID_HEAD_CARD, TCA9555_ADDRESS, Write_Buf, 2);
         }
-        if((Byte == HIGH) || (Byte == LOW_AND_HIGH))
+        else
+        if(Byte == HIGH)
         {
             Write_Buf[0] = TCA9555_OUTPUT_PORT_1;
             Write_Buf[1] = Head_I2C_EXP4_0x46.uchar[1];// & (BIT10 | BIT11) ;
@@ -218,7 +243,18 @@ uint32_t Head_Read_IO_Reg(uint8_t TCA9555_ADDRESS, uint8_t Byte) //LOW/HIGH/LOW_
 
     if(TCA9555_ADDRESS == 0x44)
     {
-        if((Byte == LOW) || (Byte == LOW_AND_HIGH))
+        if(Byte == LOW_AND_HIGH)
+        {
+            Write_Buf[0] = TCA9555_INPUT_PORT_0;
+            status |= Select_Main_Head_Mux_Channel();
+            status |= I2C_Write(I2C_ID_HEAD_CARD, TCA9555_ADDRESS, Write_Buf, 1);
+            delayUs(10);//TODO - check the delay
+            status |= I2C_Read(I2C_ID_HEAD_CARD, TCA9555_ADDRESS + 1, Read_Buf, 2);
+            Head_I2C_EXP3_0x44.uchar[0] = Read_Buf[0];
+            Head_I2C_EXP3_0x44.uchar[1] = Read_Buf[1];
+        }
+        else
+        if(Byte == LOW)
         {
             Write_Buf[0] = TCA9555_INPUT_PORT_0;
             status |= Select_Main_Head_Mux_Channel();
@@ -227,7 +263,8 @@ uint32_t Head_Read_IO_Reg(uint8_t TCA9555_ADDRESS, uint8_t Byte) //LOW/HIGH/LOW_
             status |= I2C_Read(I2C_ID_HEAD_CARD, TCA9555_ADDRESS + 1, Read_Buf, 1);
             Head_I2C_EXP3_0x44.uchar[0] = Read_Buf[0];
         }
-        if((Byte == HIGH) || (Byte == LOW_AND_HIGH))
+        else
+        if(Byte == HIGH)
         {
             Write_Buf[0] = TCA9555_INPUT_PORT_1;
             status |= Select_Main_Head_Mux_Channel();
@@ -241,7 +278,20 @@ uint32_t Head_Read_IO_Reg(uint8_t TCA9555_ADDRESS, uint8_t Byte) //LOW/HIGH/LOW_
     else
     if(TCA9555_ADDRESS == 0x46)
     {
-        if((Byte == LOW) || (Byte == LOW_AND_HIGH))
+        if(Byte == LOW_AND_HIGH)
+        {
+            Write_Buf[0] = TCA9555_INPUT_PORT_0;
+            status |= Select_Main_Head_Mux_Channel();
+            status |= I2C_Write(I2C_ID_HEAD_CARD, TCA9555_ADDRESS, Write_Buf, 1);
+            delayUs(10);//TODO - check the delay
+            status |= I2C_Read(I2C_ID_HEAD_CARD, TCA9555_ADDRESS + 1, Read_Buf, 2);
+            Head_I2C_EXP4_0x46.uchar[0] &= BIT7;//ACT_DIR
+            Head_I2C_EXP4_0x46.uchar[0] |= Read_Buf[0];
+            Head_I2C_EXP4_0x46.uchar[1] &= (BIT2 | BIT3);//ACTIN_ON ACTOT_ON
+            Head_I2C_EXP4_0x46.uchar[1] |= Read_Buf[0];
+        }
+        else
+        if(Byte == LOW)
         {
             Write_Buf[0] = TCA9555_INPUT_PORT_0;
             status |= Select_Main_Head_Mux_Channel();
@@ -253,7 +303,8 @@ uint32_t Head_Read_IO_Reg(uint8_t TCA9555_ADDRESS, uint8_t Byte) //LOW/HIGH/LOW_
 
             Head_I2C_EXP4_0x46.uchar[0] |= Read_Buf[0];
         }
-        if((Byte == HIGH) || (Byte == LOW_AND_HIGH))
+        else
+        if(Byte == HIGH)
         {
             Write_Buf[0] = TCA9555_INPUT_PORT_1;
             status |= Select_Main_Head_Mux_Channel();
diff --git a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/I2C.c b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/I2C.c
index a9f037942..04388865b 100644
--- a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/I2C.c
+++ b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/I2C.c
@@ -17,6 +17,7 @@
 #include "driverlib/sysctl.h"
 #include "driverlib/i2c.h"
 #include "I2C.h"
+#include "inc/hw_i2c.h"
 #include "i2c_fifo.h"
 
 #define I2C_DELAY 3000
@@ -71,7 +72,7 @@ void Configuring_Master_I2C2(/*uint32_t ui32SysClock*/)
         I2CMasterInitExpClk(I2C2_BASE, ui32SysClock, false);//The last parameter sets the I2C data transfer rate: false for 100kbps and true for 400kbps
     #endif
 
-    i2c_fifo_setup(I2C2_BASE);
+    //i2c_fifo_setup(I2C2_BASE);
 }
 
 //--------------------------------------------------------------
@@ -125,7 +126,7 @@ void Configuring_Master_I2C3(/*uint32_t ui32SysClock*/)
         I2CMasterInitExpClk(I2C3_BASE, ui32SysClock, false);//The last parameter sets the I2C data transfer rate: false for 100kbps and true for 400kbps
     #endif
 
-    i2c_fifo_setup(I2C3_BASE);
+    //i2c_fifo_setup(I2C3_BASE);
 }
 
 
@@ -179,7 +180,7 @@ void Configuring_Master_I2C4(/*uint32_t ui32SysClock*/)
         I2CMasterInitExpClk(I2C4_BASE, ui32SysClock, false);//The last parameter sets the I2C data transfer rate: false for 100kbps and true for 400kbps
     #endif
 
-    i2c_fifo_setup(I2C4_BASE);
+    //i2c_fifo_setup(I2C4_BASE);
 }
 
 void Init_All_I2C()
diff --git a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/I2C.h b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/I2C.h
index ed4c1228f..e8fa3efc3 100644
--- a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/I2C.h
+++ b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/I2C.h
@@ -11,7 +11,7 @@
 extern uint32_t Arb_Loss_Count;
 
 
-#define I2C_BUSY_DELAY 5000//15000
+#define I2C_BUSY_DELAY 100//15000
 
 uint32_t I2C_control(uint32_t I2C_ID, uint32_t Slave_Add, bool RW,uint32_t Byte2Write );
 void Init_All_I2C();
@@ -26,8 +26,8 @@ uint32_t I2C_WriteBuff(uint32_t I2C_BASE, unsigned char addr, unsigned char* dat
 
 uint32_t I2C_ReadBuff(uint32_t I2C_BASE, unsigned char addr, unsigned char* data, unsigned int len);
 
-uint32_t I2C_Write(uint32_t I2C_ID, uint32_t Slave_Add, uint32_t *I2C_Write_buf, uint32_t No_BytesToWrite );
-uint32_t I2C_Read(uint32_t I2C_ID, uint32_t Slave_Add, uint32_t *I2C_Read_buf, uint32_t No_BytesToRead );
+uint32_t I2C_Write(uint32_t I2C_ID, uint32_t Slave_Add, uint8_t *I2C_Write_buf, uint32_t No_BytesToWrite );
+uint32_t I2C_Read(uint32_t I2C_ID, uint32_t Slave_Add, uint8_t *I2C_Read_buf, uint32_t No_BytesToRead );
 
 uint32_t Block_Main_MUX_TCA9548A_0xE4(); ////unused I2C MUX
 
diff --git a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/I2C_Comm.c b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/I2C_Comm.c
index 8bd21b9b4..4289c1794 100644
--- a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/I2C_Comm.c
+++ b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/I2C_Comm.c
@@ -22,10 +22,8 @@
 #include "i2c_fifo.h"
 #include "i2c.h"
 
-
-
-
 uint32_t Arb_Loss_Count = 0;
+
 //*****************************************************************************
 //! Indicates whether or not the I2C bus has timed out.
 //!
@@ -37,18 +35,6 @@ uint32_t Arb_Loss_Count = 0;
 //! \return Returns \b true if the I2C bus has timed out; otherwise, returns
 //! \b false.
 //*****************************************************************************
-bool I2CMasterTimeout(uint32_t ui32Base)
-{
-    // Return the bus timeout status
-    if(HWREG(ui32Base + I2C_O_MCS) & I2C_MCS_CLKTO)
-    {
-        return(true);
-    }
-    else
-    {
-       return(false);
-    }
-}
 
 uint32_t I2C_WriteBuff(uint32_t I2C_BASE, unsigned char addr, unsigned char* data, unsigned int len)
 {
@@ -64,36 +50,29 @@ uint32_t I2C_WriteBuff(uint32_t I2C_BASE, unsigned char addr, unsigned char* dat
     if (len == 1)
     {
         I2CMasterControl(I2C_BASE, I2C_MASTER_CMD_SINGLE_SEND);
-        //while ( I2CMasterBusy(I2C_BASE) && !I2CMasterTimeout( I2C_BASE) );
-//
-//        return I2CMasterErr(I2C_BASE) == I2C_MASTER_ERR_NONE;
-        status |= I2CMasterErr(I2C_BASE);
         SysCtlDelay(I2C_BUSY_DELAY);
+        while(I2CMasterBusy(I2C_BASE));
+        status |= I2CMasterErr(I2C_BASE);
     }
     else
     {
-
-
         // Start sending consecutive data
         I2CMasterControl(I2C_BASE, I2C_MASTER_CMD_BURST_SEND_START);
-        //while ( I2CMasterBusy(I2C_BASE) && !I2CMasterTimeout( I2C_BASE) );
-    //    if (I2CMasterErr(I2C_BASE) != I2C_MASTER_ERR_NONE)
-    //        return false;
-        status |= I2CMasterErr(I2C_BASE);
         SysCtlDelay(I2C_BUSY_DELAY);
+        while(I2CMasterBusy(I2C_BASE));
+        status |= I2CMasterErr(I2C_BASE);
+
         len--;
         data++;
 
-
         // Continue sending consecutive data
         while(len > 1){
             I2CMasterDataPut(I2C_BASE, *data);
             I2CMasterControl(I2C_BASE, I2C_MASTER_CMD_BURST_SEND_CONT);
-            //while ( I2CMasterBusy(I2C_BASE) && !I2CMasterTimeout( I2C_BASE) );
-    //        if (I2CMasterErr(I2C_BASE) != I2C_MASTER_ERR_NONE)
-    //            return false;
-            status |= I2CMasterErr(I2C_BASE);
             SysCtlDelay(I2C_BUSY_DELAY);
+            while(I2CMasterBusy(I2C_BASE));
+            status |= I2CMasterErr(I2C_BASE);
+
             len--;
             data++;
         }
@@ -101,11 +80,9 @@ uint32_t I2C_WriteBuff(uint32_t I2C_BASE, unsigned char addr, unsigned char* dat
         // Send last piece of data
         I2CMasterDataPut(I2C_BASE, *data);
         I2CMasterControl(I2C_BASE, I2C_MASTER_CMD_BURST_SEND_FINISH);
-        //while ( I2CMasterBusy(I2C_BASE) && !I2CMasterTimeout( I2C_BASE) );
-    //
-    //    return I2CMasterErr(I2C_BASE) == I2C_MASTER_ERR_NONE;
-        status |= I2CMasterErr(I2C_BASE);
         SysCtlDelay(I2C_BUSY_DELAY);
+        while(I2CMasterBusy(I2C_BASE));
+        status |= I2CMasterErr(I2C_BASE);
     }
 
     return status;
@@ -132,12 +109,9 @@ uint32_t I2C_ReadBuff(uint32_t I2C_BASE, unsigned char addr, unsigned char* data
     if (len == 1)
     {
         I2CMasterControl(I2C_BASE, I2C_MASTER_CMD_SINGLE_RECEIVE);
-        //while ( I2CMasterBusy(I2C_BASE) && !I2CMasterTimeout( I2C_BASE) );
         SysCtlDelay(I2C_BUSY_DELAY);
+        while(I2CMasterBusy(I2C_BASE));
         *data = I2CMasterDataGet(I2C_BASE);
-
-        //return I2CMasterErr(I2C_BASE) == I2C_MASTER_ERR_NONE;
-
         status |= I2CMasterErr(I2C_BASE);
     }
     else
@@ -145,11 +119,9 @@ uint32_t I2C_ReadBuff(uint32_t I2C_BASE, unsigned char addr, unsigned char* data
 
         // Start receiving consecutive data
         I2CMasterControl(I2C_BASE, I2C_MASTER_CMD_BURST_RECEIVE_START);
-        //while ( I2CMasterBusy(I2C_BASE) && !I2CMasterTimeout( I2C_BASE) );
         SysCtlDelay(I2C_BUSY_DELAY);
+        while(I2CMasterBusy(I2C_BASE));
         *data = I2CMasterDataGet(I2C_BASE);
-    //    if (I2CMasterErr(I2C_BASE) != I2C_MASTER_ERR_NONE)
-    //        return false;
         status |= I2CMasterErr(I2C_BASE);
 
         len--;
@@ -159,11 +131,9 @@ uint32_t I2C_ReadBuff(uint32_t I2C_BASE, unsigned char addr, unsigned char* data
         // Continue receiving consecutive data
         while(len > 1){
             I2CMasterControl(I2C_BASE, I2C_MASTER_CMD_BURST_RECEIVE_CONT);
-            //while ( I2CMasterBusy(I2C_BASE) && !I2CMasterTimeout( I2C_BASE) );
            SysCtlDelay(I2C_BUSY_DELAY);
+           while(I2CMasterBusy(I2C_BASE));
             *data = I2CMasterDataGet(I2C_BASE);
-    //        if (I2CMasterErr(I2C_BASE) != I2C_MASTER_ERR_NONE)
-    //            return false;
             status |= I2CMasterErr(I2C_BASE);
 
             len--;
@@ -172,20 +142,17 @@ uint32_t I2C_ReadBuff(uint32_t I2C_BASE, unsigned char addr, unsigned char* data
 
         // Receive last piece of data
         I2CMasterControl(I2C_BASE, I2C_MASTER_CMD_BURST_RECEIVE_FINISH);
-        //while ( I2CMasterBusy(I2C_BASE) && !I2CMasterTimeout( I2C_BASE) );
         SysCtlDelay(I2C_BUSY_DELAY);
+        while(I2CMasterBusy(I2C_BASE));
         *data = I2CMasterDataGet(I2C_BASE);
-
-        //return I2CMasterErr(I2C_BASE) == I2C_MASTER_ERR_NONE;
         status |= I2CMasterErr(I2C_BASE);
     }
     return status;
 }
 
-uint32_t I2C_Write(uint32_t I2C_ID, uint32_t Slave_Add, uint32_t *I2C_Write_buf, uint32_t No_BytesToWrite )
+uint32_t I2C_Write(uint32_t I2C_ID, uint32_t Slave_Add, uint8_t *I2C_Write_buf, uint32_t No_BytesToWrite )
 {
     uint32_t status = OK;
-    uint8_t* buff = (uint8_t *)I2C_Write_buf;
     uint32_t I2C_BASE;
 
     if (I2C_ID == 4)
@@ -246,11 +213,10 @@ uint32_t I2C_Write(uint32_t I2C_ID, uint32_t Slave_Add, uint32_t *I2C_Write_buf,
     return status;
 }
 
-uint32_t I2C_Read(uint32_t I2C_ID, uint32_t Slave_Add, uint32_t *I2C_Read_buf, uint32_t No_BytesToRead )
+uint32_t I2C_Read(uint32_t I2C_ID, uint32_t Slave_Add, uint8_t *I2C_Read_buf, uint32_t No_BytesToRead )
 {
 
     uint32_t status = OK;
-    uint8_t* buff = (uint8_t *)I2C_Read_buf;
     uint32_t I2C_BASE;
 
     if (I2C_ID == 4)
@@ -278,7 +244,7 @@ uint32_t I2C_Read(uint32_t I2C_ID, uint32_t Slave_Add, uint32_t *I2C_Read_buf, u
     //Byte2Write = Byte2Write & 0xFF;
 
     status = I2C_ReadBuff(I2C_BASE, Slave_Add, I2C_Read_buf, No_BytesToRead);
-    //status = I2C_ReadFifo(I2C_BASE, Slave_Add, buff, No_BytesToRead);
+
     if (status)
     {
         if (Arb_Loss_Count++ >= 5)
@@ -298,4 +264,3 @@ uint32_t I2C_Read(uint32_t I2C_ID, uint32_t Slave_Add, uint32_t *I2C_Read_buf, u
 
     return status;
 }
-
diff --git a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/I2C_Task.c b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/I2C_Task.c
index 44ab8d710..708c4dde5 100644
--- a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/I2C_Task.c
+++ b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/I2C_Task.c
@@ -34,6 +34,7 @@ Task_Handle I2C_Task_Handle;
 Mailbox_Handle          I2C_ReadingMsgQ = NULL;
 typedef enum
 {
+    Head_IO_Initialization,
     Midtank_Pressure_Reading,
     Heater_Current_Reading,
     PT100_Reading,
@@ -87,6 +88,17 @@ typedef struct I2C_ReadingMessage{
     uint8_t*  parameter3;
 }I2C_ReadingMessageStruc;
 
+
+void Trigger_Head_io_Init(void)
+{
+    I2C_ReadingMessageStruc I2C_ReadingMessage;
+
+    I2C_ReadingMessage.messageId = Head_IO_Initialization;
+    if (I2C_ReadingMsgQ != NULL)
+        Mailbox_post(I2C_ReadingMsgQ , &I2C_ReadingMessage, BIOS_NO_WAIT);
+    return;
+}
+
 void Trigger_Heater_Current_Read(void)
 {
     I2C_ReadingMessageStruc I2C_ReadingMessage;
@@ -583,6 +595,9 @@ void I2C_ReadingTask(UArg arg0, UArg arg1)
         //Report("------------ I2C task  - messageId -------------", __FILE__,__LINE__, (int)(I2C_ReadingMessage.messageId), RpMessage, 0, 0);
         switch (I2C_ReadingMessage.messageId)
         {
+            case Head_IO_Initialization:
+                Head_IO_Init();
+                    break;
         case Midtank_Pressure_Reading:
             //Report("Read_MidTank_Pressure_Sensor",__FILE__,__LINE__,255,RpWarning,0,0);
             Read_MidTank_Pressure_Sensor(I2C_ReadingMessage.parameter);
diff --git a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/I2C_Task.h b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/I2C_Task.h
index 20fcb0074..2a98296b4 100644
--- a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/I2C_Task.h
+++ b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/I2C_Task.h
@@ -9,7 +9,7 @@
 #ifndef DRIVERS_I2C_COMMUNICATION_I2C_TASK_H_
 #define DRIVERS_I2C_COMMUNICATION_I2C_TASK_H_
 
-
+void Trigger_Head_io_Init(void);
 void Trigger_WHS_init_IO(void);
 void Trigger_WHS_init_fan(void);
 void Trigger_WHS_init_PT100(void);
diff --git a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/WHS_Card/D_MAX11614_ADC/MAX11614_Driver.c b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/WHS_Card/D_MAX11614_ADC/MAX11614_Driver.c
index ae37bf938..559cc0375 100644
--- a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/WHS_Card/D_MAX11614_ADC/MAX11614_Driver.c
+++ b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/WHS_Card/D_MAX11614_ADC/MAX11614_Driver.c
@@ -23,7 +23,7 @@ bool MAX11614_Read_allADC(uint8_t i2cbus, WHS_chip_address i2cadd, uint16_t *p_v
 
     status |= I2C_Read(i2cbus, i2cadd + 1, p_value , 0x10);//&p_value
 
-    delayUs(10);//TODO - check the delay
+    //delayUs(10);//TODO - check the delay
     return status;
 
 }
diff --git a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/WHS_Card/D_MAX11614_ADC/WHS_MAX11614_A2D.c b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/WHS_Card/D_MAX11614_ADC/WHS_MAX11614_A2D.c
index 3671ff4e0..29698239a 100644
--- a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/WHS_Card/D_MAX11614_ADC/WHS_MAX11614_A2D.c
+++ b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/WHS_Card/D_MAX11614_ADC/WHS_MAX11614_A2D.c
@@ -79,7 +79,7 @@ uint32_t WHS_MAX11614_Read_allADC( )
 
     status |= MAX11614_Read_allADC(U20_MAX11614.MAX11614_I2CBUS, U20_MAX11614.MAX11614_I2CADDR, U20_MAX11614.MAX11614_Read_Buf);
 
-    delayUs(10);//TODO - check the delay
+    //delayUs(10);//TODO - check the delay
     return status;
 
 }
diff --git a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/WHS_Card/D_Max5805_ADC_Blower/WHS_Blower.c b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/WHS_Card/D_Max5805_ADC_Blower/WHS_Blower.c
index c5f8037aa..3f50f9898 100644
--- a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/WHS_Card/D_Max5805_ADC_Blower/WHS_Blower.c
+++ b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/WHS_Card/D_Max5805_ADC_Blower/WHS_Blower.c
@@ -84,9 +84,8 @@ int32_t i2c_smbus_read_word_data(int i2cFileDescriptor, uint8_t command_regAddre
     uint32_t status= OK;
     uint8_t Write_Buf[1];
 
-
     Write_Buf[0] = command_regAddress;
-    status |= Block_Main_Head_Mux_Channel();
+
     status |= Select_Main_WHS_Mux_Channel();
     status |= I2C_Write(I2C_ID_WHS_CARD, I2CDAC_ADDRESS, Write_Buf, 1);
 
diff --git a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/WHS_Card/D_TCA9546_ADC_MUX/TCA9546_Driver.c b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/WHS_Card/D_TCA9546_ADC_MUX/TCA9546_Driver.c
index f7fe99aed..f5e14a48e 100644
--- a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/WHS_Card/D_TCA9546_ADC_MUX/TCA9546_Driver.c
+++ b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/WHS_Card/D_TCA9546_ADC_MUX/TCA9546_Driver.c
@@ -24,6 +24,8 @@ bool D_TCA9546_Select_Channel(uint8_t TCA9546A_I2C_bus_number, uint8_t TCA9546A_
     uint8_t Write_Buf[1] = {0};
     uint8_t Read_buf[1] = {0};
 
+    //Select_Main_WHS_Mux_Channel();
+
     status |= I2C_Write(TCA9546A_I2C_bus_number, TCA9546A_I2C_addr, Write_Buf, 1);   //status |= I2C_Write(I2C_ID_HEAD_CARD, TCA9555_ADDRESS, Write_Buf, 2);
         delayUs(10);//TODO - check the delay
     status = I2C_Read(TCA9546A_I2C_bus_number, TCA9546A_I2C_addr+1, Read_buf, 1 );
@@ -47,6 +49,8 @@ bool D_TCA9546_Read_Channel(uint32_t TCA9546A_I2C_bus_number, uint32_t TCA9546A_
     bool status= OK;
     uint8_t Read_buf[1];
 
+    //Select_Main_WHS_Mux_Channel();
+
     status = I2C_Read(TCA9546A_I2C_bus_number, TCA9546A_I2C_addr, Read_buf, 1 );
     if (Read_buf[1] & chnl_number )
     {
diff --git a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/WHS_Card/I2C_WHS_Mux.c b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/WHS_Card/I2C_WHS_Mux.c
index a5d36a264..1ada4f24a 100644
--- a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/WHS_Card/I2C_WHS_Mux.c
+++ b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/WHS_Card/I2C_WHS_Mux.c
@@ -19,7 +19,7 @@ uint32_t Select_Main_WHS_Mux_Channel()
 {
     uint32_t status= OK;
 
-    uint8_t Write_Buf[1];
+    uint8_t Write_Buf[1] = {0};
 
     if(Main_Mux_Flag != WHS)
     {
@@ -39,7 +39,9 @@ uint32_t Block_Main_WHS_Mux_Channel()
 {
     uint32_t status= OK;
 
-    uint8_t Write_Buf[1];
+    uint8_t Write_Buf[1] = {0};
+
+    Main_Mux_Flag = UNKNOWN;
 
     Write_Buf[0] = 0x00;//Block all
 
diff --git a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/WHS_Card/IO_Extender_Ports_TCA9555/TCA9555_Driver.c b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/WHS_Card/IO_Extender_Ports_TCA9555/TCA9555_Driver.c
index cf615f424..c357dbcb1 100644
--- a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/WHS_Card/IO_Extender_Ports_TCA9555/TCA9555_Driver.c
+++ b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/WHS_Card/IO_Extender_Ports_TCA9555/TCA9555_Driver.c
@@ -36,16 +36,12 @@ struct TCA9555_params
 uint32_t D_TCA9555ConfigDirection(uint8_t TCA9555_I2C_bus_number, uint8_t TCA9555_ADDRESS, uint8_t Config_dir_PORT_0, uint8_t Config_dir_PORT_1)
 {
     uint32_t status= OK;
-    uint8_t Write_Buf[2];
+    uint8_t Write_Buf[3];
 
     Write_Buf[0] = TCA9555_CONFIG_PORT_0;
     Write_Buf[1] = Config_dir_PORT_0;
-    status |= I2C_Write(TCA9555_I2C_bus_number, TCA9555_ADDRESS, Write_Buf, 2);   /*status |= I2C_Write(I2C_ID_HEAD_CARD, TCA9555_ADDRESS, Write_Buf, 2);*/
-
-
-    Write_Buf[0] = TCA9555_CONFIG_PORT_1;
-    Write_Buf[1] = Config_dir_PORT_1;
-    status |= I2C_Write(TCA9555_I2C_bus_number, TCA9555_ADDRESS, Write_Buf, 2);   /*status |= I2C_Write(I2C_ID_HEAD_CARD, TCA9555_ADDRESS, Write_Buf, 2);*/
+    Write_Buf[2] = Config_dir_PORT_1;
+    status |= I2C_Write(TCA9555_I2C_bus_number, TCA9555_ADDRESS, Write_Buf, 3);
     return status;
 }
 
diff --git a/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/test_fifo_dma.c b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/test_fifo_dma.c
new file mode 100644
index 000000000..75028fc4a
--- /dev/null
+++ b/Software/Embedded_SW/Embedded/Drivers/I2C_Communication/test_fifo_dma.c
@@ -0,0 +1,763 @@
+/*
+ * test_fifo_dma.c
+ *
+ *  Created on: Jul 1, 2020
+ *      Author: avi
+ */
+
+//*****************************************************************************
+//
+// ektm4c129_i2c_master_udma_fifo.c - I2C Master with UDMA and FIFO Data Transfer
+//
+// Copyright (c) 2013-2015 Texas Instruments Incorporated.  All rights reserved.
+// Software License Agreement
+//
+// Texas Instruments (TI) is supplying this software for use solely and
+// exclusively on TI's microcontroller products. The software is owned by
+// TI and/or its suppliers, and is protected under applicable copyright
+// laws. You may not combine this software with "viral" open-source
+// software in order to form a larger program.
+//
+// THIS SOFTWARE IS PROVIDED "AS IS" AND WITH ALL FAULTS.
+// NO WARRANTIES, WHETHER EXPRESS, IMPLIED OR STATUTORY, INCLUDING, BUT
+// NOT LIMITED TO, IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+// A PARTICULAR PURPOSE APPLY TO THIS SOFTWARE. TI SHALL NOT, UNDER ANY
+// CIRCUMSTANCES, BE LIABLE FOR SPECIAL, INCIDENTAL, OR CONSEQUENTIAL
+// DAMAGES, FOR ANY REASON WHATSOEVER.
+//
+// This is part of revision 2.1.1.71 of the EK-TM4C1294XL Firmware Package.
+//
+//*****************************************************************************
+#include <stdint.h>
+#include <stdbool.h>
+#include "inc/hw_types.h"
+#include "inc/hw_i2c.h"
+#include "inc/hw_ints.h"
+#include "inc/hw_memmap.h"
+#include "inc/hw_udma.h"
+#include "driverlib/gpio.h"
+#include "driverlib/i2c.h"
+#include "driverlib/interrupt.h"
+#include "driverlib/pin_map.h"
+#include "driverlib/sysctl.h"
+#include "driverlib/uart.h"
+#include "driverlib/udma.h"
+#include "utils/random.h"
+#include "utils/uartstdio.h"
+
+//*****************************************************************************
+//
+//! \addtogroup example_list
+//! <h1>ektm4c129_i2c_master_udma_fifo (ektm4c129_i2c_master_udma_fifo)</h1>
+//!
+//! A Simple I2C Master Code for Performing Data Transfer with UDMA and
+//! I2CFIFODATA register
+//
+//*****************************************************************************
+
+//*****************************************************************************
+//
+// Define for I2C Module
+//
+//*****************************************************************************
+uint8_t SLAVE_ADDRESS_EXT;
+#define NUM_OF_I2CBYTES   255
+
+//*****************************************************************************
+//
+// Enumerated Data Types for Master State Machine
+//
+//*****************************************************************************
+enum I2C_MASTER_STATE
+{
+    I2C_OP_IDLE = 0,
+    I2C_OP_TXADDR,
+    I2C_OP_FIFO,
+    I2C_OP_TXDATA,
+    I2C_OP_RXDATA,
+    I2C_OP_STOP,
+    I2C_ERR_STATE
+};
+
+//*****************************************************************************
+//
+// Global variable for Delay Count
+//
+//*****************************************************************************
+volatile uint16_t g_ui16SlaveWordAddress;
+uint8_t  g_ui8MasterTxData[NUM_OF_I2CBYTES];
+uint8_t  g_ui8MasterRxData[NUM_OF_I2CBYTES];
+volatile uint8_t  g_ui8MasterCurrState;
+volatile uint8_t  g_ui8MasterPrevState;
+volatile bool     g_bI2CDirection;
+volatile bool     g_bI2CRepeatedStart;
+volatile uint8_t  g_ui8MasterBytes       = NUM_OF_I2CBYTES;
+volatile uint8_t  g_ui8MasterBytesLength = NUM_OF_I2CBYTES;
+
+
+uint32_t ui32TxArbSize, ui32RxArbSize;
+
+//*****************************************************************************
+//
+// The control table used by the uDMA controller.  This table must be aligned
+// to a 1024 byte boundary.
+//
+//*****************************************************************************
+#if defined(ewarm)
+#pragma data_alignment=1024
+uint8_t pui8ControlTable[1024];
+#elif defined(ccs)
+#pragma DATA_ALIGN(pui8ControlTable, 1024)
+uint8_t pui8ControlTable[1024];
+#else
+uint8_t pui8ControlTable[1024] __attribute__ ((aligned(1024)));
+#endif
+
+//*****************************************************************************
+//
+// Interrupt Handler for I2C Master Interface
+//
+//*****************************************************************************
+void
+I2C3IntHandler(void)
+{
+    uint32_t ui32I2CMasterInterruptStatus;
+
+    //
+    // Toggle PL4 High to Indicate Entry to ISR
+    //
+    ///GPIOPinWrite(GPIO_PORTL_BASE, GPIO_PIN_4, GPIO_PIN_4);
+
+    //
+    // Get the masked interrupt status and clear the flags
+    //
+    ui32I2CMasterInterruptStatus = I2CMasterIntStatusEx(I2C3_BASE, true);
+    I2CMasterIntClearEx(I2C3_BASE, ui32I2CMasterInterruptStatus);
+
+    //
+    // Execute the State Machine
+    //
+    switch (g_ui8MasterCurrState) {
+    case I2C_OP_IDLE:
+        //
+        // Move from IDLE to Transmit Address State
+        //
+        g_ui8MasterPrevState = g_ui8MasterCurrState;
+        g_ui8MasterCurrState = I2C_OP_TXADDR;
+
+        //
+        // Write the upper bits of the page to the Slave
+        //
+        I2CMasterSlaveAddrSet(I2C3_BASE, SLAVE_ADDRESS_EXT, false);
+        I2CMasterDataPut(I2C3_BASE, (g_ui16SlaveWordAddress >> 8));
+        I2CMasterControl(I2C3_BASE, I2C_MASTER_CMD_BURST_SEND_START);
+        break;
+
+    case I2C_OP_TXADDR:
+        //
+        // Assign the current state to the previous state
+        //
+        g_ui8MasterPrevState = g_ui8MasterCurrState;
+
+        //
+        // If Address has been NAK'ed then go to stop state
+        // else go the FIFO Priming State
+        //
+        if(ui32I2CMasterInterruptStatus & I2C_MASTER_INT_NACK)
+        {
+            g_ui8MasterCurrState = I2C_OP_STOP;
+        }
+        else
+        {
+            g_ui8MasterCurrState = I2C_OP_FIFO;
+        }
+
+        //
+        // Write the lower bits of the page to the Slave if
+        // Address has been ACK-ed
+        //
+        I2CMasterDataPut(I2C3_BASE, (g_ui16SlaveWordAddress >> 0));
+        I2CMasterControl(I2C3_BASE, I2C_MASTER_CMD_BURST_SEND_CONT);
+        break;
+
+    case I2C_OP_FIFO:
+        //
+        // If Last Data has been NAK'ed then go to stop state
+        //
+        if(ui32I2CMasterInterruptStatus & I2C_MASTER_INT_NACK)
+        {
+            g_ui8MasterCurrState = I2C_OP_STOP;
+        }
+        //
+        // Based on the direction move to the appropriate state
+        // of Transmit or Receive. Also send the BURST command
+        // for FIFO Operations.
+        //
+        else if(!g_bI2CDirection)
+        {
+            g_ui8MasterCurrState = I2C_OP_TXDATA;
+            I2CMasterControl(I2C3_BASE, I2C_MASTER_CMD_FIFO_BURST_SEND_FINISH);
+        }
+        else
+        {
+            g_ui8MasterCurrState = I2C_OP_RXDATA;
+            I2CMasterSlaveAddrSet(I2C3_BASE, SLAVE_ADDRESS_EXT, true);
+            I2CMasterControl(I2C3_BASE, I2C_MASTER_CMD_FIFO_SINGLE_RECEIVE);
+        }
+        break;
+
+    case I2C_OP_TXDATA:
+        //
+        // Move the current state to the previous state
+        // Else continue with the transmission till last byte
+        //
+        g_ui8MasterPrevState = g_ui8MasterCurrState;
+
+        //
+        // If Address or Data has been NAK'ed then go to stop state
+        // If a Stop condition is seen due to number of bytes getting
+        // done then move to STOP state
+        //
+        if(ui32I2CMasterInterruptStatus & I2C_MASTER_INT_NACK)
+        {
+            g_ui8MasterCurrState = I2C_OP_STOP;
+        }
+        else if(ui32I2CMasterInterruptStatus & I2C_MASTER_INT_STOP)
+        {
+            g_ui8MasterCurrState = I2C_OP_STOP;
+        }
+        else if(ui32I2CMasterInterruptStatus & I2C_MASTER_INT_TX_DMA_DONE)
+        {
+            g_ui8MasterCurrState = I2C_OP_STOP;
+        }
+        else
+        {
+            g_ui8MasterCurrState = I2C_ERR_STATE;
+        }
+        break;
+
+    case I2C_OP_RXDATA:
+        //
+        // Move the current state to the previous state
+        // Else continue with the transmission till last byte
+        //
+        g_ui8MasterPrevState = g_ui8MasterCurrState;
+
+        //
+        // If Address has been NAK'ed then go to stop state
+        // If a Stop condition is seen due to number of bytes getting
+        // done then move to STOP state and read the last data byte
+        //
+        if(ui32I2CMasterInterruptStatus & I2C_MASTER_INT_NACK)
+        {
+            g_ui8MasterCurrState = I2C_OP_STOP;
+        }
+        else if(ui32I2CMasterInterruptStatus & I2C_MASTER_INT_STOP)
+        {
+            g_ui8MasterCurrState = I2C_OP_STOP;
+        }
+        else if(ui32I2CMasterInterruptStatus & I2C_MASTER_INT_RX_DMA_DONE)
+        {
+            g_ui8MasterCurrState = I2C_OP_STOP;
+        }
+        break;
+
+    case I2C_OP_STOP:
+        //
+        // Move the current state to the previous state
+        // Else continue with the transmission till last byte
+        //
+        g_ui8MasterPrevState = g_ui8MasterCurrState;
+        break;
+
+    case I2C_ERR_STATE:
+        g_ui8MasterCurrState = I2C_ERR_STATE;
+        break;
+
+    default:
+        g_ui8MasterCurrState = I2C_ERR_STATE;
+        break;
+    }
+
+    //
+    // Toggle PL4 Low to Indicate Exit from ISR
+    //
+    ///GPIOPinWrite(GPIO_PORTL_BASE, GPIO_PIN_4, 0x0);
+}
+
+//*****************************************************************************
+//
+// This function sets up UDMA Channel Control Structures for TX
+//
+//*****************************************************************************
+void
+ConfigureuDMATX(uint8_t ui8Length, uint32_t ui32ArbSize)
+{
+    //
+    // Put the attributes in a known state for the uDMA channel.
+    // These should already be disabled by default.
+    //
+    uDMAChannelAttributeDisable(UDMA_CH19_I2C3TX,
+                                UDMA_ATTR_USEBURST | UDMA_ATTR_ALTSELECT |
+                                (UDMA_ATTR_HIGH_PRIORITY |
+                                UDMA_ATTR_REQMASK));
+
+    //
+    // Configure the control parameters for the I2C2 TX channel.  The channel
+    // will be used to transfer between memory buffer and I2CFIFODATA register
+    // 8 bits at a time. Therefore the data size is 8 bits, and the address
+    // increment is 8 bits for source. The destination increment is none.
+    // The arbitration size will be set based on the TX FIFO Threshold, which
+    // causes the uDMA controller to rearbitrate after N items are transferred.
+    // This keeps this channel from hogging the uDMA controller once the transfer
+    // is started and allows other channels cycles if they are higher priority.
+    //
+    uDMAChannelControlSet(UDMA_CH19_I2C3TX | UDMA_PRI_SELECT,
+                          UDMA_SIZE_8 | UDMA_SRC_INC_8 | UDMA_DST_INC_NONE |
+                          ui32ArbSize);
+
+    //
+    // Set up the transfer parameters for the hardware channel.  This will
+    // configure the transfer buffers and the transfer size.  Basic mode
+    // is used in this example
+    //
+    uDMAChannelTransferSet(UDMA_CH19_I2C3TX | UDMA_PRI_SELECT,
+                           UDMA_MODE_BASIC, g_ui8MasterTxData,
+                           (void *)(I2C3_BASE + I2C_O_FIFODATA),
+                           ui8Length);
+
+    //
+    // The channel must be enabled.  For hardware based transfers, a request
+    // must be issued by the peripheral.  After this, the uDMA memory transfer begins.
+    //
+    uDMAChannelEnable(UDMA_CH19_I2C3TX);
+
+}
+
+//*****************************************************************************
+//
+// This function sets up UDMA Channel Control Structures for RX
+//
+//*****************************************************************************
+void
+ConfigureuDMARX(uint8_t ui8Length, uint32_t ui32ArbSize)
+{
+    //
+    // Put the attributes in a known state for the uDMA software channel.
+    // These should already be disabled by default.
+    //
+    uDMAChannelAttributeDisable(UDMA_CH18_I2C3RX,
+                                UDMA_ATTR_USEBURST | UDMA_ATTR_ALTSELECT |
+                                (UDMA_ATTR_HIGH_PRIORITY |
+                                UDMA_ATTR_REQMASK));
+
+    //
+    // Configure the control parameters for the I2C2 RX channel.  The channel
+    // will be used to transfer between I2CFIFODATA register and memory buffers,
+    // 8 bits at a time. Therefore the data size is 8 bits, and the address
+    // increment is 8 bits for destination. The source increment is none.
+    // The arbitration size will be set based on the RX FIFO Threshold, which
+    // causes the uDMA controller to rearbitrate after N items are transferred.
+    // This keeps this channel from hogging the uDMA controller once the transfer
+    // is started and allows other channels cycles if they are higher priority.
+    //
+    uDMAChannelControlSet(UDMA_CH18_I2C3RX | UDMA_PRI_SELECT,
+                          UDMA_SIZE_8 | UDMA_SRC_INC_NONE | UDMA_DST_INC_8 |
+                          ui32ArbSize);
+
+    //
+    // Set up the transfer parameters for the hardware channel.  This will
+    // configure the transfer buffers and the transfer size.  Basic mode
+    // is used in this example
+    //
+    uDMAChannelTransferSet(UDMA_CH18_I2C3RX | UDMA_PRI_SELECT,
+                           UDMA_MODE_BASIC, (void *)(I2C3_BASE + I2C_O_FIFODATA),
+                           g_ui8MasterRxData,
+                           ui8Length);
+
+    //
+    // The channel must be enabled.  For hardware based transfers, a request
+    // must be issued by the peripheral.  After this, the uDMA memory transfer begins.
+    //
+    uDMAChannelEnable(UDMA_CH18_I2C3RX);
+
+}
+
+//*****************************************************************************
+//
+// This function sets up UART0 to be used for a console to display information
+// as the example is running.
+//
+//*****************************************************************************
+/*
+void
+InitConsole(void)
+{
+    //
+    // Enable GPIO port A which is used for UART0 pins.
+    // TODO: change this to whichever GPIO port you are using.
+    //
+    SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOA);
+
+    //
+    // Configure the pin muxing for UART0 functions on port A0 and A1.
+    // This step is not necessary if your part does not support pin muxing.
+    // TODO: change this to select the port/pin you are using.
+    //
+    GPIOPinConfigure(GPIO_PA0_U0RX);
+    GPIOPinConfigure(GPIO_PA1_U0TX);
+
+    //
+    // Enable UART0 so that we can configure the clock.
+    //
+    SysCtlPeripheralEnable(SYSCTL_PERIPH_UART0);
+
+    //
+    // Use the internal 16MHz oscillator as the UART clock source.
+    //
+    UARTClockSourceSet(UART0_BASE, UART_CLOCK_PIOSC);
+
+    //
+    // Select the alternate (UART) function for these pins.
+    // TODO: change this to select the port/pin you are using.
+    //
+    GPIOPinTypeUART(GPIO_PORTA_BASE, GPIO_PIN_0 | GPIO_PIN_1);
+
+    //
+    // Initialize the UART for console I/O.
+    //
+    UARTStdioConfig(0, 115200, 16000000);
+}
+*/
+//*****************************************************************************
+//
+// Main Program to Configure and Use the I2C Master
+//
+//*****************************************************************************
+int
+InitI2C3_udma_fifo(void)
+{
+    uint32_t ui32SysClock;
+    uint8_t  /*ui8Count, */ui8MasterBytesLength;
+    //
+
+
+    //
+    // Set up the serial console to use for displaying messages.  This is
+    // just for this example program and is not needed for EPI operation.
+    //
+    ///InitConsole();
+
+    //
+    // Display the setup on the console.
+    //
+    ///UARTprintf("\033[2J\033[H");
+    ///UARTprintf("\r\nExample Code for I2C Master with");
+    ///UARTprintf("\nUDMA and FIFO Data Transfer\n\n");
+
+    //
+    // Stop the Clock, Reset and Enable UDMA Module
+    // in Master Function
+    //
+    SysCtlPeripheralDisable(SYSCTL_PERIPH_UDMA);
+    SysCtlPeripheralReset(SYSCTL_PERIPH_UDMA);
+    SysCtlPeripheralEnable(SYSCTL_PERIPH_UDMA);
+
+    //
+    // Wait for the Peripheral to be ready for programming
+    //
+    while(!SysCtlPeripheralReady(SYSCTL_PERIPH_UDMA));
+
+    //
+    // Enable the uDMA controller.
+    //
+    uDMAEnable();
+
+    //
+    // Point at the control table to use for channel control structures.
+    //
+    uDMAControlBaseSet(pui8ControlTable);
+
+    //
+    // Assign the UDMA Channel for I2C2 RX and TX DMA Request
+    //
+    uDMAChannelAssign(UDMA_CH18_I2C3RX);
+    uDMAChannelAssign(UDMA_CH19_I2C3TX);
+
+    //
+    // Enable GPIO for Configuring the I2C Interface Pins
+    //
+    SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOR);
+
+    //
+    // Wait for the Peripheral to be ready for programming
+    //
+    while(!SysCtlPeripheralReady(SYSCTL_PERIPH_GPIOR));
+
+    //
+    // Configure Pins for I2C2 Master Interface
+    //
+    GPIOPinConfigure(GPIO_PR4_I2C3SCL);
+    GPIOPinConfigure(GPIO_PR5_I2C3SDA);
+    GPIOPinTypeI2C(GPIO_PORTR_BASE, GPIO_PIN_5);
+    GPIOPinTypeI2CSCL(GPIO_PORTR_BASE, GPIO_PIN_4);
+
+    //
+    // Configure GPIO Pin PL4 for Interrupt Time Processing
+    //
+    ///GPIOPinTypeGPIOOutput(GPIO_PORTL_BASE, GPIO_PIN_4);
+    ///GPIOPinWrite(GPIO_PORTL_BASE, GPIO_PIN_4, 0x0);
+
+    //
+    // Setup System Clock for 120MHz
+    //
+    ui32SysClock = SysCtlClockFreqSet((SYSCTL_OSC_MAIN | SYSCTL_USE_PLL | SYSCTL_XTAL_25MHZ |
+                        SYSCTL_CFG_VCO_480), 120000000);
+
+    //
+    // Stop the Clock, Reset and Enable I2C Module
+    // in Master Function
+    //
+    SysCtlPeripheralDisable(SYSCTL_PERIPH_I2C3);
+    SysCtlPeripheralReset(SYSCTL_PERIPH_I2C3);
+    SysCtlPeripheralEnable(SYSCTL_PERIPH_I2C3);
+
+    //
+    // Wait for the Peripheral to be ready for programming
+    //
+    while(!SysCtlPeripheralReady(SYSCTL_PERIPH_I2C3));
+
+    //
+    // Initialize and Configure the Master Module
+    //
+    I2CMasterInitExpClk(I2C3_BASE, ui32SysClock, true);
+
+    //
+    // Assign the Transmit and Receive FIFO to the Master
+    // Transmit threshold of 2 means that when there are less
+    // than or equal to 2 bytes in the TX FIFO then generate
+    // an interrupt
+    // Receive threshold of 6 means that when there are more
+    // than or qual to 6 bytes in the RX FIFO then generate
+    // an interrupt
+    //
+    I2CTxFIFOConfigSet(I2C3_BASE, I2C_FIFO_CFG_TX_MASTER_DMA | I2C_FIFO_CFG_TX_TRIG_2);
+    I2CRxFIFOConfigSet(I2C3_BASE, I2C_FIFO_CFG_RX_MASTER_DMA | I2C_FIFO_CFG_RX_TRIG_3);
+
+    //
+    // Transmit uDMA Arbitrartion size is calculated as the
+    // smaller mod 2 which can write to the TXFIFO without
+    // overrun
+    // TX TRIGGER               ARB
+    // I2C_FIFO_CFG_TX_NO_TRIG  UDMA_ARB_8
+    // I2C_FIFO_CFG_TX_TRIG_1   UDMA_ARB_4
+    // I2C_FIFO_CFG_TX_TRIG_2   UDMA_ARB_4
+    // I2C_FIFO_CFG_TX_TRIG_3   UDMA_ARB_4
+    // I2C_FIFO_CFG_TX_TRIG_4   UDMA_ARB_4
+    // I2C_FIFO_CFG_TX_TRIG_5   UDMA_ARB_2
+    // I2C_FIFO_CFG_TX_TRIG_6   UDMA_ARB_2
+    // I2C_FIFO_CFG_TX_TRIG_7   UDMA_ARB_1
+    //
+    ui32TxArbSize = UDMA_ARB_4;
+
+    //
+    // Receive uDMA Arbitrartion size is calculated as the
+    // Trigger Level to read back data from RXFIFO without
+    // an underrun
+    // RX TRIGGER               ARB
+    // I2C_FIFO_CFG_RX_NO_TRIG  INVALID
+    // I2C_FIFO_CFG_RX_TRIG_1   UDMA_ARB_1
+    // I2C_FIFO_CFG_RX_TRIG_2   UDMA_ARB_2
+    // I2C_FIFO_CFG_RX_TRIG_3   UDMA_ARB_2
+    // I2C_FIFO_CFG_RX_TRIG_4   UDMA_ARB_4
+    // I2C_FIFO_CFG_RX_TRIG_5   UDMA_ARB_4
+    // I2C_FIFO_CFG_RX_TRIG_6   UDMA_ARB_4
+    // I2C_FIFO_CFG_RX_TRIG_7   UDMA_ARB_4
+    //
+    ui32RxArbSize = UDMA_ARB_2;
+
+    //
+    // Flush any existing data in the FIFO
+    //
+    I2CTxFIFOFlush(I2C3_BASE);
+    I2CRxFIFOFlush(I2C3_BASE);
+
+    //
+    // Enable Interrupts for Arbitration Lost, Stop, NAK,
+    // Clock Low Timeout and Data.
+    //
+    I2CMasterIntEnableEx(I2C3_BASE, (I2C_MASTER_INT_ARB_LOST |
+            I2C_MASTER_INT_STOP | I2C_MASTER_INT_NACK |
+            I2C_MASTER_INT_TIMEOUT | I2C_MASTER_INT_DATA));
+
+    //
+    // Enable the Interrupt in the NVIC from I2C Master
+    //
+    IntEnable(INT_I2C3);
+
+    //
+    // Enable the Glitch Filter. Writting a value 0 will
+    // disable the glitch filter
+    // I2C_MASTER_GLITCH_FILTER_DISABLED
+    // I2C_MASTER_GLITCH_FILTER_1
+    // I2C_MASTER_GLITCH_FILTER_2 : Ideal Value when in HS Mode
+    //                              for 120MHz clock
+    // I2C_MASTER_GLITCH_FILTER_4
+    // I2C_MASTER_GLITCH_FILTER_8 : Ideal Value when in Std,
+    //                              Fast, Fast+ for 120MHz clock
+    // I2C_MASTER_GLITCH_FILTER_16
+    // I2C_MASTER_GLITCH_FILTER_32
+    //
+    I2CMasterGlitchFilterConfigSet(I2C3_BASE, I2C_MASTER_GLITCH_FILTER_8);
+
+    //
+    // Initialize and Configure the Master Module State Machine
+    //
+    g_ui8MasterCurrState = I2C_OP_IDLE;
+
+    return 0;
+
+}
+
+uint32_t I2C_Write_udmaFifo(uint32_t base, uint8_t addr, uint8_t *data, uint32_t len)
+{
+    uint8_t  ui8Count;
+    //uint32_t ui32TxArbSize;
+    SLAVE_ADDRESS_EXT = addr;
+    uint8_t  ui8MasterBytesLength;
+    //
+    // Check if the Bus is Busy or not
+    //
+    while(I2CMasterBusBusy(base));
+
+    //
+    // Randomly Initialize the Transmit buffer and
+    // set the receive buffer to 0xFF
+    //
+    for(ui8Count=0 ; ui8Count < len ; ui8Count++)
+    {
+        g_ui8MasterTxData[ui8Count]   = data[ui8Count];
+
+        //
+        // Change the Random Value for the next
+        // iteration..
+        //
+        ///RandomAddEntropy(RandomSeed());
+
+        //
+        // Init the receive buffers with the value
+        // 0xFF
+        //
+        ////g_ui8MasterRxData[ui8Count]   = 0xFF;
+    }
+
+    //
+    // When sending data to slave make sure
+    // RX DMA DONE interrupt bit is masked and
+    // TX DMA DONE interrupt bit is unmasked
+    //
+    I2CMasterIntEnableEx(base, I2C_MASTER_INT_TX_DMA_DONE);
+    I2CMasterIntDisableEx(base, I2C_MASTER_INT_RX_DMA_DONE);
+
+    //
+    // Set the I2CMBLEN register and also initialize
+    // the internal flag
+    //
+    ui8MasterBytesLength = 32;
+
+    I2CMasterBurstLengthSet(base, ui8MasterBytesLength);
+
+    //
+    // Configure the uDMA Control Channel Structure for TX
+    //
+    ConfigureuDMATX(ui8MasterBytesLength, ui32TxArbSize);
+
+    //
+    // Set Transmit Flag and set the Page Address in
+    // external slave to 0x0000
+    //
+    g_bI2CDirection        = false;
+    g_ui16SlaveWordAddress = 0x0;
+    g_ui8MasterBytes       = 0;
+
+    //
+    // Print Message before sending data
+    //
+    ///UARTprintf("Transmit %d bytes to external Slave...\n\n",ui8MasterBytesLength);
+
+    //
+    // Trigger the Transfer using Software Interrupt
+    //
+    g_ui8MasterCurrState = I2C_OP_IDLE;
+    IntTrigger(INT_I2C3);
+    while(g_ui8MasterCurrState != I2C_OP_STOP);
+
+    return 0;
+}
+
+uint32_t I2C_Read_udmaFifo(uint32_t base, uint8_t addr, uint8_t *data, uint32_t len)
+{
+    uint8_t  ui8Count;
+    bool     bError;
+    //uint32_t ui32RxArbSize;
+    uint8_t  ui8MasterBytesLength;
+
+    SLAVE_ADDRESS_EXT = addr;
+    //
+    // When receiving data from slave make sure
+    // TX DMA DONE interrupt bit is masked and
+    // RX DMA DONE interrupt bit is unmasked
+    //
+    I2CMasterIntEnableEx(base, I2C_MASTER_INT_RX_DMA_DONE);
+    I2CMasterIntDisableEx(base, I2C_MASTER_INT_TX_DMA_DONE);
+
+    //
+    // Set the I2CMBLEN register and also initialize
+    // the internal flag
+    //
+    ui8MasterBytesLength = 32;
+
+    I2CMasterBurstLengthSet(base, ui8MasterBytesLength);
+
+    //
+    // Configure the uDMA Control Channel Structure for RX
+    //
+    ConfigureuDMARX(ui8MasterBytesLength,ui32RxArbSize);
+
+    //
+    // Set receive Flag and set the Page Address in
+    // external slave to 0x0000
+    //
+    g_bI2CDirection        = true;
+    g_bI2CRepeatedStart    = true;
+    g_ui16SlaveWordAddress = 0x0;
+    g_ui8MasterBytes       = 0;
+
+    //
+    // Print Message before receiving data
+    //
+    ///UARTprintf("Receiving %d bytes from external Slave...\n\n",ui8MasterBytesLength);
+
+    //
+    // Trigger the Transfer using Software Interrupt
+    //
+    g_ui8MasterCurrState = I2C_OP_IDLE;
+    IntTrigger(INT_I2C3);
+    while(g_ui8MasterCurrState != I2C_OP_STOP);
+
+    //
+    // Perform Data Integrity Check...
+    //
+    bError = false;
+    for(ui8Count = 0 ; ui8Count < len ; ui8Count++)
+    {
+        data[ui8Count] = g_ui8MasterRxData[ui8Count];
+
+    }
+
+    return 0;
+
+}
+
+
+
diff --git a/Software/Embedded_SW/Embedded/Main.c b/Software/Embedded_SW/Embedded/Main.c
index 3be36668d..5c6c13686 100644
--- a/Software/Embedded_SW/Embedded/Main.c
+++ b/Software/Embedded_SW/Embedded/Main.c
@@ -84,7 +84,7 @@
 #include "Drivers/I2C_Communication/Head_Card/IO_Ports/Head_IO.h"
 #include "Drivers/I2C_Communication/Head_Card/EEPROM/Head_EEPROM.h"
 #include "modules/AlarmHandling/AlarmHandling.h"
-//#include <Drivers/I2C_Communication/I2C_Task.h>
+#include <Drivers/I2C_Communication/I2C_Task.h>
 #include <Drivers/I2C_Communication/WHS_Card/EEPROM/WHS_EEPROM.h>
 #include <Drivers/I2C_Communication/RFID_NFC/I2C_IFS_Mux.h>
 //*****************************************************************************
@@ -265,21 +265,23 @@ int main(void)
 
    Init_All_I2C();
    Block_Main_MUX_TCA9548A_0xE4();//unused, same i2c as head and whs cards
-   Check_Head_Type_Via_EEPROM();
+   Check_Head_Type_Via_EEPROM();//without trigger
+   Check_WHS_Type_Via_EEPROM();//without trigger
+   if ((Head_Type >= HEAD_TYPE_UNKNOWN_WITH_CARD)&&(WHS_Type == WHS_TYPE_NEW))
+   {
+       PP_Machine = true;
+   }
+   if ((Head_Type > HEAD_TYPE_SYLKO_WITHOUT_CARD)||(WHS_Type == WHS_TYPE_NEW))
+   {
+       I2C_ReadingTask_Init();
+   }
    if (Head_Type > HEAD_TYPE_SYLKO_WITHOUT_CARD)
    {
-       Head_IO_Init();//moved to set 24 to pressure sensor in arc earlier
+       Trigger_Head_io_Init();//Head_IO_Init();//moved to set 24 to pressure sensor in arc earlier
    }
 
-   //Trigger_Check_WHS_Type_Via_EEPROM();
-   Check_WHS_Type_Via_EEPROM();
-
    Check_IFS_Availability();
 
-   if ((Head_Type >= HEAD_TYPE_UNKNOWN_WITH_CARD)&&(WHS_Type == WHS_TYPE_NEW))
-   {
-       PP_Machine = true;
-   }
    #ifndef EVALUATION_BOARD
    STATUS_RED_LED_ON;
    STATUS_GREEN_LED_ON;
@@ -290,8 +292,6 @@ int main(void)
 
     SysCtlUSBPLLEnable();
 
-
-
 #ifndef EVALUATION_BOARD
     DeActivateAllSSR();
     Control_Dryer_Fan(STOP,0);//Preventing unwanted movement at power up, since the default from FPGA is "0" (= START)
@@ -310,8 +310,7 @@ int main(void)
 	//ActivateHeadMagnet();
     Set_Speed_Sensor_TypeII_Registers(10,5);//set default values
 #endif
-    if ((Head_Type > HEAD_TYPE_SYLKO_WITHOUT_CARD)||(WHS_Type == WHS_TYPE_NEW))
-        I2C_ReadingTask_Init();
+
 	//EMAC_initEMAC();
     ControlInit();
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
index 8a4960433..c27725b68 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
@@ -799,21 +799,21 @@ uint32_t HandleJobThreadControlParameters(ThreadParameters* ThreadParams)
         return OK;
     }
     if(ThreadParams->feederp)
-        MotorControlConfig[FEEDER_MOTOR].m_params.Kd = ThreadParams->feederp;
+        MotorControlConfig[FEEDER_MOTOR].m_params.Kp = ThreadParams->feederp;
     if(ThreadParams->feederi)
         MotorControlConfig[FEEDER_MOTOR].m_params.Ki = ThreadParams->feederi;
     if(ThreadParams->feederd)
         MotorControlConfig[FEEDER_MOTOR].m_params.Kd = ThreadParams->feederd;
 
     if(ThreadParams->pullerp)
-        MotorControlConfig[POOLER_MOTOR].m_params.Kd = ThreadParams->pullerp;
+        MotorControlConfig[POOLER_MOTOR].m_params.Kp = ThreadParams->pullerp;
     if(ThreadParams->pulleri)
         MotorControlConfig[POOLER_MOTOR].m_params.Ki = ThreadParams->pulleri;
     if(ThreadParams->pullerd)
         MotorControlConfig[POOLER_MOTOR].m_params.Kd = ThreadParams->pullerd;
 
     if(ThreadParams->winderp)
-        MotorControlConfig[WINDER_MOTOR].m_params.Kd = ThreadParams->winderp;
+        MotorControlConfig[WINDER_MOTOR].m_params.Kp = ThreadParams->winderp;
     if(ThreadParams->winderi)
         MotorControlConfig[WINDER_MOTOR].m_params.Ki = ThreadParams->winderi;
     if(ThreadParams->winderd)