index : Tango	software
	Twine softwares	Thomas Vanbesien

aboutsummaryrefslogtreecommitdiffstats

log msg author committer range

path: root/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c

blob: f9401c248787e1e69d0d0cf715bd174b6da1151f (plain)

pre { line-height: 125%; } td.linenos .normal { color: inherit; background-color: transparent; padding-left: 5px; padding-right: 5px; } span.linenos { color: inherit; background-color: transparent; padding-left: 5px; padding-right: 5px; } td.linenos .special { color: #000000; background-color: #ffffc0; padding-left: 5px; padding-right: 5px; } span.linenos.special { color: #000000; background-color: #ffffc0; padding-left: 5px; padding-right: 5px; } .highlight .hll { background-color: #ffffcc } .highlight .c { color: #888888 } /* Comment */ .highlight .err { color: #a61717; background-color: #e3d2d2 } /* Error */ .highlight .k { color: #008800; font-weight: bold } /* Keyword */ .highlight .ch { color: #888888 } /* Comment.Hashbang */ .highlight .cm { color: #888888 } /* Comment.Multiline */ .highlight .cp { color: #cc0000; font-weight: bold } /* Comment.Preproc */ .highlight .cpf { color: #888888 } /* Comment.PreprocFile */ .highlight .c1 { color: #888888 } /* Comment.Single */ .highlight .cs { color: #cc0000; font-weight: bold; background-color: #fff0f0 } /* Comment.Special */ .highlight .gd { color: #000000; background-color: #ffdddd } /* Generic.Deleted */ .highlight .ge { font-style: italic } /* Generic.Emph */ .highlight .ges { font-weight: bold; font-style: italic } /* Generic.EmphStrong */ .highlight .gr { color: #aa0000 } /* Generic.Error */ .highlight .gh { color: #333333 } /* Generic.Heading */ .highlight .gi { color: #000000; background-color: #ddffdd } /* Generic.Inserted */ .highlight .go { color: #888888 } /* Generic.Output */ .highlight .gp { color: #555555 } /* Generic.Prompt */ .highlight .gs { font-weight: bold } /* Generic.Strong */ .highlight .gu { color: #666666 } /* Generic.Subheading */ .highlight .gt { color: #aa0000 } /* Generic.Traceback */ .highlight .kc { color: #008800; font-weight: bold } /* Keyword.Constant */ .highlight .kd { color: #008800; font-weight: bold } /* Keyword.Declaration */ .highlight .kn { color: #008800; font-weight: bold } /* Keyword.Namespace */ .highlight .kp { color: #008800 } /* Keyword.Pseudo */ .highlight .kr { color: #008800; font-weight: bold } /* Keyword.Reserved */ .highlight .kt { color: #888888; font-weight: bold } /* Keyword.Type */ .highlight .m { color: #0000DD; font-weight: bold } /* Literal.Number */ .highlight .s { color: #dd2200; background-color: #fff0f0 } /* Literal.String */ .highlight .na { color: #336699 } /* Name.Attribute */ .highlight .nb { color: #003388 } /* Name.Builtin */ .highlight .nc { color: #bb0066; font-weight: bold } /* Name.Class */ .highlight .no { color: #003366; font-weight: bold } /* Name.Constant */ .highlight .nd { color: #555555 } /* Name.Decorator */ .highlight .ne { color: #bb0066; font-weight: bold } /* Name.Exception */ .highlight .nf { color: #0066bb; font-weight: bold } /* Name.Function */ .highlight .nl { color: #336699; font-style: italic } /* Name.Label */ .highlight .nn { color: #bb0066; font-weight: bold } /* Name.Namespace */ .highlight .py { color: #336699; font-weight: bold } /* Name.Property */ .highlight .nt { color: #bb0066; font-weight: bold } /* Name.Tag */ .highlight .nv { color: #336699 } /* Name.Variable */ .highlight .ow { color: #008800 } /* Operator.Word */ .highlight .w { color: #bbbbbb } /* Text.Whitespace */ .highlight .mb { color: #0000DD; font-weight: bold } /* Literal.Number.Bin */ .highlight .mf { color: #0000DD; font-weight: bold } /* Literal.Number.Float */ .highlight .mh { color: #0000DD; font-weight: bold } /* Literal.Number.Hex */ .highlight .mi { color: #0000DD; font-weight: bold } /* Literal.Number.Integer */ .highlight .mo { color: #0000DD; font-weight: bold } /* Literal.Number.Oct */ .highlight .sa { color: #dd2200; background-color: #fff0f0 } /* Literal.String.Affix */ .highlight .sb { color: #dd2200; background-color: #fff0f0 } /* Literal.String.Backtick */ .highlight .sc { color: #dd2200; background-color: #fff0f0 } /* Literal.String.Char */ .highlight .dl { color: #dd2200; background-color: #fff0f0 } /* Literal.String.Delimiter */ .highlight .sd { color: #dd2200; background-color: #fff0f0 } /* Literal.String.Doc */ .highlight .s2 { color: #dd2200; background-color: #fff0f0 } /* Literal.String.Double */ .highlight .se { color: #0044dd; background-color: #fff0f0 } /* Literal.String.Escape */ .highlight .sh { color: #dd2200; background-color: #fff0f0 } /* Literal.String.Heredoc */ .highlight .si { color: #3333bb; background-color: #fff0f0 } /* Literal.String.Interpol */ .highlight .sx { color: #22bb22; background-color: #f0fff0 } /* Literal.String.Other */ .highlight .sr { color: #008800; background-color: #fff0ff } /* Literal.String.Regex */ .highlight .s1 { color: #dd2200; background-color: #fff0f0 } /* Literal.String.Single */ .highlight .ss { color: #aa6600; background-color: #fff0f0 } /* Literal.String.Symbol */ .highlight .bp { color: #003388 } /* Name.Builtin.Pseudo */ .highlight .fm { color: #0066bb; font-weight: bold } /* Name.Function.Magic */ .highlight .vc { color: #336699 } /* Name.Variable.Class */ .highlight .vg { color: #dd7700 } /* Name.Variable.Global */ .highlight .vi { color: #3333bb } /* Name.Variable.Instance */ .highlight .vm { color: #336699 } /* Name.Variable.Magic */ .highlight .il { color: #0000DD; font-weight: bold } /* Literal.Number.Integer.Long */ <?xml version="1.0" encoding="utf-8"?> <packages> <package id="CommonServiceLocator" version="1.3" targetFramework="net46" /> <package id="EntityFramework" version="6.2.0" targetFramework="net461" /> <package id="Expression.Blend.Sdk" version="1.0.2" targetFramework="net46" /> <package id="MahApps.Metro" version="1.5.0" targetFramework="net46" /> <package id="MaterialDesignColors" version="1.1.2" targetFramework="net46" /> <package id="MaterialDesignThemes" version="2.3.1.953" targetFramework="net46" /> </packages>

generated by cgit v1.3.1 (git 2.54.0) at 2026-07-11 12:48:51 +0000

/************************************************************************************************************************
 * Millisec.c
 * Millisec module
 *
 * The millisec task is called once every 1 millisecond to gather data from the FPGA crucial modules,
 * so it will be ready for the Millisec operations. it is called 300 (TBD, configurable)
 * microsecond before the Millisec task, so that the data will be ready for the Millisec handling.
 * the ,millisecond task holds the pointer to a double buffer of results, so that the Millisec will handle the
 * most updated data, without disturbing data gathering
 *
 **************************************************************************************************************************/

////////////////////////////////State machine operation////////////////////////////////////
//the state machine operation is used to operate in runtime correct profile flow execution
//by recieved design flow of the user from the UI
///////////////////////////////////////////////////////////////////////////////////////////
#include "include.h"
#include "Modules/General/GeneralHardware.h"

#include "Modules/AlarmHandling/AlarmHandling.h"
#include "Modules/General/MachineStatus.h"

#include "MillisecTask.h"
#include <driverlib/timer.h>
#include <time.h>
#include "Common/Utilities/Utils.h"
#include "driverlib/hibernate.h"

#include <Drivers/SSI_Comm/SSI_Comm.h>
#include <Drivers/SSI_Comm/Speed_Sensor/Speed_Sensor.h>
#include <Drivers/SSI_Comm/Dancer/Dancer.h>
#include "drivers/I2C_Communication/ADC_MUX/ADC_MUX.h"
#include <inc/hw_ints.h>

#include "drivers/adc_sampling/adc.h"
#include "control.h"

#include "drivers/Motors/Motor.h"
#include "drivers/Heater/TemperatureSensor.h"
#include "drivers/FPGA/FPGA_SPI_Comm.h"
#include "drivers/FPGA/FPGA.h"
#include <drivers/FPGA/FPGA_Comm.h>
#include "drivers/Valves/Valve.h"
#include "drivers/FPGA/FPGA_GPIO/FPGA_GPIO.h"
#include "Drivers/I2C_Communication/I2C.h"

#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/fatfs/ff.h"
#include "drivers/Flash_ram/FlashProgram.h"

#include "Common/SWUpdate/FileSystem.h"

#include "Communication/Connection.h"
#include "Modules/General/buttons.h"
#include "Drivers/I2C_Communication/Head_Card/PT100/Head_PT100_ADC.h"

#include <Drivers/I2C_Communication/WHS_Card/D_ADS122_PT100/WHS_PT100_ADC.h>
#include <Drivers/I2C_Communication/I2C_Task.h>
#include <Drivers/I2C_Communication/WHS_Card/D_Max5805_ADC_Blower/WHS_Blower.h>
#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>
#include "Drivers/Uart_Comm/BTSR/BTSR.h"

Task_Handle Millisecond_Task_Handle;
/******************** Definitions  ********************************************/

/******************** STRUCTURES AND ENUMs  ********************************************/
typedef struct MillisecMotorData
{
    bool WaitForData;
    bool DataRequired;
    bool SyncRequired;
    MSecFptr Callback;
    unsigned long Data;
    int Length;
    bool Active;
}MillisecMotorDataStruc;
typedef enum
{
    OneMillisec,
}MillisecMessages;

typedef struct MillisecMessage{
    uint16_t messageId;
    uint16_t msglen;
    uint32_t tick;
    uint8_t messageData[20];
}MillisecMessageStruc;

//uint32_t  ADC_Data[MAX_ADC_DEVICES] = {0};
//uint32_t  TemperatureSensor_Data[MAX_MAIN_CARD_TEMPERATURE_SENSOR_ID] = {0};
//uint32_t  MotorSpeed_Data[NUM_OF_MOTORS] = {0};
//uint32_t  MotorStatus_Data[NUM_OF_MOTORS] = {0};
//uint32_t  MotorPosition_Data[NUM_OF_MOTORS] = {0};
//bool      MotorBusy_Data[NUM_OF_MOTORS] = {true};
uint32_t  Dancer_Data[NUM_OF_DANCERS] = {0};
float  Speed_Data = 0;
uint32_t DrawerFansStatus = 0;
uint32_t SystemFansStatus = 0;
bool watchdogCriticalAlarm = false;

uint32_t msec_millisecondCounter = 0;

extern bool Machine_Idle_Mode;

uint8_t Gas_PPM_Info;
MillisecMotorDataStruc ScrewSetMaxSpeedPending = {0};
MillisecMotorDataStruc ScrewMovePending = {0};
MillisecMotorDataStruc MotorData[NUM_OF_MOTORS] = {0};
MillisecMotorDataStruc SpeedSetPending[NUM_OF_MOTORS] = {0};
MillisecMotorDataStruc PT100Data[MAX_MAIN_CARD_TEMP_SENS_ID] = {0};
/******************** GLOBAL PARAMETERS  ********************************************/
Mailbox_Handle          MillisecMsgQ = NULL;
Mailbox_Handle          TenMillisecMsgQ = NULL;
Mailbox_Handle          MotorsMsgQ[NUM_OF_MOTORS] = {NULL};
bool                    MillisecRestart;
static GateMutex_Handle gateMillisecDB;

uint32_t        Millisec_timerBase = TIMER1_BASE;        //Timer handle
/******************** Functions  ********************************************/
//**********************************************************************
/******************** CODE  ********************************************/
//**********************************************************************
#define LOG_SIZE 200
uint32_t MSBacklog[LOG_SIZE+1]={0};
uint8_t Motor_Id[LOG_SIZE+1]={0};
uint32_t MSTick[LOG_SIZE+1]={0};
uint16_t MsecLogindex = 0;

void MillisecInit(void)
{
    Error_Block eb;
    int i;

    Error_init(&eb);

    MillisecMsgQ = Mailbox_create(sizeof(MillisecMessageStruc), 2, NULL,&eb);
    TenMillisecMsgQ = Mailbox_create(sizeof(MillisecMessageStruc), 2, NULL,&eb);
    for (i=0;i<NUM_OF_MOTORS;i++)
    {
        MotorsMsgQ[i] = Mailbox_create(sizeof(MillisecMotorDataStruc), 5, NULL,&eb);
    }

    MillisecRestart = false;


    gateMillisecDB = GateMutex_create(NULL, &eb);
    if (gateMillisecDB == NULL)
    {
        System_abort("Could not create USB Wait gate");
    }

    ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_TIMER1);
    ROM_TimerConfigure(Millisec_timerBase, TIMER_CFG_PERIODIC);   // 32 bits Timer
    //TimerIntRegister(Millisec_timerBase, TIMER_A, Timer0Isr);    // Registering  isr
    ROM_TimerEnable(Millisec_timerBase, TIMER_A);
    ROM_IntEnable(INT_TIMER1A);
    ROM_TimerIntEnable(Millisec_timerBase, TIMER_TIMA_TIMEOUT);

    ADCAcquireInit();

    return;
}
void MillisecStop(void)
{
    MillisecRestart = false;
    ADCAcquireStop();
}
#ifdef HUNDRED_MICROSECONDS_DANCER_READ
#define HIGH_TASK_FREQUENCY 12000
#else
#define HIGH_TASK_FREQUENCY 120000
#endif
void MillisecStart(void)
{
    MillisecRestart = true;

    ROM_TimerLoadSet(Millisec_timerBase, TIMER_A,HIGH_TASK_FREQUENCY);
    ROM_TimerEnable(Millisec_timerBase, TIMER_A);
    ROM_IntEnable(INT_TIMER1A);
    ROM_TimerIntEnable(Millisec_timerBase, TIMER_TIMA_TIMEOUT);
    ADCAcquireStart(0,1);

}

