File content as of revision 38:8d86e0d8a816:

/*
ELEC 351 Group T
Team Members : Christopher Hills, Thomas Morris
Current Verision 18
Overiew: Working Tasks 1,2,3,5,6,7,8,9,10,11,12,13

Last Revision: Added Mail Box to serial
Todo:
make a mailbox within SD card, LCD, network so they can be used from within
Fix the network so it uses the mail box data.
*/

#include "SETUP.hpp"
#include "NETWORK.hpp"
#include "FIFO.hpp"

#define Do_Read_Data 1
#define Dont_Read_Data 0
#define Do_Delete_Data 1
#define Dont_Delete_Data 0

Mutex Time_Lock_Main;
Mutex Data_Buffer_Lock;

int Write_Data(DATA Data_Store, int Write_Pointer)
{
    Data_Buffer_Lock.lock();//Mutex Lock
    
    Data_Buffer[Write_Pointer] = Data_Store;
    if(Write_Pointer < mailsize - 1)
    {
        Write_Pointer = Write_Pointer + 1;
    }
    else
    {
        Write_Pointer = 0;   
    }
    Data_Buffer_Lock.unlock();
    return Write_Pointer;
    
}
DATA Read_Data(int Read_Pointer)
{
    Thread::signal_wait(Do_Read_Data);
    Data_Buffer_Lock.lock();
    DATA Temp_Data = Data_Buffer[Read_Pointer];
    Data_Buffer_Lock.unlock();
    
    return Temp_Data;
}
void Delete_Data(int Delete_Pointer)
{
    Thread::signal_wait(Do_Delete_Data);
    Data_Buffer_Lock.lock();
    Data_Buffer[Delete_Pointer].set_all_zero();
    Data_Buffer_Lock.unlock();
}
void Network()
{
    while(1)
    {
        Thread::wait(5000);//Waits 5 seconds
        if(Log_Value==1){cout << "In Network Thread" << endl;} //Log this  
        
        time_t Time = Data_Active.get_time();
        tm* Time_Pointer = localtime(&Time);
        int temp_day = Time_Pointer->tm_mday;
        int temp_month = (Time_Pointer->tm_mon+1);
        int temp_year = (Time_Pointer->tm_year+1900);
        
        int temp_hours = Time_Pointer->tm_hour;
        int temp_minute = Time_Pointer->tm_min;
        
        float temp_temperature = Data_Active.get_temperature();
        float temp_pressure = Data_Active.get_pressure();
        float temp_light = Data_Active.get_light();

        
       Networking(temp_day,temp_month,temp_year,temp_hours,temp_minute,temp_temperature,temp_pressure,temp_light);
    }
}
void LCD_Write_Year()
{
        Time_Lock_Main.lock();
        if(Log_Value == 1){cout<<"In LCD_Write_Year Time lock taken"<<endl;}
        time_t Time = time(NULL);
        tm* Time_Pointer = localtime(&Time);
        int Years = 1900 + Time_Pointer->tm_year;
        Time_Lock_Main.unlock();
        if(Log_Value == 1){cout<<"In LCD_Write_Year Time lock released"<<endl;}
        stringstream ss;
        ss << Years;
        string Year_String = ss.str();
        LCD.DDRAM_Address(0x00);
        LCD.Write_String("Set Year");
        LCD.DDRAM_Address(0x40);
        LCD.Write_String(Year_String);   
}

