File content as of revision 11:12e73437dc9f:

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



#include <std_msgs/UInt16.h>
#include <std_msgs/UInt8MultiArray.h>
#include <std_msgs/UInt16MultiArray.h>
#include <std_msgs/UInt32MultiArray.h>

#include <std_msgs/String.h>
#include <ros/time.h>
#include <sensor_msgs/Range.h>




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


ros::NodeHandle_<mySTM32> nh;

ros::Subscriber<std_msgs::UInt8> sub("keyControl", &MotorKeySub);  // Subscriber for Motor Control by Keyboard.
ros::Subscriber<std_msgs::UInt8> sub1("ServoControl", &servoKeySub); // Subscriber for Servo Control by keyboard.

//TOF publisher
std_msgs::UInt8MultiArray range_msg;
ros::Publisher TOFreadings("TOFreadings", &range_msg);




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


Ticker power_monitor;

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


//Class defines
cAdafruit_VL6180X VL6180X(TOF1,TOF4,TOF5,TOF6);  //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
------------------------------------------------------------------------------*/


std_msgs::UInt8MultiArray m;


 
float TOFRange[4];
 
DigitalOut led = LED1;

 
/* Private Functions----------------------------------------------------------*/ 
void power_check(void);


/*--------------------------------------------------------------------------------
                                    MAIN CONTROL LOOP
-------------------------------------------------------------------------------*/
int main()
{
    
    
    nh.initNode();
    nh.subscribe(sub);
    nh.subscribe(sub1);

  
    //Publishers
    nh.advertise(TOFreadings); //TOF readings
    
    //TOF publisher parameters
    range_msg.layout.dim = (std_msgs::MultiArrayDimension *)
    malloc(sizeof(std_msgs::MultiArrayDimension) * 2);
    range_msg.layout.dim[0].label = "TOF readings";
    range_msg.layout.dim[0].size = 4;
    range_msg.layout.dim[0].stride = 1*4;
    range_msg.layout.data_offset = 0;
    range_msg.data = (uint8_t *)malloc(sizeof(int)*4);
    range_msg.data_length = 4;
    
    servo1.position(0);
    servo1.position(0);
    power_monitor.attach(power_check, 30.0); //Monitor battery every 30 seconds 
    
    //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, ADDR5);
        TOFRange[3] = serviceTOF(VL6180X, ADDR6);
        // 
        TOFrangePtr[0] = &TOFRange[0];
        TOFrangePtr[1] = &TOFRange[1];
        TOFrangePtr[2] = &TOFRange[2];
        TOFrangePtr[3] = &TOFRange[3];
        
        //Assign to TOFreadings publisher
        range_msg.data[0] = TOFRange[0];
        range_msg.data[1] = TOFRange[1];
        range_msg.data[2] = TOFRange[2];
        range_msg.data[3] = TOFRange[3];
        
        //Publish data
        TOFreadings.publish(&range_msg);
        // Need to write code like the obstical ovaoidance, to stop the motors, by the nucleo 
        // sending a char to the PI
        
       
        nh.spinOnce();
        wait_ms(5);
    }
}

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 
}