Diff: CODE/System.cpp

Revision:

0:d0c18e423b13

Child:

1:871a329fd172

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/CODE/System.cpp	Tue Sep 18 18:35:23 2012 +0000
@@ -0,0 +1,768 @@
+#include "System.h"
+
+DigitalOut EXT_LED0(LED2);
+DigitalOut ETH_LED_GREEN(p29);
+DigitalOut ETH_LED_YELLOW(p30);
+DigitalIn  ActionButton(p28);
+DigitalIn  SDCardDetect(p11);
+Ticker  SystemTick;
+
+WORD UpdateHostTerminalTick;
+WORD DisplayUpdateTick;
+WORD ButtonCheckTick;
+WORD DisplayShutOffTick;
+
+BYTE SystemState;
+CHAR DTBuf[32];
+CHAR DataLineBuf[256];
+
+//*****************************************************************
+//smart Display related stuff
+//*****************************************************************
+
+#define DISPLAY_MEASUREMENTS       0x00
+#define DISPLAY_BATTERY            0x01
+#define DISPLAY_DATE_TIME          0x02
+#define DISPLAY_CARD_STATUS        0x03
+#define DISPLAY_BUFFER_STATS       0x04
+#define DISPLAY_FIRMWARE_VERSION   0x05
+#define DISPLAY_ID                 0x06
+#define DISPLAY_OFF                0xFF
+
+#define NUM_DISPLAYS               7
+#define DISPLAY_SHUTOFF_TIME    10000
+
+BYTE DisplayMode;
+BYTE LastBackLightColor = 0;
+
+//*****************************************************************
+//Action Button Related stuff
+//*****************************************************************
+#define WAITING_FOR_PRESS   0x00
+#define WAITING_FOR_RELEASE 0x01
+
+BYTE PreviousButtonState;   
+BYTE CurrentButtonState;   
+WORD ButtonHoldTime;
+BYTE ButtonCheckState;
+void ActionButtonHandler(WORD ButtonPressTime);
+
+//time Variables
+time_t CurrentTime_InSec;
+tm *CurrentTime;
+
+//*****************************************************************
+//Logging Related Stuff
+//*****************************************************************
+CHAR CurrentLogFileName[256];
+BOOL InitDataLog();
+BOOL DataLogError;
+CHAR *ErrorMsg;
+
+void CreateLogFileName();
+
+FATFS MyFileSystem;
+FIL  CurrentLogFileHandle;
+
+//DataBlock MyDataBlock[2];
+DataBlock MyDataBlock;
+
+BYTE ActiveDataBlock;
+DWORD   DataBlocksWritten;
+DWORD   DataPointIndex;
+SIGNED_DWORD ReadWriteDifferential;
+
+DWORD   WritesSinceLastFlush;
+
+#define BINARY_WRITE_CACHE_THRESHOLD   512
+BYTE    BinaryDataCache[BINARY_WRITE_CACHE_THRESHOLD + 64];
+DWORD   BinaryDataCachePtr;
+
+#define NUM_WRITES_BEFORE_FLUSH (10)
+
+//*****************************************************************
+//Misc System Related Stuff
+//*****************************************************************
+void SystemTickIrq();
+void AdjustIOffset();
+
+void InitRobotPowerMeasurementSystem()
+{
+    InitDataBlocks(&MyDataBlock);
+     
+    ActiveDataBlock = 0;
+    DataBlocksWritten = 0;
+    DataPointIndex = 0;
+    
+    SystemTick.attach_us(&SystemTickIrq,10000);
+    CurrentTime_InSec = time(NULL);
+  
+    DataLogError = FALSE;
+}
+
+
+void InitDataBlocks(DataBlock * DB)
+{
+    int i;
+  
+    for(i=0;i<DATA_BLOCK_SIZE;i++)
+    {
+        DB->Voltage[i] = 0;
+        DB->Current[i] = 0;
+    }
+    
+     DB->WriteOutPtr = 0;
+     DB->ReadInPtr = 0;
+}
+
+
+void EnterSystemState(BYTE NextState)
+{
+    switch(NextState)
+    {
+        default:
+        case SYSTEM_STATE_INIT:
+            DisplayMode = DISPLAY_MEASUREMENTS;
+            SystemState = NextState;
+        break;
+        
+        case SYSTEM_STATE_LOGGING:
+              DisplayMode = DISPLAY_CARD_STATUS;
+              
+              if(InitDataLog() == FALSE)
+              {
+                SystemState = NextState;
+              
+              }
+              else
+              {
+                SystemState = SYSTEM_STATE_IDLE;
+              }
+              
+        break;
+        
+        case SYSTEM_STATE_IDLE:
+            DisplayMode = DISPLAY_CARD_STATUS;
+            if(SystemState == SYSTEM_STATE_LOGGING)
+            {
+                f_close(&CurrentLogFileHandle);
+                f_mount(0,NULL);
+                PrintfEnqueue(&PCBackDoorTx,"Logging terminated on file %s\r\n>",CurrentLogFileName);
+            }
+            
+            SystemState = NextState;
+        break;
+    }
+}
+
+BOOL InitDataLog()
+{
+    UINT BytesWritten;
+    
+    PrintfEnqueue(&PCBackDoorTx,"\r\n\r\bInitializing Data log....\r\n");
+    CreateLogFileName();
+    PrintfEnqueue(&PCBackDoorTx,"Filename: %s\r\n",CurrentLogFileName);
+    PrintfEnqueue(&PCBackDoorTx,"Attempting File Open....\r\n");
+ 
+    
+    f_mount(0,&MyFileSystem);
+    if(f_open(&CurrentLogFileHandle,&CurrentLogFileName[0],FA_WRITE | FA_OPEN_ALWAYS) != FR_OK)
+      {
+                DataLogError = TRUE;
+                PrintfEnqueue(&PCBackDoorTx,"Could not open file!\r\n>");
+                ErrorMsg = "Write Error!";
+                return TRUE;
+      }
+   
+     
+     DataLogError = FALSE;
+     PrintfEnqueue(&PCBackDoorTx,"Writing Headers....\r\n");
+     time(&CurrentTime_InSec);
+     strftime(DTBuf,128, "%Y.%m.%d", localtime(&CurrentTime_InSec));
+     f_printf(&CurrentLogFileHandle, "Date      %s\r\n",DTBuf);
+     strftime(DTBuf,128, "%H:%M:%S", localtime(&CurrentTime_InSec));
+     f_printf(&CurrentLogFileHandle, "Time      %s\r\n\n",DTBuf);
+     sprintf(DTBuf, "Sample Rate   %.1f Hz\r\n\r\n",SAMPLE_RATE);
+     f_write(&CurrentLogFileHandle,DTBuf,strlen(DTBuf),&BytesWritten);
+     PrintfEnqueue(&PCBackDoorTx,"Headers Written.... Starting log\r\n");
+    
+    
+    PrintfEnqueue(&PCBackDoorTx,"\r\n>"); 
+
+
+          
+    BinaryDataCachePtr = 0;
+    WritesSinceLastFlush = 0;
+     
+    DataBlocksWritten = 0;
+    DataPointIndex = 0;
+
+    return FALSE;
+}
+
+
+void CreateLogFileName()
+{
+     time(&CurrentTime_InSec);
+     strftime(CurrentLogFileName,256, "F%Y.%m.%d.%H.%M.%S.csv", localtime(&CurrentTime_InSec));
+}     
+
+
+
+void SystemTickIrq()
+{
+    if(UpdateHostTerminalTick<0xFFFF)
+        UpdateHostTerminalTick++;
+        
+    if(DisplayUpdateTick<0xFFFF)
+        DisplayUpdateTick++;
+        
+    if(ButtonCheckTick<0xFFFF)
+         ButtonCheckTick++;
+         
+     if(DisplayShutOffTick<0xFFFF)
+        DisplayShutOffTick++;
+}
+
+void InitButton()
+{
+    ButtonCheckState = WAITING_FOR_PRESS;
+    ButtonCheckTick = 0;
+    PreviousButtonState = FALSE;
+    CurrentButtonState = FALSE;
+
+}
+void CheckButton()
+{
+    if(ButtonCheckTick>0)
+    {
+        ButtonCheckTick = 0;
+        
+        PreviousButtonState = CurrentButtonState;
+        CurrentButtonState = ActionButton.read();    
+    
+        switch(ButtonCheckState)
+        {
+            default:
+            case WAITING_FOR_PRESS:
+            
+            if(CurrentButtonState == TRUE && PreviousButtonState == FALSE)
+            {
+                ButtonCheckState = WAITING_FOR_RELEASE; 
+            }
+            
+            ButtonHoldTime = 0;
+            
+            break;
+                        
+            case WAITING_FOR_RELEASE:
+            
+            if(CurrentButtonState == TRUE && PreviousButtonState == TRUE)
+            {
+                ButtonHoldTime++;
+            }
+            else if(CurrentButtonState == FALSE && PreviousButtonState == TRUE)
+            {
+                ActionButtonHandler(ButtonHoldTime);
+                ButtonCheckState = WAITING_FOR_PRESS;    
+            }
+            else
+            {
+               ButtonCheckState = WAITING_FOR_PRESS;    
+            }
+            break;
+        }
+    }
+}
+
+
+void ActionButtonHandler(WORD ButtonPressTime)
+{
+    DisplayShutOffTick = 0;
+
+    if(ButtonPressTime<50)
+    {
+       if(DisplayMode == DISPLAY_OFF)
+       {
+         PowerUpSmartSwitch();
+         SmartSwitch_Reset();
+         SmartSwitchClear();
+         DisplayMode = DISPLAY_MEASUREMENTS;
+       }
+       else
+       {
+       
+            if(DataLogError == TRUE)
+            {
+                DataLogError = FALSE;
+            }
+            else
+            {
+                DisplayMode++;
+               
+                if(DisplayMode >= NUM_DISPLAYS)
+                {
+                    DisplayMode = 0;
+                }
+            }
+        }
+    }
+    
+    else if(ButtonPressTime>100 && ButtonPressTime<500)
+    {
+        switch(SystemState)
+        {
+            default:
+            case SYSTEM_STATE_IDLE:
+               switch(DisplayMode)
+               {
+                   default:
+                 
+                    break;
+               
+                    case DISPLAY_MEASUREMENTS:
+                        AdjustIOffset();
+                    break;
+                    
+                    
+                    case DISPLAY_FIRMWARE_VERSION:
+                        LoadConfiguration();
+                    break;
+                    
+                    
+                    case DISPLAY_CARD_STATUS:
+                       //  if(SDCardDetect == 1)
+                            {
+                                EnterSystemState(SYSTEM_STATE_LOGGING);
+                            }
+                    break;
+               }
+            break;
+            
+         
+            case SYSTEM_STATE_LOGGING:
+              EnterSystemState(SYSTEM_STATE_IDLE);
+             // unmount the file system
+              f_mount(0,NULL);
+            break;
+        }
+    }
+}
+
+void AdjustIOffset()
+{
+    DWORD SamplesToTake;
+    DWORD i;
+    float RunningSum = 0;
+    
+    PrintfEnqueue(&PCBackDoorTx,"Zeroing ACS576.....\r\n");
+    
+    ADCDataRdy=0;
+    
+    SamplesToTake = (DWORD)(SAMPLE_RATE)*2;
+    
+    for(i=0;i<SamplesToTake;i++)
+    {
+        SmartSwitch_SetBackLightColor2((BYTE)((float)i/(float)SamplesToTake * 64));
+        
+        while(ADCDataRdy == FALSE)
+        {
+        
+        }
+        ADCDataRdy = FALSE;
+        RunningSum += RobotBusCurrentHR;
+    }
+    
+    ACS576_IOFFSET_TRIM += RunningSum / SamplesToTake;
+
+    SmartSwitch_SetBackLightColor(3,3,3);
+    
+    PrintfEnqueue(&PCBackDoorTx,"Exporting new configuration file.....\r\n>");
+    
+    ExportConfigurationSettings();
+}
+
+
+
+
+BOOL CheckforConfig(CHAR *LineIn)
+{
+    if(!strcmp(LineIn,"config"))
+    {
+        LoadConfiguration();
+    }
+    return TRUE;
+}
+
+
+BOOL CheckForTimeInit(CHAR * LineIn)
+{
+    if(!strcmp(LineIn,"tinit"))
+    {
+        set_time(1256729737);
+       PrintfEnqueue(&PCBackDoorTx,"\r\nTime Reset\r\n");
+    }
+    
+    return TRUE;
+
+}
+
+BOOL CheckForTime(CHAR *LineIn)
+{
+    int Hour,Minute,Second=0;
+    int Items;
+    BOOL Error = FALSE;
+     time_t TimeStampTemp;
+    
+    
+    Items = sscanf(LineIn, "time=%d : %d  : %d", &Hour, &Minute, &Second);
+    if(Items == 3)
+    {
+        PrintfEnqueue(&PCBackDoorTx,"\r\n");
+        if(Hour>23)
+        {
+            PrintfEnqueue(&PCBackDoorTx,"Hour entry must be between 0 and 24\r\n");
+            Error = TRUE;
+        }
+        if(Minute>60)
+        {
+            PrintfEnqueue(&PCBackDoorTx,"Minute entry must be between 0 and 60\r\n");
+            Error = TRUE;
+        }
+        if(Second>60)
+        {
+            PrintfEnqueue(&PCBackDoorTx,"Second entry must be between 0 and 60\r\n");
+            Error = TRUE;
+        }
+        
+        if(Error == TRUE)
+        {
+         PrintfEnqueue(&PCBackDoorTx,"Error in time format.  Time not changed.\r\n");
+        }
+        else
+        {
+            TimeStampTemp = time(NULL);
+            //convert to tm struct 
+            CurrentTime = localtime(&TimeStampTemp);
+            //dump in our new valus
+            CurrentTime->tm_sec = Second;
+            CurrentTime->tm_min = Minute;
+            CurrentTime->tm_hour = Hour;
+            //set the new time
+            set_time(mktime(CurrentTime));
+            PrintfEnqueue(&PCBackDoorTx,"Time set to %d:%d:%d\r\n",Hour,Minute,Second);
+        }
+     
+      }
+   return FALSE;
+}
+
+BOOL CheckForDate(CHAR *LineIn)
+{
+    int Day,Month,Year=0;
+    int Items;
+    BOOL Error = FALSE;
+    time_t TimeStampTemp;
+    
+    Items = sscanf(LineIn, "date=%d / %d  / %d", &Month, &Day, &Year);
+    if(Items == 3)
+    {
+        PrintfEnqueue(&PCBackDoorTx,"\r\n");
+        if(Month>12 || Month < 1)
+        {
+            PrintfEnqueue(&PCBackDoorTx,"Month entry must be between 1 and 12\r\n");
+            Error = TRUE;
+        }
+        if(Day>31 || Day<1)
+        {
+            PrintfEnqueue(&PCBackDoorTx,"Day entry must be between 1 and 31\r\n");
+            Error = TRUE;
+        }
+        if(Year<1900)
+        {
+            PrintfEnqueue(&PCBackDoorTx,"Year entry must be greater than 1900\r\n");
+            Error = TRUE;
+        }
+        
+        if(Error == TRUE)
+        {
+         PrintfEnqueue(&PCBackDoorTx,"Error in time format.  Date not changed.\r\n");
+        }
+        else
+        {
+            //Get the current time in seconds since unix epoch
+            TimeStampTemp = time(NULL);
+            //convert to tm struct fom
+            CurrentTime = localtime(&TimeStampTemp);
+            //dump in our new valus
+            CurrentTime->tm_mon = Month-1;
+            CurrentTime->tm_mday = Day;
+            CurrentTime->tm_year = Year - 1900;
+            //set the new time
+            set_time(mktime(CurrentTime));
+            PrintfEnqueue(&PCBackDoorTx,"Date set to %d/%d/%d\r\n",Month,Day,Year);
+        }
+     
+      }
+   return FALSE;
+}
+
+
+
+void UpdateDisplay()
+{
+
+    if(DisplayShutOffTick > DISPLAY_SHUTOFF_TIME)
+    {
+        DisplayMode = DISPLAY_OFF;
+        SmartSwitchClear();
+       PowerDownSmartSwitch();
+    }
+    else
+    {
+        if(DisplayUpdateTick > 25)
+        {
+            GFX_FullDisplayBufferClear(&BackBuffer);
+            DisplayUpdateTick = 0;
+                switch(DisplayMode)
+                {
+                    default:
+                    case DISPLAY_MEASUREMENTS:
+                      SmartSwitch_SetBackLightColor(3,3,3);
+                       GFX_DrawString(&BackBuffer,"Robot Bus",0,0,&Font5x7);
+                       GFX_DrawHline(&BackBuffer,0,PHYSICAL_DISPLAY_XRES-1,10);
+                       GFX_DrawHline(&BackBuffer,0,PHYSICAL_DISPLAY_XRES-1,11);
+                       GFX_DrawHline(&BackBuffer,0,PHYSICAL_DISPLAY_XRES-1,12);
+                        
+                       GFX_printf(&BackBuffer,0,16,&Font5x7,"V: %.2fv",RobotBusVoltageHR);
+                       GFX_printf(&BackBuffer,0,24,&Font5x7,"I: %.2fa",RobotBusCurrentHR);
+                    break;
+                    
+                   
+                    case DISPLAY_BATTERY:
+                       SmartSwitch_SetBackLightColor(3,3,0);
+                       GFX_DrawString(&BackBuffer,"Battery",0,0,&Font5x7);
+                       GFX_DrawHline(&BackBuffer,0,PHYSICAL_DISPLAY_XRES-1,10);
+                       GFX_DrawHline(&BackBuffer,0,PHYSICAL_DISPLAY_XRES-1,11);
+                       GFX_DrawHline(&BackBuffer,0,PHYSICAL_DISPLAY_XRES-1,12);
+                        
+                       GFX_printf(&BackBuffer,0,16,&Font5x7,"V: %.1fv",BatteryVoltage);
+                      
+                    break;
+                    
+                     case DISPLAY_DATE_TIME:
+                       SmartSwitch_SetBackLightColor(1,3,0);
+                       GFX_DrawString(&BackBuffer,"Date/Time",0,0,&Font5x7);
+                       GFX_DrawHline(&BackBuffer,0,PHYSICAL_DISPLAY_XRES-1,10);
+                       GFX_DrawHline(&BackBuffer,0,PHYSICAL_DISPLAY_XRES-1,11);
+                       GFX_DrawHline(&BackBuffer,0,PHYSICAL_DISPLAY_XRES-1,12);
+                        
+                       time(&CurrentTime_InSec);
+                       strftime(DTBuf, 32, "%m.%d.%Y", localtime(&CurrentTime_InSec));
+                       GFX_printf(&BackBuffer,0,16,&Font5x7,"%s",DTBuf);
+                      
+                       strftime(DTBuf, 32, "%H:%M:%S", localtime(&CurrentTime_InSec));
+                       GFX_printf(&BackBuffer,0,24,&Font5x7,"%s",DTBuf);
+                    break;
+                    
+                     case DISPLAY_BUFFER_STATS:
+                     SmartSwitch_SetBackLightColor(3,3,3);
+                     GFX_DrawString(&BackBuffer,"Buf Status",0,0,&Font5x7);
+                     GFX_DrawHline(&BackBuffer,0,PHYSICAL_DISPLAY_XRES-1,10);
+                     GFX_DrawHline(&BackBuffer,0,PHYSICAL_DISPLAY_XRES-1,11);
+                     GFX_DrawHline(&BackBuffer,0,PHYSICAL_DISPLAY_XRES-1,12);
+                     GFX_printf(&BackBuffer,0,16,&Font3x5,"R/W Diff:");
+                     GFX_printf(&BackBuffer,0,24,&Font3x5,"%04d/%d",ReadWriteDifferential,DATA_BLOCK_SIZE);
+                    break;
+                    
+                    case DISPLAY_FIRMWARE_VERSION:
+                     SmartSwitch_SetBackLightColor(3,3,3);
+                     GFX_DrawString(&BackBuffer,"Firmware Version",0,0,&Font3x5);
+                     GFX_DrawHline(&BackBuffer,0,PHYSICAL_DISPLAY_XRES-1,10);
+                     GFX_DrawHline(&BackBuffer,0,PHYSICAL_DISPLAY_XRES-1,11);
+                     GFX_DrawHline(&BackBuffer,0,PHYSICAL_DISPLAY_XRES-1,12);
+                     GFX_printf(&BackBuffer,19,20,&Font5x7,"%d.%d",FIRMWARE_VERSION_MAJOR,FIRMWARE_VERSION_MINOR);
+                    break;
+                    
+                    case DISPLAY_ID:
+                     SmartSwitch_SetBackLightColor(3,3,3);
+                     GFX_DrawString(&BackBuffer,"ID",0,0,&Font3x5);
+                     GFX_DrawHline(&BackBuffer,0,PHYSICAL_DISPLAY_XRES-1,10);
+                     GFX_DrawHline(&BackBuffer,0,PHYSICAL_DISPLAY_XRES-1,11);
+                     GFX_DrawHline(&BackBuffer,0,PHYSICAL_DISPLAY_XRES-1,12);
+                     GFX_printf(&BackBuffer,0,20,&Font5x7,"%s",ID);
+                    break;
+                    
+                    case DISPLAY_CARD_STATUS:
+                                         
+                      if(1)
+                      {
+                        
+                          switch(SystemState)
+                          {
+                            default:
+                            case SYSTEM_STATE_IDLE:
+                                switch(LastBackLightColor)
+                                 {
+                                    default:
+                                    case SMART_SWITCH_BACKLIGHT_GREEN:
+                                         LastBackLightColor = SMART_SWITCH_BACKLIGHT_YELLOW;  
+                                         SmartSwitch_SetBackLightColor2(LastBackLightColor);
+                                    break;
+                                    
+                                    case SMART_SWITCH_BACKLIGHT_YELLOW:
+                                        LastBackLightColor = SMART_SWITCH_BACKLIGHT_GREEN;
+                                        SmartSwitch_SetBackLightColor2(LastBackLightColor);
+                                    break;
+                                  }
+                               
+                               
+                              if(DataLogError == TRUE)
+                              {
+                                   GFX_DrawString(&BackBuffer,"Error!",0,0,&Font5x7); 
+                                   GFX_DrawString(&BackBuffer,ErrorMsg,0,16,&Font3x5);
+                              }  
+                              else
+                              { 
+                                  GFX_DrawString(&BackBuffer,"SD Detected",0,16,&Font5x7);
+                                  GFX_DrawString(&BackBuffer,"Idle....",0,0,&Font5x7);
+                                  GFX_DrawString(&BackBuffer,"Not Logging",0,24,&Font5x7);    
+                              }
+                            break;
+                         
+                         
+                            case SYSTEM_STATE_LOGGING:
+                                SmartSwitch_SetBackLightColor2(SMART_SWITCH_BACKLIGHT_GREEN);
+                               
+                                GFX_DrawString(&BackBuffer,"Logging Data....",0,0,&Font5x7);
+                                GFX_DrawString(&BackBuffer,&CurrentLogFileName[4],0,16,&Font5x7);
+                                GFX_printf(&BackBuffer,0,26,&Font3x5,"Block: %d",DataBlocksWritten);
+                                
+                            break;
+                         }
+                      }
+                      else
+                      {
+                          GFX_DrawString(&BackBuffer,"No SD Card!",0,0,&Font5x7);
+                          
+                            switch(LastBackLightColor)
+                            {
+                                default:
+                                case SMART_SWITCH_BACKLIGHT_RED:
+                                     LastBackLightColor = SMART_SWITCH_BACKLIGHT_YELLOW;  
+                                     SmartSwitch_SetBackLightColor2(LastBackLightColor);
+                                break;
+                                
+                                case SMART_SWITCH_BACKLIGHT_YELLOW:
+                                    LastBackLightColor = SMART_SWITCH_BACKLIGHT_RED;
+                                    SmartSwitch_SetBackLightColor2(LastBackLightColor);
+                                break;
+                              }
+                      } 
+                    
+                       GFX_DrawHline(&BackBuffer,0,PHYSICAL_DISPLAY_XRES-1,10);
+                       GFX_DrawHline(&BackBuffer,0,PHYSICAL_DISPLAY_XRES-1,11);
+                       GFX_DrawHline(&BackBuffer,0,PHYSICAL_DISPLAY_XRES-1,12);
+                    
+                    break;
+                }
+             GFX_DumpRenderContextToPhysicalScreen(&BackBuffer);
+        }
+    }
+}
+
+
+void CheckSDCardStatus()
+{
+    //Make sure that the SD card stays in while logging
+    switch(SystemState)
+    {
+        case SYSTEM_STATE_LOGGING:
+            //if(SDCardDetect == 0)
+            {
+                //Gracefully shut the logging system down.....
+              //  EnterSystemState(SYSTEM_STATE_IDLE);
+            }
+        break;
+        
+        default:
+        break;
+    } 
+}
+
+
+void LogData()
+{
+    WORD i;
+    int ElementsToWrite;
+    UINT BytesWritten;
+    
+    if(RobotBusVoltageHR < 9)
+    {
+       // EnterSystemState(SYSTEM_STATE_IDLE);
+        return;
+    }
+    
+    if(DataLogError == TRUE)
+    {
+        //EnterSystemState(SYSTEM_STATE_IDLE);
+    }
+    else
+    {
+        if(MyDataBlock.ReadInPtr < MyDataBlock.WriteOutPtr)
+        {
+           ElementsToWrite =  MyDataBlock.ReadInPtr + (DATA_BLOCK_SIZE - MyDataBlock.WriteOutPtr);
+        }
+        else
+        {
+            ElementsToWrite = MyDataBlock.ReadInPtr - MyDataBlock.WriteOutPtr;
+        }
+        
+        if(ElementsToWrite > WRITE_BLOCK_THRESH)
+        {
+                for(i=0;i<ElementsToWrite;i++)
+                {
+                    BinaryDataCachePtr += sprintf((char *)&BinaryDataCache[BinaryDataCachePtr],"%d,%.2f,%.2f\r\n",DataPointIndex,MyDataBlock.Voltage[MyDataBlock.WriteOutPtr],MyDataBlock.Current[MyDataBlock.WriteOutPtr]);
+                    if(BinaryDataCachePtr>=BINARY_WRITE_CACHE_THRESHOLD)
+                    {
+                        EXT_LED0 = 1;  
+                        f_write(&CurrentLogFileHandle,BinaryDataCache,BinaryDataCachePtr,&BytesWritten);
+                        
+                        BinaryDataCachePtr = 0;
+                      
+                        EXT_LED0=0;
+                    }
+                    DataPointIndex++;
+                    MyDataBlock.WriteOutPtr++;
+                    if(MyDataBlock.WriteOutPtr == DATA_BLOCK_SIZE)
+                    {
+                        MyDataBlock.WriteOutPtr = 0;
+                    }
+                }
+                if(WritesSinceLastFlush > NUM_WRITES_BEFORE_FLUSH)
+                 {
+                            f_close(&CurrentLogFileHandle);
+                            WritesSinceLastFlush = 0;
+                            f_open(&CurrentLogFileHandle,&CurrentLogFileName[0],FA_WRITE | FA_OPEN_ALWAYS);
+                            f_lseek(&CurrentLogFileHandle, CurrentLogFileHandle.fsize);
+                 }
+               DataBlocksWritten++;
+      }
+    }
+}
+
+
+
+void DisplayBootMsg()
+{
+   PrintfEnqueue(&PCBackDoorTx,"\r\n\r\n");
+   PrintfEnqueue(&PCBackDoorTx,".______     _______. __    __            ___      .______       __   \r\n");
+   PrintfEnqueue(&PCBackDoorTx,"|   _  \\   /       ||  |  |  |          /   \\     |   _  \\     |  |  \r\n");
+   PrintfEnqueue(&PCBackDoorTx,"|  |_)  | |   (----`|  |  |  |  ______ /  ^  \\    |  |_)  |    |  |  \r\n");   
+   PrintfEnqueue(&PCBackDoorTx,"|   ___/   \\   \\    |  |  |  | |______/  /_\\  \\   |      /     |  |  \r\n");
+   PrintfEnqueue(&PCBackDoorTx,"|  |   .----)   |   |  `--'  |       /  _____  \\  |  |\\  \\----.|  `----.\r\n");
+   PrintfEnqueue(&PCBackDoorTx,"| _|   |_______/     \\______/       /__/     \\__\\ | _| `._____||_______|\r\n");
+   PrintfEnqueue(&PCBackDoorTx,"---------------------------------------------------------------------------\r\n");
+   PrintfEnqueue(&PCBackDoorTx,"Robot Power Logger Control Terminal\r\n\r\n>");
+}
+