void LCD_Write_Month()
{
        Time_Lock_Main.lock();
        if(Log_Value == 1){cout<<"In LCD_Write_Month Time lock taken"<<endl;}
        time_t Time = time(NULL);
        tm* Time_Pointer = localtime(&Time);
        int Months = 1 + Time_Pointer->tm_mon;
        Time_Lock_Main.unlock();
        if(Log_Value == 1){cout<<"In LCD_Write_Month Time lock released"<<endl;}
        stringstream ss;
        ss << Months;
        string Month_String = ss.str();
        LCD.DDRAM_Address(0x00);
        LCD.Write_String("Set Month");
        LCD.DDRAM_Address(0x40);
        LCD.Write_String(Month_String);   
}
void LCD_Write_Day()
{
        Time_Lock_Main.lock();
        if(Log_Value == 1){cout<<"In LCD_Write_Day Time lock taken"<<endl;}
        time_t Time = time(NULL);
        tm* Time_Pointer = localtime(&Time);
        int Days = Time_Pointer->tm_mday;
        Time_Lock_Main.unlock();
        if(Log_Value == 1){cout<<"In LCD_Write_Day Time lock released"<<endl;}
        stringstream ss;
        ss << Days;
        string Day_String = ss.str();
        LCD.DDRAM_Address(0x00);
        LCD.Write_String("Set Day");
        LCD.DDRAM_Address(0x40);
        LCD.Write_String(Day_String);   
}


void LCD_Write_Hour()
{
        Time_Lock_Main.lock();
        if(Log_Value == 1){cout<<"In LCD_Write_Hour Time lock taken"<<endl;}
        time_t Time = time(NULL);
        tm* Time_Pointer = localtime(&Time);
        int Hours = Time_Pointer->tm_hour;
        Time_Lock_Main.unlock();
        if(Log_Value == 1){cout<<"In LCD_Write_Hour Time lock released"<<endl;}
        stringstream ss;
        ss << Hours;
        string Hour_String = ss.str();
        LCD.DDRAM_Address(0x00);
        LCD.Write_String("Set Hour");
        LCD.DDRAM_Address(0x40);
        LCD.Write_String(Hour_String);   
}
void LCD_Write_Minute()
{
        Time_Lock_Main.lock();
        if(Log_Value == 1){cout<<"In LCD_Write_Minute Time lock taken"<<endl;}
        time_t Time = time(NULL);
        tm* Time_Pointer = localtime(&Time);
        int Minutes = Time_Pointer->tm_min;
        Time_Lock_Main.unlock();
        if(Log_Value == 1){cout<<"In LCD_Write_Minute Time lock released"<<endl;}
        stringstream ss;
        ss << Minutes;
        string Minute_String = ss.str();
        LCD.DDRAM_Address(0x00);
        LCD.Write_String("Set Minute");
        LCD.DDRAM_Address(0x40);
        LCD.Write_String(Minute_String);   
}
void LCD_Write_Seconds()
{
        Time_Lock_Main.lock();
        if(Log_Value == 1){cout<<"In LCD_Write_Seconds Time lock taken"<<endl;}
        time_t Time = time(NULL);
        tm* Time_Pointer = localtime(&Time);
        int Seconds = Time_Pointer->tm_sec;
        Time_Lock_Main.unlock();
        if(Log_Value == 1){cout<<"In LCD_Write_Seconds Time lock released"<<endl;}
        stringstream ss;
        ss << Seconds;
        string Second_String = ss.str();
        LCD.DDRAM_Address(0x00);
        LCD.Write_String("Set Second");
        LCD.DDRAM_Address(0x40);
        LCD.Write_String(Second_String);   
}

