File content as of revision 11:0b9098ec11c7:

/*------------------------------------------------------------------------------
    Created by: Sami Sequeira & Jonathan Wheadon
    @ The University of Plymouth
    November 2019
    For the SOFT564Z module on distributed systems
------------------------------------------------------------------------------*/

#include "mbed.h"
//#include "Debug.hpp"
//#include "dateTime.hpp"
#include "General.hpp"
#include "Battery_Monitor.hpp"
#include "Pins.h"
#include "ros.h"
#include "rtos.h"
#include "ROS_Handler.hpp"
#include <motordriver.h>
#include "math.h"
#include "Motors.hpp"
//#include "VL6180.hpp"

DigitalOut debug_LED(LED1);

// Threads
Thread ROS_Thread(osPriorityRealtime);   // Create THREAD with highest priority for ROS
//Thread Debug_Thread(osPriorityNormal);
//Thread Movement_Thread(osPriorityNormal);

// Thread ID for the Main function (CMSIS API)
osThreadId tidMain;

// main starts all threads
int main(void)
{
    //TOF_Thread();
    // Main thread ID
    tidMain = Thread::gettid();

    // Start each thread
    ROS_Thread.start(ROS_Handler);
    //Debug_Thread.start(TerminalThread);
    //Thread2.start(main3);
    
    // Creates an object to monitor and handle changes in battery level using an RGB LED as an OutPut
    Battery_Monitor VBatt_Monitor(ADC_VBAT, 0.5f);

    /**************************************************************************/
    initialize(); //initialize(); only needs to be called once, at the start of the program.
    //the encoder interrupts should be able to handle themselves from there, and do not require
    //resetting.
    /**************************************************************************/
        
    while(1)
    {
        //Flag_Error(warning, "Flashing LED\n\r");
        debug_LED = 1;
        wait(1);
        debug_LED = 0;
        wait(1);

        
        /**********************************************************************/
        //careful, putting this in a while loop will mean the bot moves in an infinite square!
        /**********************************************************************/
        //tempMove(40);
        //tempRotate(-90);
        //tempMove(40);
        //tempRotate(-90);
        //tempMove(40);
        //tempRotate(-90);
        //tempMove(40);
        //tempRotate(-90);
    }
}