File content as of revision 3:d55665050fcb:

#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;
BYTE WeAreInNormalMode;
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_SERIAL_STATUS      0x07

#define DISPLAY_OFF                0xFF

#define NUM_DISPLAYS               8
#define DISPLAY_SHUTOFF_TIME    10000

BYTE DisplayMode;
BYTE LastBackLightColor = 0;
BYTE LastSerialBrightness = 7;
BOOL SerialFileIsOpen = FALSE;
char SerialLineBuffer[256];
UINT SerialLineBufferPtr;
UINT LinesWritten =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;
WORD SerialCount=0;
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_SERIAL_LOGGING:
            if(InitSerialDataLog() == FALSE){
                PrintfLogEnqueue(&PCBackDoorTx,"%cS",SERIALSPEED);
                SerialCount=0;
                SystemState = NextState;
            }
            else{
                SystemState=SYSTEM_STATE_IDLE;
            }
            break;

        case SYSTEM_STATE_IDLE:
            DisplayMode = DISPLAY_CARD_STATUS;
            switch (SystemState){
                 case SYSTEM_STATE_SERIAL_LOGGING:
                    DisplayMode=DISPLAY_SERIAL_STATUS; //Need to stay here for serial logging
                    //NO BREAK ON PURPOSE

                 case SYSTEM_STATE_LOGGING:
                    f_close(&CurrentLogFileHandle);
                    f_mount(0,NULL);
                    PrintfEnqueue(&PCBackDoorTx,"Logging terminated on file %s\r\n>",CurrentLogFileName);
                    break;
            }
            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;
}

BOOL InitSerialDataLog()
{
    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!";
        f_mount(0,0);
        return TRUE;
    }


    DataLogError = FALSE;
    PrintfEnqueue(&PCBackDoorTx,"Writing Headers....\r\n");
    time(&CurrentTime_InSec);
    f_printf(&CurrentLogFileHandle,"Rate,V1,C1,V2,C2,V3,C3,V4,C4,V5,C5,V6,C6,B1V,B1C,B2V,B2C,");
    strftime(DTBuf,128, "%Y.%m.%d", localtime(&CurrentTime_InSec));
   
    f_printf(&CurrentLogFileHandle, "Date      ,%s,",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);
    f_sync(&CurrentLogFileHandle);
    PrintfEnqueue(&PCBackDoorTx,"Headers Written.... Starting log\r\n");


    PrintfEnqueue(&PCBackDoorTx,"\r\n>");
    SerialLineBufferPtr=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;
        }
    }
}
char SpeedString[20];
void TranslateToText(){
    switch(SERIALSPEED){
        case '0':
            sprintf(&SpeedString[0],"20 mS");
            break;
        case '1':
            sprintf(&SpeedString[0],"30 mS");
            break;
        case '2':
            sprintf(&SpeedString[0],"40 mS");
            break;
        case '3':
            sprintf(&SpeedString[0],"50 mS");
            break;
        case '4':
            sprintf(&SpeedString[0],"100 mS");
            break;
        case '5':
            sprintf(&SpeedString[0],"1 S");
            break;
        case '6':
            sprintf(&SpeedString[0],"2 S");
            break;
        case '7':
            sprintf(&SpeedString[0],"5 S");
            break;
        case '8':
            sprintf(&SpeedString[0],"10 S");
            break;
        case '9':
            sprintf(&SpeedString[0],"30 S");
            break;
        default:
            sprintf(&SpeedString[0],"ERROR");
            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) { //Long action button
        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;
                    
                    case DISPLAY_SERIAL_STATUS:
                        EnterSystemState(SYSTEM_STATE_SERIAL_LOGGING);
                        break;
                }
                break;

            case SYSTEM_STATE_SERIAL_LOGGING:
                PrintfLogEnqueue(&PCBackDoorTx,"QQQ\r\n");  //Shutdown
                EnterSystemState(SYSTEM_STATE_IDLE);
                DisplayMode=DISPLAY_SERIAL_STATUS; //Need to stay here
                f_mount(0,NULL);
                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;
            SmartSwitch_SetBrightnss(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_SERIAL_STATUS:
 
                    //SmartSwitch_SetBrightnss(LastSerialBrightness);
                    SmartSwitch_SetBackLightColor(3,0,3);
                    GFX_DrawString(&BackBuffer,"Serial Log",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);
                     TranslateToText();
                    if(SystemState != SYSTEM_STATE_SERIAL_LOGGING){
                        GFX_printf(&BackBuffer,0,16,&Font5x7,"Rate %s",&SpeedString[0]);
                        GFX_printf(&BackBuffer,0,24,&Font5x7,"Not Logging");
                    }
                    else{   //we are logging
                        GFX_printf(&BackBuffer,0,16,&Font5x7,"C-%d",SerialCount);
                        GFX_printf(&BackBuffer,0,24,&Font5x7,"Logging"); 
                        SmartSwitch_SetBackLightColor(0,0,3); 
                    }                        
                    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>");
}