void LCD_Output()
{      
    while(1)
    {
        Thread::wait(10);//Dont Delete
        if(mode == 0)//Default mode
        { 
            if(Log_Value == 1){cout << "In mode 0 " << endl;}//Log this 
            
            Thread::wait(Default_Mode_Toggle_Time);
            if(Log_Value == 1){cout<<"Writing Data to LCD"<<endl;}
            sprintf (LCD_buffer, "%1.1f %1.1f %1.1f",Data_Active.get_temperature(),Data_Active.get_pressure(),Data_Active.get_light());//Used for converting to a sting  
             
            LCD.DDRAM_Address(0x00);
            LCD.Write_String("Temp Pres  li");
            LCD.DDRAM_Address(0x40);
            LCD.Write_String(LCD_buffer);

            Thread::wait(Default_Mode_Toggle_Time);    
            if(Log_Value == 1){cout<<"Writing Time and Date to LCD"<<endl;}
            Time_Lock_Main.lock();
            time_t Time = Data_Active.get_time();
            tm* Time_Pointer = localtime(&Time);
            LCD.Display_Clear();
            sprintf (LCD_buffer, "%d:%d    %d,%d",Time_Pointer->tm_hour,Time_Pointer->tm_min,(Time_Pointer->tm_mon+1),(Time_Pointer->tm_year+1900));//Used for converting to a sting  
            Time_Lock_Main.unlock(); 
            
            LCD.DDRAM_Address(0x00);
            LCD.Write_String("Current Time:");
            LCD.DDRAM_Address(0x40);
            LCD.Write_String(LCD_buffer);
            
            if(Log_Value == 1){cout<<"Checking Switches for next mode"<<endl;}
            if(SW1.read() & SW2.read() == 1)
            {
                mode = 1;   
            }
        }
        else if(mode == 1)//Choose either date setting or time setting
        {
            if(Log_Value == 1){cout << "In Mode 1" << endl;}
            LCD.Display_Clear();
            while(1)
            {
                LCD.DDRAM_Address(0x00);
                LCD.Write_String("Date        Time");
                LCD.DDRAM_Address(0x40);
                LCD.Write_String("<              >");
                Thread::wait(1000);
                if(Log_Value == 1){cout<<"Checking SW1 to go to Date setting"<<endl;}
                if(SW1.read() == 1 & SW2.read() == 0)
                {
                    mode = 2;//Date Setting
                    break;   
                }
                if(Log_Value == 1){cout<<"Checking SW2 to go to Time setting"<<endl;}
                if(SW2.read() == 1 & SW1.read() == 0)
                {
                    mode = 5;//Time Setting
                    break;
                }
            }
        } 
        else if(mode == 2)//Set the Year
        {
            if(Log_Value == 1){cout << "In Mode 2" << endl;}
            LCD.Display_Clear();
            while(1)
            {          
                LCD_Write_Year();
                Thread::wait(1000);
                 if(Log_Value == 1){cout<<"Checking SW1 and SW2 to go to Month setting"<<endl;}
                if(SW1.read() & SW2.read() == 1)
                {
                    mode = 3;
                    break;   
                }
                if(Log_Value == 1){cout<<"Checking SW1 to add Year"<<endl;}
                if(SW1.read() == 1 & SW2.read() == 0)
                {
                    Add_Year();
                }
                if(Log_Value == 1){cout<<"Checking SW2 to subtract Year"<<endl;}
                else if(SW2.read() == 1 & SW1.read() == 0)
                {
                    Subtract_Year();   
                }
                LCD_Write_Year();
            }
        } 
        else if(mode == 3)//Set the Month
        {
            if(Log_Value == 1){cout << "In Mode 3" << endl;}
            LCD.Display_Clear();
            while(1)
            {
                LCD_Write_Month();
                Thread::wait(1000);
                if(Log_Value == 1){cout<<"Checking SW1 and SW2 to go to Day setting"<<endl;}
                if(SW1.read() & SW2.read() == 1)
                {
                    mode = 4;
                    break;   
                }
                if(Log_Value == 1){cout<<"Checking SW1 to add Month"<<endl;}                
                if(SW1.read() == 1 & SW2.read() == 0)
                {
                    Add_Month();
                }
                if(Log_Value == 1){cout<<"Checking SW2 to subtract Month"<<endl;}  
                else if(SW2.read() == 1 & SW1.read() == 0)
                {
                    Subtract_Month();   
                }                

            }
        }
        else if(mode == 4)//Set the Day
        {
            if(Log_Value == 1){cout << "In Mode 4" << endl;}
            LCD.Display_Clear();
            while(1)
            {      
                LCD_Write_Day();
                Thread::wait(1000);
                if(Log_Value == 1){cout<<"Checking SW1 and SW2 to go Default setting"<<endl;}
                if(SW1.read() & SW2.read() == 1)
                {
                    mode = 0;   
                    break;
                }
                if(Log_Value == 1){cout<<"Checking SW1 to add Day"<<endl;}
                if(SW1.read() == 1 & SW2.read() == 0)
                {
                    Add_Day();
                }
                if(Log_Value == 1){cout<<"Checking SW2 to subtract Day"<<endl;}
                else if(SW2.read() == 1 & SW1.read() == 0)
                {
                    Subtract_Day();   
                }                
            }
        } 
        else if(mode == 5)//Set the Hour
        {
            if(Log_Value == 1){cout << "In Mode 5" << endl;}
            LCD.Display_Clear();
            while(1)
            {
                LCD_Write_Hour();
                Thread::wait(1000);
                if(Log_Value == 1){cout<<"Checking SW1 and SW2 to go Minute setting"<<endl;}
                if(SW1.read() & SW2.read() == 1)
                {
                    mode = 6;  
                    break; 
                }
                if(Log_Value == 1){cout<<"Checking SW1 to add Hour"<<endl;}                
                if(SW1.read() == 1 & SW2.read() == 0)
                {
                    Add_Hour();
                }
                if(Log_Value == 1){cout<<"Checking SW2 to subtract Hour"<<endl;} 
                else if(SW2.read() == 1 & SW1.read() == 0)
                {
                    Subtract_Hour();   
                }
            }
        } 
        else if(mode == 6)//Set the Minute
        {
            if(Log_Value == 1){cout << "In Mode 6" << endl;}
            LCD.Display_Clear();
            while(1)
            {
                LCD_Write_Minute();
                Thread::wait(1000);
                if(Log_Value == 1){cout<<"Checking SW1 and SW2 to go Seconds setting"<<endl;}
                if(SW1.read() & SW2.read() == 1)
                {
                    mode = 7;  
                    break; 
                }                
                if(Log_Value == 1){cout<<"Checking SW1 to add Minute"<<endl;}
                if(SW1.read() == 1 & SW2.read() == 0)
                {
                    Add_Minute();
                }
                if(Log_Value == 1){cout<<"Checking SW2 to subtract Minute"<<endl;}
                else if(SW2.read() == 1 & SW1.read() == 0)
                {
                    Subtract_Minute();   
                }                
            }
        }  
        else if(mode == 7)//Set the Seconds
        {
            if(Log_Value == 1){cout << "In Mode 7" << endl;}
            LCD.Display_Clear();
            while(1)
            {
                LCD_Write_Seconds();
                Thread::wait(1000);
                if(Log_Value == 1){cout<<"Checking SW1 and SW2 to go Default setting"<<endl;}
                if(SW1.read() & SW2.read() == 1)
                {
                    mode = 0;  
                    break; 
                }
                if(Log_Value == 1){cout<<"Checking SW1 to add Second"<<endl;}                
                if(SW1.read() == 1 & SW2.read() == 0)
                {
                    Add_Second();
                }
                if(Log_Value == 1){cout<<"Checking SW1 to subtract Second"<<endl;}   
                else if(SW2.read() == 1 & SW1.read() == 0)
                {
                    Subtract_Second();   
                }                
            }
        }
        else
        {
            if(Log_Value == 1){cout<<"Mode Error occured mode now set to 0"<<endl;}   
            mode = 0;
        }
    }
}
void Serial_Commands()//Used for getting input from the user to determine the opperations to perform
{
    
    char input[100];
    while(1) {
        
        if(Log_Value == 1){cout<<"In Serial_Commands"<<endl;}   
         for (int x = 0; x < 100; x++){input[x] = ' ';};
        
        cout << "Please type in a command" << endl;//Log this 
        
        cin.getline(input,sizeof(input),'\r');
        
        //READ ALL FIX THIS
        if(input[0] == 'R' & input[1] == 'E' & input[2] == 'A' & input[3] == 'D' & input[4] == ' ' & input[5] == 'A' & input[6] == 'L' & input[7] == 'L' & input[8] == ' ')
        {
            Console_Output_Timer.attach(&Console_Output_ISR,Sample_Rate);
            if(Log_Value == 1){cout<<"READ ALL Confirmed"<<endl;}
        }
        
        //DELETE ALL FIX THIS
        else if(input[0] == 'D' & input[1] == 'E' & input[2] == 'L' & input[3] == 'E' & input[4] == 'T' & input[5] == 'E' & input[6] == ' ' & input[7] == 'A' & input[8] == 'L' & input[9] == 'L' & input[10] == ' ')
        {   
            Sampling_Timer.detach();
            Console_Output_Timer.detach();
            Sampling_Timer.attach(&Sampling_ISR,Sample_Rate);
            cout << "Sampling Restarted" << endl;
            if(Log_Value == 1){cout<<"DELETE ALL Confirmed"<<endl;}
        }
        
        //READ look into this
        else if(input[0] == 'R' & input[1] == 'E' & input[2] == 'A' & input[3] == 'D' & input[4] == ' ')
        {
            int NumberOfChars = 0; int ArrayAddress = 0; string ReadNumber; int NumberToRead;
            while(input[ArrayAddress] != '\0'){NumberOfChars++; ArrayAddress++;}
            for(int x=5; x < NumberOfChars; x++){ReadNumber += input[x];}
            stringstream Number(ReadNumber);
            Number >> NumberToRead;
            
            Serial_Comms_Data_Get();
            if(Log_Value == 1){cout << "Read " << NumberToRead << " samples" << endl;}
            
        } 
        
        //DELETE look into this
        else if(input[0] == 'D' & input[1] == 'E' & input[2] == 'L' & input[3] == 'E' & input[4] == 'T' & input[5] == 'E' & input[6] == ' ')
        {
            Sampling_Timer.detach();
            Console_Output_Timer.detach();
            
            
            int NumberOfChars = 0; int ArrayAddress = 0; string DeleteNumber; int NumberToDelete;
            while(input[ArrayAddress] != '\0'){NumberOfChars++; ArrayAddress++;}
            for(int x=7; x < NumberOfChars; x++){DeleteNumber += input[x];}
            stringstream Number(DeleteNumber);
            Number >> NumberToDelete;
            
            if(Log_Value == 1){cout << "Deleted " << NumberToDelete << " samples" << endl;}
            
            Sampling_Timer.attach(&Sampling_ISR,Sample_Rate);
            cout << "Sampling Restarted" << endl;
        }
        
        //STATE
        else if(input[0] == 'S' & input[1] == 'T' & input[2] == 'A' & input[3] == 'T' & input[4] == 'E' & input[5] == ' ')
        {
            int NumberOfChars = 0; int ArrayAddress = 0; string StateNumber; int NumberToState;
            while(input[ArrayAddress] != '\0'){NumberOfChars++; ArrayAddress++;}
            for(int x=6; x < NumberOfChars; x++){StateNumber += input[x];}
            stringstream Number(StateNumber);
            Number >> NumberToState;
            
            if(NumberToState==0){Sampling_Timer.detach(); Console_Output_Timer.detach();}
            if(NumberToState==1){Sampling_Timer.attach(&Sampling_ISR,Sample_Rate);}  
        } 
        
        //SETDATE
        else if(input[0] == 'S' & input[1] == 'E' & input[2] == 'T' & input[3] == 'D' & input[4] == 'A' & input[5] == 'T' & input[6] == 'E' & input[7] == ' ' & input[10] == ' ' & input[13] == ' ')
        {
            int NumberOfChars = 0; int ArrayAddress = 0; 
            string DayNumber, MonthNumber, YearNumber;
            int NumberToDay, NumberToMonth, NumberToYear;
            
            for(int x=8; x < 10; x++){DayNumber += input[x];}
            stringstream Number_1(DayNumber);
            Number_1 >> NumberToDay;
            
            for(int x=11; x < 13; x++){MonthNumber += input[x];}
            stringstream Number_2(MonthNumber);
            Number_2 >> NumberToMonth;
            
            for(int x=14; x < 18; x++){YearNumber += input[x];}
            stringstream Number_3(YearNumber);
            Number_3 >> NumberToYear;
            
            set_new_date(NumberToDay,NumberToMonth,NumberToYear);
        }
        
        //SETTIME
        else if(input[0] == 'S' & input[1] == 'E' & input[2] == 'T' & input[3] == 'T' & input[4] == 'I' & input[5] == 'M' & input[6] == 'E' & input[7] == ' ' & input[10] == ' ' & input[13] == ' ')
        {
            int NumberOfChars = 0; int ArrayAddress = 0; 
            string HourNumber, MinuteNumber, SecondNumber;
            int NumberToHour, NumberToMinute, NumberToSecond;
            
            for(int x=8; x < 10; x++){HourNumber += input[x];}
            stringstream Number_1(HourNumber);
            Number_1 >> NumberToHour;
            
            for(int x=11; x < 13; x++){MinuteNumber += input[x];}
            stringstream Number_2(MinuteNumber);
            Number_2 >> NumberToMinute;
            
            for(int x=14; x < 16; x++){SecondNumber += input[x];}
            stringstream Number_3(SecondNumber);
            Number_3 >> NumberToSecond;
            
            set_new_time(NumberToHour,NumberToMinute,NumberToSecond);    
        } 
        
        //SETT
        else if(input[0] == 'S' & input[1] == 'E' & input[2] == 'T' & input[3] == 'T' & input[4] == ' ')
        {
            
            int NumberOfChars = 0; int ArrayAddress = 0; string SettNumber; int NumberToSett;
            while(input[ArrayAddress] != '\0'){NumberOfChars++; ArrayAddress++;}
            for(int x=5; x < NumberOfChars; x++){SettNumber += input[x];}
            stringstream Number(SettNumber);
            Number >> NumberToSett;
               
            if(NumberToSett >= 1){Sample_Rate = NumberToSett;}
             
            Sampling_Timer.detach();
            Console_Output_Timer.detach();
            Sampling_Timer.attach(&Sampling_ISR,Sample_Rate);
        }
        
        //LOGGING
        else if(input[0] == 'L' & input[1] == 'O' & input[2] == 'G' & input[3] == 'G' & input[4] == 'I' & input[5] == 'N' & input[6] == 'G' & input[7] == ' ')
        {
            int NumberOfChars = 0; int ArrayAddress = 0; string LoggingNumber; int NumberToLogging;
            while(input[ArrayAddress] != '\0'){NumberOfChars++; ArrayAddress++;}
            for(int x=8; x < NumberOfChars; x++){LoggingNumber += input[x];}
            stringstream Number(LoggingNumber);
            Number >> NumberToLogging;
            
            cout << "Successfully entered logging" << endl;
            cout << NumberToLogging  << endl; 
            Log_Value = NumberToLogging;
        } 
        
        //HELP
        else if(input[0] == 'H' & input[1] == 'E' & input[2] == 'L' & input[3] == 'P')// Use this to display all of the availble commands
        { 
            HELP();
        }
        
        //STOP
        else if(input[0] == 'S' & input[1] == 'T' & input[2] == 'O' & input[3] == 'P')
        {
            Console_Output_Timer.detach(); //Stops Sampling   
        }
        else 
        {
            cout << "Please enter an acceptable command" << endl;
        }
    }
}
void Serial_Comms_Data_Get()
{
    if(Log_Value == 1){cout<<"Getting Data"<<endl;} 
    time_t Time = Data_Active.get_time();
    tm* Time_Pointer = localtime(&Time);
    strftime(scom_time_buffer, 32, "%I:%M %p\t", Time_Pointer);                         //Format time as a string
    pc.printf("Date = %d %d %d\t", Time_Pointer->tm_mday, (Time_Pointer->tm_mon+1),(Time_Pointer->tm_year+1900));
    pc.printf("Time = %s", scom_time_buffer);                                           //Print the string formatted time
    pc.printf("Temperature = %f\t", Data_Active.get_temperature());                     //Print Temperature
    pc.printf("Pressure = %f\t", Data_Active.get_pressure());                           //Print Pressure
    pc.printf("Light = %f\n\r", Data_Active.get_light());                               //Print Light
    
    Green_led.Toggle();
    
}
void Serial_Comms()//Thread for Serial Communications
{
    while(1) 
    {
        Thread::signal_wait(SerialCommsTime);//Stupid 0 Bug
        Serial_Comms_Data_Get();
        if(Log_Value == 1){cout<<"In Serial Comms"<<endl;} 
    }
}

