File content as of revision 12:82b8fe254222:

/*------------------------------------------------------------------------------
    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 TOF_Thread(osPriorityNormal);
//Thread Debug_Thread(osPriorityNormal);
//Thread Motor_Thread(osPriorityNormal);

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

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

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

    /**************************************************************************/
    //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_ms(500);
        debug_LED = 0;
        wait_ms(500);
    }
}