File content as of revision 9:326b8f261ef0:

/*--------------------------------------------------------------------------------
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) {};
};

/*
void servo1_cb( const std_msgs::UInt16& cmd_msg) {
    servo1.position(cmd_msg.data); //set servo angle, should be from 0-180
}
void servo2_cb( const std_msgs::UInt16& cmd_msg) {
    servo2.position(cmd_msg.data); //set servo angle, should be from 0-180
}    
*/

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.

//std_msgs::UInt16MultiArray range_msg;
//ros::Publisher TOFstuff("TOFstuff", &range_msg);

sensor_msgs::Range range_msg1, range_msg2,range_msg3,range_msg4;
ros::Publisher tof1("tof1", &range_msg1);
ros::Publisher tof2("tof2", &range_msg2);
ros::Publisher tof3("tof3", &range_msg3);
ros::Publisher tof4("tof4", &range_msg4);

//std_msgs::Int32MultiArray range_msg;
//array.data.clear();


/* NOT USED ANYMORE */
//ros::Subscriber<std_msgs::UInt16> sub2("servo1", servo1_cb);
//ros::Subscriber<std_msgs::UInt16> sub3("servo2", servo2_cb);


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 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
------------------------------------------------------------------------------*/


std_msgs::UInt8MultiArray m;
//sensor_msgs::Range range_msg[4];

 
float TOFRange[4];
 
DigitalOut led = LED1;
Timer t;  // Timer 

 char frameid1[] = "/Rear Sensor";
 char frameid2[] = "/Front Left Sensor";
 char frameid3[] = "/Front Center Sensor"; 
 char frameid4[] = "/Front Right Sensor";
/* Private Functions----------------------------------------------------------*/ 
void power_check(void);


/*--------------------------------------------------------------------------------
                                    MAIN CONTROL LOOP
-------------------------------------------------------------------------------*/
int main()
{
    //t.start(); 
    IncSize=1;
    
    nh.initNode();
    nh.subscribe(sub);
    nh.subscribe(sub1);
    //nh.subscribe(sub2);
    
    nh.advertise(tof1);
    nh.advertise(tof2);
    nh.advertise(tof3);
    nh.advertise(tof4);
    
    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, ADDR6);
        TOFRange[3] = serviceTOF(VL6180X, ADDR7);

        //Check_for_obstacles(TOFRange); //Run obstacle avoidance
       range_msg1.header.frame_id =frameid1;
       range_msg2.header.frame_id =frameid2;
       range_msg3.header.frame_id =frameid3;
       range_msg4.header.frame_id =frameid4;
       
       range_msg1.range = TOFRange[0];
       range_msg2.range = TOFRange[1];
       range_msg3.range = TOFRange[2];
       range_msg4.range = TOFRange[3];
        tof1.publish(&range_msg1);
        tof2.publish(&range_msg2);
        tof3.publish(&range_msg3);
        tof4.publish(&range_msg4);
        
        // Need to write code like the obstical ovaoidance, to stop the motors, by the nucleo 
        // sending a char to the PI
        
        //pc.printf("Spinning...");
        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 
}