File content as of revision 8:262c6c40bff9:

/*--------------------------------------------------------------------------------
Filename: main.cpp
Description: Main program loop
--------------------------------------------------------------------------------*/
#include "mbed.h"
#include "TOF.h"
#include "Motor.h"
#include "power.h"
#include "Buzzer.h"
#include "LED.h"
#include <ros.h>
#include <std_msgs/UInt8.h>

class mySTM32 : public MbedHardware
{
public:
    mySTM32(): MbedHardware(PD_5, PD_6, 57600) {};
};

ros::NodeHandle_<mySTM32> nh;

ros::Subscriber<std_msgs::UInt8> sub("keyControl", &keySub);

DigitalIn user_button(USER_BUTTON);
float power_levels[3]; //Array to voltage levels

Serial pc(SERIAL_TX, SERIAL_RX, 9600);    //set-up serial to pc

Ticker power_monitor;

//TOF chip shutdown signals
DigitalOut TOF1(PC_8);
DigitalOut TOF4(PC_11);
DigitalOut TOF6(PC_12);
DigitalOut TOF7(PD_2);

//Class defines
cAdafruit_VL6180X VL6180X(TOF1,TOF4,TOF6,TOF7);  //Define TOF sensor class and initialise devices
cPower cPower(VBATT, V5, V3);

/*--------------------------------------------------------------------------------
Function name: power_check
Input Parameters: N/A
Output Parameters: N/A
Description: Routine interrupt to monitor battery levels
----------------------------------------------------------------------------------*/
void power_check()
{
    power_levels[0] = cPower.monitor_battery(); //Monitors raw battery
    power_levels[1] = cPower.monitor_5v_line(); //Monitors 5V line
    power_levels[2] = cPower.monitor_3v3_line();//Monitors 3V3 Line

    update_power_levels(power_levels[0]); //Sends the raw battery levels to the LED class 
}


/*--------------------------------------------------------------------------------
                                    MAIN CONTROL LOOP
----------------------------------------------------------------------------------*/
int main()
{
    nh.initNode();
    nh.subscribe(sub);
    
    power_monitor.attach(power_check, 30.0); //Monitor battery every 30 seconds
    
    uint8_t TOFRange[4]; //Array to store TOF measurements 0-sensor1, 1-sensor4, 2-sensor6, 3-sensor7
    
    //Wait for user button to be pressed
    pc.printf("Press user button to start\n\r");
    
    while(user_button != 1) {}
    
    while(1) {
        
        /*
        //Perform TOF measurements
        TOFRange[0] = serviceTOF(VL6180X, ADDR1);
        TOFRange[1] = serviceTOF(VL6180X, ADDR4);
        TOFRange[2] = serviceTOF(VL6180X, ADDR6);
        TOFRange[3] = serviceTOF(VL6180X, ADDR7);

        Check_for_obstacles(TOFRange); //Run obstacle avoidance
        */
        pc.printf("Spinning...");
        nh.spinOnce();
        wait_ms(1);
    }
}