void OneMilliSecondMillisecInterrupt(UArg arg0)
{
    MillisecMessageStruc MillisecMessage;
    uint32_t Tenmsec_millisecondCounter = 0;
#ifndef HUNDRED_MICROSECONDS_DANCER_READ

    ROM_IntMasterDisable();
    ROM_TimerIntClear(Millisec_timerBase, TIMER_TIMA_TIMEOUT);  // Clear the timer interrupt
    if (MillisecRestart == true)
    {
        ROM_TimerLoadSet(Millisec_timerBase, TIMER_A,HIGH_TASK_FREQUENCY);
    }
    else
	{
        ROM_TimerDisable(Millisec_timerBase,TIMER_A);
	    ROM_IntMasterEnable();
		return;
	}
#endif
    if (msec_millisecondCounter%10 == 0)
    {
        Tenmsec_millisecondCounter = msec_millisecondCounter;
    }
    //send message to the Millisec task
    MillisecMessage.messageId = OneMillisec;
    MillisecMessage.tick = msec_millisecondCounter++;
    MillisecMessage.msglen = sizeof(MillisecMessageStruc);
    if (MillisecMsgQ != NULL)
        Mailbox_post(MillisecMsgQ , &MillisecMessage, BIOS_NO_WAIT);

    if (Tenmsec_millisecondCounter)
    {
        MillisecMessage.messageId = OneMillisec;
        MillisecMessage.tick = Tenmsec_millisecondCounter;
        MillisecMessage.msglen = sizeof(MillisecMessageStruc);
        if (TenMillisecMsgQ != NULL)
            Mailbox_post(TenMillisecMsgQ , &MillisecMessage, BIOS_NO_WAIT);
    }

    //
    // Enable all interrupts.
    //
    ROM_IntMasterEnable();
    return ;
}
uint32_t PT100Activity = 0;