void Console_Output_ISR()   {t2.signal_set(SerialCommsTime);}
void Sampling_ISR()         {t1.signal_set(SamplingTime);}


void Sample()//Samples the hardware and prints the result to the LCD
{
    while(1) 
    {
        Thread::signal_wait(SamplingTime);      //Set the time between samples
        if(Log_Value == 1){cout<<"Sample Time"<<endl;} 
        Data_Active.set_temperature(sensor.getTemperature());   //Read Temperature
        Data_Active.set_pressure(sensor.getPressure());         //Read Pressure
        Data_Active.set_light(adcIn.read());                    //Read Light
        
        Time_Lock_Main.lock();
        time_t buffer_time = time(NULL);        
        Time_Lock_Main.unlock();

        Data_Active.set_time(buffer_time);

        Red_led.Toggle();
        t1.signal_set(NotSamplingTime);
        if(Log_Value == 1){cout<<"New Sample avaliable"<<endl;} 
    }
}
int main()
{
    set_time(1515352584);               //Set RTC time to December 10 2017
    
    pc.baud(9600);                      //Sets the Serial Comms Baud Rate

    LCD.Initialise();
    LCD.DDRAM_Address(0x00);
 
    post();     //Power on Self Test

    //Initialise the SD card (this needs to move)
    if ( sd.init() != 0) {
        printf("Init failed \n");
        LCD.Display_Clear();
        LCD.Write_String("CANNOT INIT SD");        //Change me
        errorCode(FATAL);
    }
    //Create a filing system for SD Card
    FATFileSystem fs("sd", &sd);

    //Open to WRITE
    FILE* fp = fopen("/sd/test.csv","a");
    if (fp == NULL) {
        error("Could not open file for write\n");
        LCD.Display_Clear();
        LCD.Write_String("CANNOT OPEN FILE");
        errorCode(FATAL);
    }
    int network_temp;
    network_temp = Network_Init();
    if(network_temp == 1)//Sets up the network and checks if the network cable is not pluged in
    {
        error("Could not access network");
        LCD.Display_Clear();
        LCD.Write_String("Could not access network");
        errorCode(FATAL);   
    }
    //Last message before sampling begins
    LCD.Display_Clear();
    LCD.Write_String("READY     PLAYER");
    LCD.DDRAM_Address(0x40);
    LCD.Write_String("      One     ");
    LCD.DDRAM_Address(0x00);

    Sample_Rate = TimerInterval;
    //Run interrupt
    Sampling_Timer.attach(&Sampling_ISR,Sample_Rate);

    t1.start(Sample);
    t2.start(Serial_Comms);
    t3.start(LCD_Output);
    t4.start(Network);
    t5.start(Serial_Commands);

    //Main thread ID

    idMain = osThreadGetId();   //CMSIS RTOS call

    //Thread ID
    id1 = t1.gettid();
    id2 = t2.gettid();
    id3 = t3.gettid();
    id4 = t4.gettid();
    id5 = t5.gettid();

    while (onBoardSwitch == 0) {

    }

    //Close File
    /*
    fclose(fp);

    //Close down
    sd.deinit();
    printf("Unmounted...\n");
    lcd.cls();
    lcd.printf("Unmounted...\n\n");
    */

    while(true) {
        greenLED = 1;
        wait(0.5);
        greenLED = 0;
        wait(0.1);
    }
}