int32_t MillisecReadFromTempSensor(uint32_t TempSensorId,  MSecFptr Callback)
{
    if (TempSensorId >= MAX_MAIN_CARD_TEMP_SENS_ID) return -1;
    PT100Activity++; //read request
    PT100Data[TempSensorId].Callback = Callback;
    PT100Data[TempSensorId].Active = true;
    return OK;
}
//typedef  uint32_t (* MSecFptr)(uint32_t deviceID, uint32_t ReadValue);
uint32_t MotorActivity = 0;

int32_t MillisecSetMotorSpeed(TimerMotors_t MotorId, unsigned long Data, int Length, MSecFptr Callback)
{
    if (MotorId >= NUM_OF_MOTORS) return -1;
    //==========================
    if ((MotorId == HARDWARE_MOTOR_TYPE__MOTO_WINDER)||
        (MotorId == HARDWARE_MOTOR_TYPE__MOTO_RDRIVING)||
        (MotorId == HARDWARE_MOTOR_TYPE__MOTO_LDRIVING)||
		(MotorId == HARDWARE_MOTOR_TYPE__MOTO_SCREW))
    {
    MSBacklog[MsecLogindex]=Data;
    MSTick[MsecLogindex]=msec_millisecondCounter;
    Motor_Id[MsecLogindex]=MotorId;
    MsecLogindex++;
    if (MsecLogindex>=LOG_SIZE)
        MsecLogindex = 0;
    }
    //==========================

    SpeedSetPending[MotorId].Callback = Callback;
    SpeedSetPending[MotorId].Data = Data;
    SpeedSetPending[MotorId].Length = Length;
    SpeedSetPending[MotorId].DataRequired = false;
    if (SpeedSetPending[MotorId].Active == false)
    {
        MotorActivity++;
        SpeedSetPending[MotorId].Active = true;
    }

    return OK;
}
int MillisecFlushMsgQ(TimerMotors_t MotorId)
{
    MillisecMotorDataStruc MotorInfo = {0};
    int pend = Mailbox_getNumPendingMsgs(MotorsMsgQ[MotorId]);
    int i;
    if (pend)
    {
        for (i=0;i<pend;i++)
            Mailbox_pend(MotorsMsgQ[MotorId] , &MotorInfo, BIOS_NO_WAIT);
    }
   return pend;
}
int32_t MillisecWriteToMotor(TimerMotors_t MotorId, unsigned long Data, int Length, MSecFptr Callback)
{
    MillisecMotorDataStruc MotorInfo = {0};
    //==========================
    if ((MotorId == HARDWARE_MOTOR_TYPE__MOTO_WINDER)||
        (MotorId == HARDWARE_MOTOR_TYPE__MOTO_RDRIVING)||
        (MotorId == HARDWARE_MOTOR_TYPE__MOTO_LDRIVING)||
		(MotorId == HARDWARE_MOTOR_TYPE__MOTO_SCREW))
    {
    MSBacklog[MsecLogindex]=Data;
    MSTick[MsecLogindex]=msec_millisecondCounter;
    Motor_Id[MsecLogindex]=MotorId;
    MsecLogindex++;
    if (MsecLogindex>=LOG_SIZE)
        MsecLogindex = 0;
    }
    //==========================
    if (MotorId >= NUM_OF_MOTORS) return -1;
    MotorActivity++;
    MotorInfo.Callback = Callback;
    MotorInfo.Data = Data;
    MotorInfo.Length = Length;
    MotorInfo.DataRequired = false;
    if (MotorsMsgQ[MotorId] != NULL)
        return Mailbox_post(MotorsMsgQ[MotorId] , &MotorInfo, BIOS_NO_WAIT);
    else return false;
}
int32_t MillisecReadFromMotor(TimerMotors_t MotorId, unsigned long Data, int Length, MSecFptr Callback)
{
    MillisecMotorDataStruc MotorInfo = {0};
    if (MotorId >= NUM_OF_MOTORS) return -1;
//==========================
    if ((MotorId == HARDWARE_MOTOR_TYPE__MOTO_WINDER)||
        (MotorId == HARDWARE_MOTOR_TYPE__MOTO_RDRIVING)||
        (MotorId == HARDWARE_MOTOR_TYPE__MOTO_LDRIVING)||
		(MotorId == HARDWARE_MOTOR_TYPE__MOTO_SCREW))
    {
    MSBacklog[MsecLogindex]=Data;
    Motor_Id[MsecLogindex]=MotorId;
    MSTick[MsecLogindex]=msec_millisecondCounter;
    MsecLogindex++;
    if (MsecLogindex>=LOG_SIZE)
        MsecLogindex = 0;
    }
    //==========================
    MotorActivity++;
    MotorActivity++;
    MotorInfo.Callback = Callback;
    MotorInfo.Data = Data;
    MotorInfo.Length = Length;
    MotorInfo.DataRequired = true;
    if (MotorsMsgQ[MotorId] != NULL)
        return Mailbox_post(MotorsMsgQ[MotorId] , &MotorInfo, BIOS_NO_WAIT);
    else return false;
}

uint32_t MillisecLoop(uint32_t tick)
{
    uint8_t Motor_i;
    TEMPERATURE_SENSOR_ID_ENUM Sensor_i;
    unsigned int MotorInfo = 0;
    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, Hundred_msTick, Onesecond_Tick,O900Millisecond_Tick,Tick98,OneMinute_Tick;
    Ten_msTick      =   (tick%eTenMillisecond == 0)      ?true:false;
    Hundred_msTick  =   (tick%eHundredMillisecond == 0)  ?true:false;
    O900Millisecond_Tick  =   (tick%eOneSecond == 900)           ?true:false;
    Onesecond_Tick  =   (tick%eOneSecond == 0)           ?true:false;
    OneMinute_Tick  =   (tick%eOneMinute == 0)           ?true:false;
    Tick98          =   (tick%eHundredMillisecond == 99) ?true:false;
    //gather Motor data from FPGA
    //ROM_IntMasterDisable();
*/
#ifndef EVALUATION_BOARD
        FPGA_GetBusy();  //load the busy motor information to all motors
        FPGA_Read_LS_Safty_Ind_Reg();
        //Read_FPGA_GPI_Rgisters();//FPGA_Read_limit_Switches();
#endif

        temp += MotorActivity;
   if (MotorActivity)
   {
        for (Motor_i = 0;Motor_i < NUM_OF_MOTORS;Motor_i++)
        {
            if (MotorDriverResponse[Motor_i].Busy == true)
            {
                temp++;
                continue;
            }
            if (MotorData[Motor_i].WaitForData == true) //Read request sent, data is waiting
            {
                if (MotorGetFPGAResponse((HardwareMotorType)Motor_i,&MotorInfo) == OK) //got the data from the FPGA
                {
                    MotorData[Motor_i].WaitForData = false;
                    if (MotorData[Motor_i].Callback)
                        MotorData[Motor_i].Callback(Motor_i,MotorInfo);
                }
                MotorActivity--;
            }
            if (SpeedSetPending[Motor_i].Active == true)
            {
                MotorSendFPGARequest((HardwareMotorType)Motor_i,SpeedSetPending[Motor_i].Data,SpeedSetPending[Motor_i].Length);
                MotorActivity--;
                SpeedSetPending[Motor_i].Active = false;
                if (SpeedSetPending[Motor_i].Callback)
                    SpeedSetPending[Motor_i].Callback(Motor_i,0);
            }
            else if (Mailbox_pend(MotorsMsgQ[Motor_i] , &MotorData[Motor_i], BIOS_NO_WAIT)==true)
            {
                if (MotorSendFPGARequest((HardwareMotorType)Motor_i,MotorData[Motor_i].Data,MotorData[Motor_i].Length) == OK) //sent the data to the FPGA
                {
                    if (MotorData[Motor_i].DataRequired == true)
                    {
                        MotorData[Motor_i].WaitForData = true; // mark the motor for data request next round
                    }
                    else
                    {
                        if (MotorData[Motor_i].Callback)
                            MotorData[Motor_i].Callback(Motor_i,0); // call the callback to report execution
                    }
                }
                MotorActivity--;
            }
        }
   }
    //FPGA_GetTempSensorBusy();
   //int PT100Busy[MAX_MAIN_CARD_TEMPERATURE_SENSOR_ID] = {0,0,0,0,0,0,0,0,0,0};
   if (PT100Activity)
   {

       for (Sensor_i = 0;Sensor_i < MAX_MAIN_CARD_TEMP_SENS_ID;Sensor_i++)
       {
           /*if (TempSensorResponse[Sensor_i].Busy == true)
           {
               PT100Busy[Sensor_i]++;
               continue;
           }*/
           if (PT100Data[Sensor_i].Active == true)
           {
               if(Sensor_i < MAX_MAIN_CARD_TEMP_SENS_ID)
               {
                   TemperatureSendSensorDummyClk(Sensor_i);
               }
               PT100Data[Sensor_i].Active = false;
               PT100Data[Sensor_i].WaitForData = true; // mark the motor for data request next round
               break; //  one PT100 activitiy per MS 
           }
           else if (PT100Data[Sensor_i].WaitForData == true) //Read request sent, data is waiting
           {
               if(Sensor_i < MAX_MAIN_CARD_TEMP_SENS_ID)
               {
                   TemperatureSensorReadFromFPGA_Res(Sensor_i); //got the data from the FPGA
               }
               /*else if(Sensor_i < MAX_HEAD_CARD_TEMP_SENS_ID) - move to the function  control_HeadCard_PT100
               {
                   //HeadCard_Toggle_PT100(Sensor_i);
                   //delay??
                   //HeadADCPT100_SendReadDataCommand(Sensor_i);
               }
               else if(Sensor_i < MAX_WHS_CARD_TEMP_SENS_ID)
               {
                   //for WHS...
               }
               else
               {
                   //TBD
               }*/

               PT100Data[Sensor_i].WaitForData = false;
               PT100Data[Sensor_i].SyncRequired = true;
               if (PT100Data[Sensor_i].Callback)
                       PT100Data[Sensor_i].Callback(Sensor_i,MotorInfo);
               break; // one PT100 activitiy per MS 
           }
           else if (PT100Data[Sensor_i].SyncRequired == true)
           {
               if(Sensor_i < MAX_MAIN_CARD_TEMP_SENS_ID)
               {
                   TemperatureSensorSync(Sensor_i);
               }
               /*else if(Sensor_i < MAX_HEAD_CARD_TEMP_SENS_ID)  - NA move to the function  control_HeadCard_PT100
               {
                   //HeadADCPT100_StartSync(Sensor_i);
               }
               else if(Sensor_i < MAX_WHS_CARD_TEMP_SENS_ID)
               {
                   //StartSync WHS...
               }
               else
               {
                   //TBD...
               }*/


               PT100Data[Sensor_i].SyncRequired = false;
               PT100Activity--;
               break; // one PT100 activitiy per MS 
           }
       }
   }
#ifdef HUNDRED_MICROSECONDS_DANCER_READ
   SaveLogData();
#else

    #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;
}
int TemperatureSum[MAX_HEAD_CARD_TEMP_SENS_ID];
int TemperatureMin[MAX_HEAD_CARD_TEMP_SENS_ID];
int TemperatureMax[MAX_HEAD_CARD_TEMP_SENS_ID];
int TemperatureCount[MAX_HEAD_CARD_TEMP_SENS_ID];
int TemperatureCalc[MAX_HEAD_CARD_TEMP_SENS_ID] = {2.1,2.1,2.1,2.1,2.1,2.1,2.1,2.1,2.1,2.1,2.1,2.1,2.1,2.1,2.1,2.1,2.1,2.1,2.1,2.1,2.1,2.1,2.1,2.1,2.1,2.1,2.1};
void MillisecUpdateTemperatures (TEMPERATURE_SENSOR_ID_ENUM SensorId,int temperature)
{
   //if(TemperatureCount[SensorId]++>=10)
   //    TemperatureCount[SensorId] = 0;
    if ((temperature>28000)||(temperature<900))
        return;
    TemperatureCount[SensorId]++;
    if (TemperatureMax[SensorId]<temperature) TemperatureMax[SensorId]=temperature;
    if (TemperatureMin[SensorId]>temperature) TemperatureMin[SensorId]=temperature;
    TemperatureSum[SensorId]+=temperature;
}
int MillisecCalculateTemperatures (TEMPERATURE_SENSOR_ID_ENUM SensorId)
{
    int calc = 0;
    TemperatureSum[SensorId]-=TemperatureMax[SensorId];
    TemperatureSum[SensorId]-=TemperatureMin[SensorId];
    calc = TemperatureSum[SensorId] / (TemperatureCount[SensorId]-2);
    if (TemperatureSum[SensorId]>10)
    {

        if(abs(TemperatureMax[SensorId]-TemperatureMin[SensorId])>2000)
        {
            if ((Head_Type == HEAD_TYPE_FLAT_WITHOUT_CARD)||(SensorId!=TEMP_SENSE_AN_ENCLOSURETEMP3))
                Report("Millisec Temp spike",__FILE__,SensorId,(int)TemperatureMax[SensorId],RpWarning,(int) TemperatureMin[SensorId],0);
        }
    }
    TemperatureSum[SensorId] = 0;
    TemperatureCount[SensorId] = 0;
    TemperatureMin[SensorId] = 30000;
    TemperatureMax[SensorId] = -30000;
    return calc;
}
int MillisecGetTemperatures (TEMPERATURE_SENSOR_ID_ENUM SensorId)
{
    if (SensorId > WHS_PT100_4_0X82_1)
        return 0;
    return TemperatureCalc[SensorId];
}
bool RapidPressureRead = true;
float PressureSum[MAX_SYSTEM_DISPENSERS];
float PressureMin[MAX_SYSTEM_DISPENSERS];
float PressureMax[MAX_SYSTEM_DISPENSERS];
float PressureCount[MAX_SYSTEM_DISPENSERS];
float PressureCalc[MAX_SYSTEM_DISPENSERS];
void MillisecUpdatePressures (int SensorId,float Pressure)
{
   //if(PressureCount[SensorId]++>=10)
   //    PressureCount[SensorId] = 0;
    PressureCount[SensorId]++;
    if (PressureMax[SensorId]<Pressure) PressureMax[SensorId]=Pressure;
    if (PressureMin[SensorId]>Pressure) PressureMin[SensorId]=Pressure;
    PressureSum[SensorId]+=Pressure;
}
float MillisecCalculatePressures (int SensorId)
{
    float calc = 0;
    PressureSum[SensorId]-=PressureMax[SensorId];
    PressureSum[SensorId]-=PressureMin[SensorId];
    calc = PressureSum[SensorId] / (PressureCount[SensorId]-2);

    PressureSum[SensorId] = 0;
    PressureCount[SensorId] = 0;
    PressureMin[SensorId] = 30000;
    PressureMax[SensorId] = -30000;
    return calc;
}
float MillisecGetPressures (int SensorId)
{
    if (SensorId > MAX_SYSTEM_DISPENSERS)
        return 0;
    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];
uint16_t dbgflag[50];
uint32_t dbttime[50];
bool Ten_msTick, Fifty_msTick, Hundred_msTick , m20msecTick,m90msecTick, Onesecond_Tick,Tensecond_Tick,OneMinute_Tick,TenMinutes_Tick,OneHourTick,Gradient_Tick;
bool O700Millisecond_Tick,O200Millisecond_Tick,O400Millisecond_Tick,O500Millisecond_Tick,O600Millisecond_Tick,O800Millisecond_Tick,m50msecTick;

uint32_t MillisecLowLoop(uint32_t tick)
{
    uint8_t Motor_i,Disp_i,temp;
    TEMPERATURE_SENSOR_ID_ENUM Sensor_i;
#ifdef USE_RFID_LOGIC
    RFID_READER_ID readerID;
#endif
    //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 O100Millisecond_Tick,O200Millisecond_Tick,O400Millisecond_Tick,O500Millisecond_Tick,O600Millisecond_Tick,O800Millisecond_Tick,O900Millisecond_Tick;
    Ten_msTick      =   (tick%eTenMillisecond == 0)      ?true:false;
    Fifty_msTick      =   (tick%eHundredMillisecond == 40)      ?true:false; //eFiftyMillisecond
    Hundred_msTick  =   (tick%eHundredMillisecond == 0)  ?true:false;
    m20msecTick  =   (tick%eHundredMillisecond == 20)  ?true:false;
    m50msecTick  =   (tick%50== 0)  ?true:false;
    m90msecTick  =   (tick%eHundredMillisecond == 90)  ?true:false;
    O700Millisecond_Tick  =   (tick%eOneSecond == 700)           ?true:false;
    O200Millisecond_Tick  =   (tick%eOneSecond == 200)           ?true:false;
    O400Millisecond_Tick  =   (tick%eOneSecond == 400)           ?true:false;
    O500Millisecond_Tick  =   (tick%eOneSecond == 500)           ?true:false;
    O600Millisecond_Tick  =   (tick%eOneSecond == 600)           ?true:false;
    O800Millisecond_Tick  =   (tick%eOneSecond == 800)           ?true:false;
    //O900Millisecond_Tick  =   (tick%eOneSecond == 900)           ?true:false;
    Gradient_Tick  =   (tick%400 == 0)           ?true:false;
    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]++;
    dbttime[0] = msec_millisecondCounter;
    //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_FLAT_WITHOUT_CARD)
        StartPT100 = TEMP_SENSE_ANALOG_DRYER_TEMP1;
    if (Ten_msTick)
    {
        //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();
        dbgflag[1]++;
        dbttime[1] = msec_millisecondCounter;
        Trigger_HeaterWriting();
        dbgflag[2]++;
        dbttime[2] = msec_millisecondCounter;
        Read_Dryer_ENC_Position();
        dbgflag[3]++;
    }
    if (m50msecTick) //50 milli
    {
#ifdef USE_UART4_FOR_BTSR
        //call every 50mSec
        BTSR_State_Machine(RUFeeder1);
#endif
    }
    if (m20msecTick)
    {
        ADC0SS0Handler();
    }
    if(Fifty_msTick)
    {
        WHS_Read_GPI_Registers();
        Trigger_PT100_Read();//call every 50mSec (minimum delay 30mSec)
        //Set_HeadCard_PT100();//call every 50mSec (minimum delay 30mSec)
    }
    if (m90msecTick)
    {
        for (Sensor_i = StartPT100;Sensor_i < MAX_HEAD_CARD_TEMP_SENS_ID;Sensor_i++)
        {
            MillisecReadFromTempSensor(Sensor_i, NULL);
        }
    }
    if (Hundred_msTick)
    {
        Speed_Data = Calculate_Speed_Sensor_Velocity();
#ifndef EVALUATION_BOARD
        Read_Buttons_Reg();
#endif
        ///////////////////////////////////////////////////////////////////


        for (Sensor_i = StartPT100;Sensor_i < MAX_HEAD_CARD_TEMP_SENS_ID;Sensor_i++)
        {
            MillisecUpdateTemperatures (Sensor_i,TemperatureSensorRead(Sensor_i));
        }
        if (GeneralHwReady == true)
        {
            if (watchdogCriticalAlarm == false)
            {
                Control_WD(ENABLE,10);  //activate heaters/dispenser watchdog, 0.5 seconds
                //LOG_ERROR (1111, "Control_WD");
            }
        }
        dbgflag[3]++;
        dbttime[3] = msec_millisecondCounter;
        if (RapidPressureRead == true)
        {
            for (Disp_i = 0;Disp_i < MAX_SYSTEM_DISPENSERS;Disp_i++)
            {
                MillisecUpdatePressures(Disp_i, CalculateDispenserPressure(Disp_i));
            }
            ADC_TriggerCollection();
        }

        Trigger_InputsReading();

        #ifdef USE_RFID_LOGIC
            //every 100 m sec
            for(readerID = READER_1; readerID < Max_Readers ; readerID++)
            {
                if(DiscoverRFIDTagEvery100mSec[readerID] == true)
                {
                    RFIDCallEvery100mSec(readerID);
                    //break;
                }
            }
        #endif
    }
    if (Gradient_Tick)
        DispensersCollectionCall();
    if (O200Millisecond_Tick)
    {

        ulocaltime(umktime(&LocalTime)+1,&LocalTime);
        HibernateCalendarSet(&LocalTime);
        //ReportWithPackageFilter(ThreadFilter,"Time: ",__FILE__,LocalTime.tm_min,LocalTime.tm_sec,RpWarning,(int) msec_millisecondCounter,0);

        Trigger_WHS_MAX11614_Read_allADC();
        FPGA_GetAllDispensersValveBusyOCD();
        Read_Dryer_Fan_Tacho();
        temp = Read_Fans_Tacho();
        DrawerFansStatus = temp & 0x1F;
        SystemFansStatus = temp & 0xE0;
    }
    if (O400Millisecond_Tick)
    {
        for (Motor_i = 0;Motor_i < NUM_OF_MOTORS;Motor_i++)
        {
            if (Motor_i == HARDWARE_MOTOR_TYPE__MOTO_SCREW)
                continue;  //
            if (isMotorConfigured(Motor_i))
                MotorGetStatusFromFPGA(Motor_i);
        }
    }
    if ((O500Millisecond_Tick)&&(RapidPressureRead == false))
    {
        ADC_TriggerCollection();
    }
    if (O600Millisecond_Tick)
    {
        Trigger_WHSReadAllFanTacho ();
        DrierHeaterVoltageSetup();
        if (RapidPressureRead == false)
        {
            for (Disp_i = 0;Disp_i < MAX_SYSTEM_DISPENSERS;Disp_i++)
            {
                CalculateDispenserPressure(Disp_i);
            }
        }
    }
    if (O700Millisecond_Tick)
    {
        Trigger_Heater_Current_Read();
    }
    if (O800Millisecond_Tick)
    {
        AlarmHandling_ControlTrigger(0,0);
    }
    if (Onesecond_Tick)
    {
        //char Lenstr[160];
        //static int Counter = 0;
        //MachineUpdateResponseFunc();
        KeepAliveOneSecondCall();
        //TemperatureListString(Lenstr);
        //ReportWithPackageFilter(ThreadFilter,Lenstr,__FILE__,__LINE__,(int)Counter++,RpWarning,(int) msec_millisecondCounter,0);
        dbgflag[4]++;
        dbttime[4] = msec_millisecondCounter;
        for (Sensor_i = StartPT100;Sensor_i < MAX_HEAD_CARD_TEMP_SENS_ID;Sensor_i++)
        {
            TemperatureCalc[Sensor_i] =  MillisecCalculateTemperatures ( Sensor_i);
        }
        if (RapidPressureRead == true)
        {
            for (Disp_i = 0;Disp_i < MAX_SYSTEM_DISPENSERS;Disp_i++)
            {
                PressureCalc[Disp_i] = MillisecCalculatePressures(Disp_i);
            }
        }
        HeaterPrepareFailureCheck();
        if (WHS_Type == WHS_TYPE_NEW)
        {
            //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
            if(Whs_emptying_cycle >= 2)
            {
                waste_seq_step1();// include 1Sec delay
                Whs_emptying_cycle = 0;
            }
            else
            {
                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();
        }
        else
        {
            FlatHeadAlarms();
        }

        //call waste state machine
        Waste_StateMachine_OneSecond_Call();

        //call IFS state machine
        midTankStateMachine();
        dbgflag[5]++;
        dbttime[5] = msec_millisecondCounter;
    }
    if (Tensecond_Tick)
    {
	#ifdef USE_RFID_LOGIC
	        RFIDCycleEvery1Sec();
	#endif
    }
    if (OneMinute_Tick)
    {
        MachineUpdateResponseFunc();
        /*        for (Motor_i = 0;Motor_i < NUM_OF_MOTORS;Motor_i++)
        {
            if (Motor_i == HARDWARE_MOTOR_TYPE__MOTO_SCREW)
                continue;  //
            if (isMotorConfigured(Motor_i))
                MotorGetStatusFromFPGA(Motor_i);
        }*/
        midtankDisplay = 1-midtankDisplay;
        /*if (WHS_Type == WHS_TYPE_UNKNOWN)
            Gas_PPM_Info = Calculate_Gas_Power_Consumption();*/
//            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
        PumpCounter++;
        if (PumpCounter>=PUMP_LIMIT)
        {
            PumpActivation(900);
            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();


    return OK;
}
/******************************************************************************
 *  ======== messageTsk ========
 *  Task for this function is created statically. See the project's .cfg file.
 *  this message task is created statically in system initialization,
 ******************************************************************************/
void MillisecTask(UArg arg0, UArg arg1)
{
    MillisecMessageStruc MillisecMessage;
    //char str[60];
    //uint16_t length;
    //Clock_setTimeout(HostKAClock, 1000);
    //Clock_start(HostKAClock);
    //MillisecInit();
    Millisecond_Task_Handle = Task_self();
    while(1)
    {
        Mailbox_pend(MillisecMsgQ , &MillisecMessage, BIOS_WAIT_FOREVER);
        switch (MillisecMessage.messageId)
        {
            case OneMillisec:
                MillisecLoop(MillisecMessage.tick);
                break;
            default:
                break;
        }
    }
}
/******************************************************************************
 *  ======== messageTsk ========
 *  Task for this function is created statically. See the project's .cfg file.
 *  this message task is created statically in system initialization,
 ******************************************************************************/
void MillisecLowTask(UArg arg0, UArg arg1)
{
    MillisecMessageStruc MillisecLowMessage;
    //char str[60];
    //uint16_t length;
    //Clock_setTimeout(HostKAClock, 1000);
    //Clock_start(HostKAClock);
    //MillisecInit();
    //Millisecond_Task_Handle = Task_self();
    while(1)
    {
        Mailbox_pend(TenMillisecMsgQ , &MillisecLowMessage, BIOS_WAIT_FOREVER);
        switch (MillisecLowMessage.messageId)
        {
            case OneMillisec:
                MillisecLowLoop(MillisecLowMessage.tick);
                break;
            default:
                break;
        }
    }
}


/*uint32_t getMotorStatusData(int MotorId)
{
    assert (MotorId < NUM_OF_MOTORS);
    return MotorStatus_Data[MotorId];
}*/
/*uint32_t getMotorSpeedData(int MotorId)
{
    assert (MotorId < NUM_OF_MOTORS);
    return MotorSpeed_Data[MotorId];
}
*/
/*uint32_t getTemperatureSensorData(int SensorId)
{
    assert (SensorId < MAX_MAIN_CARD_TEMPERATURE_SENSOR_ID);
    return TemperatureSensor_Data[SensorId];
}*/

/*uint32_t getADCData(int DeviceId)
{
    assert (DeviceId < MAX_ADC_DEVICES);
    return ADC_Data[DeviceId];
}*/
float getSensorSpeedData(void)
{
    return Speed_Data;
}

uint32_t getDrawerFansStatus(void)
{
    return DrawerFansStatus;
}
uint8_t getGasReading(void)
{
    return Gas_PPM_Info;
}

uint32_t getSystemFansStatus(void)
{
    return SystemFansStatus;
}
#ifdef HUNDRED_MICROSECONDS_DANCER_READ
uint32_t DancerData[NUM_OF_DANCERS];
uint32_t Control_Read_Dancer_Position(HardwareDancerType DancerId, uint32_t Parameter1, uint32_t Parameter2)
{
    return DancerData[DancerId];
}
uint32_t dancer1;
uint32_t dancer2;
uint32_t dancer3;
uint32_t dancer1sum;
uint32_t dancer2sum;
uint32_t dancer3sum;
uint32_t dancer_count;
/*-----------------------*/
uint32_t StoreBuffer[2][128];
//char * StoreBuffer[2][512];
/*-----------------------*/
int  StoreBufferId = 0;
int  StoreBufferCounter=0;
uint32_t  BufferCounter=0xEEEEEEEE;
bool storeData=false;
uint8_t len=0;
void HundredMicroTimerInterrupt(int ARG0)
{
    ROM_IntMasterDisable();
    ROM_TimerIntClear(Millisec_timerBase, TIMER_TIMA_TIMEOUT);  // Clear the timer interrupt
    if (MillisecRestart == true)
    {
        ROM_TimerLoadSet(Millisec_timerBase, TIMER_A,HIGH_TASK_FREQUENCY);
    }
    else
    {
        ROM_TimerDisable(Millisec_timerBase,TIMER_A);
        ROM_IntMasterEnable();
        return;
    }
    dancer1 = Read_Dancer_Position(WINDER_DANCER);
    dancer2 = Read_Dancer_Position(POOLER_DANCER);
    dancer3 = Read_Dancer_Position(FEEDER_DANCER);
//data store - logging
//double buffer switch
    /*-----------------------*/
    if (StoreBufferCounter>125)
    //if (StoreBufferCounter>490)
        /*-----------------------*/
    {
        StoreBufferId = 1-StoreBufferId;//switch buffer
        StoreBufferCounter=0;
        storeData = true;
    }
//double buffer initialize
    /*-----------------------*/
    if (StoreBufferCounter==0)
    {
        StoreBuffer[StoreBufferId][StoreBufferCounter] = BufferCounter++;
        StoreBufferCounter++;
    }
    /*-----------------------*/
//store data
    /*-----------------------*/
    StoreBuffer[StoreBufferId][StoreBufferCounter++] = dancer1;
    StoreBuffer[StoreBufferId][StoreBufferCounter++] = dancer2;
    StoreBuffer[StoreBufferId][StoreBufferCounter++] = dancer3;
    /*-----------------------*/
    //len = usprintf(&StoreBuffer[StoreBufferId][StoreBufferCounter], "%d %d %d", dancer1[dancer_count],dancer2[dancer_count],dancer3[dancer_count]);
    //StoreBufferCounter+=(len+1);
    /*-----------------------*/
    dancer1sum+=dancer1;
    dancer2sum+=dancer2;
    dancer3sum+=dancer3;
    dancer_count++;


    if (dancer_count == 10)
    {
        DancerData[WINDER_DANCER] = dancer1sum/dancer_count;
        DancerData[POOLER_DANCER] = dancer2sum/dancer_count;
        DancerData[FEEDER_DANCER] = dancer3sum/dancer_count;
        dancer_count = 0;
        dancer1sum = 0;
        dancer2sum = 0;
        dancer3sum = 0;
        OneMilliSecondMillisecInterrupt(ARG0);
    }
    ROM_IntMasterEnable();
    return ;

}
char MillisecPath[50] = "0://SysInfo//Millisec.txt";
FIL *FileHandle;
void SaveLogData(void)
{
    uint32_t WrittenBytes = 0;
    int BufferID = 1- StoreBufferId;

    if (storeData == true)
    {
        if (FileHandle)
        {
            f_write(FileHandle,StoreBuffer[BufferID],512,&WrittenBytes );
            storeData = false;
        }
    }
}
void MillisecLogInit(void)
{
    FRESULT Fresult = FR_OK;
    ROM_IntMasterDisable();
    BufferCounter = 0;
    FileHandle = my_malloc(sizeof(FIL));
    if (FileHandle == 0)
        Fresult = FR_DENIED;
    else
        Fresult = f_open(FileHandle,MillisecPath,FA_WRITE |  FA_OPEN_ALWAYS|FA_CREATE_ALWAYS);
    ROM_IntMasterEnable();
    return ;

}
void MillisecLogClose(void)
{
    FRESULT Fresult = FR_OK;
    if (FileHandle == 0)
        Fresult = FR_DENIED;
    else
    {
        storeData = false;
        ROM_IntMasterDisable();
        Fresult = f_close(FileHandle);
        my_free(FileHandle);
        FileHandle = 0;
        ROM_IntMasterEnable();
    }
    return ;

}
#endif
void MillisecInterrupt(UArg arg0)
{
#ifdef HUNDRED_MICROSECONDS_DANCER_READ
    HundredMicroTimerInterrupt(arg0);
#else
    OneMilliSecondMillisecInterrupt(arg0);
#endif
}